diff --git a/core/crypto/aes-ccm.c b/core/crypto/aes-ccm.c new file mode 100644 index 0000000..dceec77 --- /dev/null +++ b/core/crypto/aes-ccm.c @@ -0,0 +1,211 @@ +/* + * Counter with CBC-MAC (CCM) with AES + * + * Copyright (c) 2010-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" + + +static void xor_aes_block(u8 *dst, const u8 *src) +{ + u32 *d = (u32 *) dst; + u32 *s = (u32 *) src; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; +} + + +static void aes_ccm_auth_start(void *aes, size_t M, size_t L, const u8 *nonce, + const u8 *aad, size_t aad_len, size_t plain_len, + u8 *x) +{ + u8 aad_buf[2 * AES_BLOCK_SIZE]; + u8 b[AES_BLOCK_SIZE]; + + /* Authentication */ + /* B_0: Flags | Nonce N | l(m) */ + b[0] = aad_len ? 0x40 : 0 /* Adata */; + b[0] |= (((M - 2) / 2) /* M' */ << 3); + b[0] |= (L - 1) /* L' */; + os_memcpy(&b[1], nonce, 15 - L); + WPA_PUT_BE16(&b[AES_BLOCK_SIZE - L], plain_len); + + wpa_hexdump_key(_MSG_EXCESSIVE_, "CCM B_0", b, AES_BLOCK_SIZE); + aes_encrypt(aes, b, x); /* X_1 = E(K, B_0) */ + + if (!aad_len) + return; + + WPA_PUT_BE16(aad_buf, aad_len); + os_memcpy(aad_buf + 2, aad, aad_len); + os_memset(aad_buf + 2 + aad_len, 0, sizeof(aad_buf) - 2 - aad_len); + + xor_aes_block(aad_buf, x); + aes_encrypt(aes, aad_buf, x); /* X_2 = E(K, X_1 XOR B_1) */ + + if (aad_len > AES_BLOCK_SIZE - 2) { + xor_aes_block(&aad_buf[AES_BLOCK_SIZE], x); + /* X_3 = E(K, X_2 XOR B_2) */ + aes_encrypt(aes, &aad_buf[AES_BLOCK_SIZE], x); + } +} + + +static void aes_ccm_auth(void *aes, const u8 *data, size_t len, u8 *x) +{ + size_t last = len % AES_BLOCK_SIZE; + size_t i; + + for (i = 0; i < len / AES_BLOCK_SIZE; i++) { + /* X_i+1 = E(K, X_i XOR B_i) */ + xor_aes_block(x, data); + data += AES_BLOCK_SIZE; + aes_encrypt(aes, x, x); + } + if (last) { + /* XOR zero-padded last block */ + for (i = 0; i < last; i++) + x[i] ^= *data++; + aes_encrypt(aes, x, x); + } +} + + +static void aes_ccm_encr_start(size_t L, const u8 *nonce, u8 *a) +{ + /* A_i = Flags | Nonce N | Counter i */ + a[0] = L - 1; /* Flags = L' */ + os_memcpy(&a[1], nonce, 15 - L); +} + + +static void aes_ccm_encr(void *aes, size_t L, const u8 *in, size_t len, u8 *out, + u8 *a) +{ + size_t last = len % AES_BLOCK_SIZE; + size_t i; + + /* crypt = msg XOR (S_1 | S_2 | ... | S_n) */ + for (i = 1; i <= len / AES_BLOCK_SIZE; i++) { + WPA_PUT_BE16(&a[AES_BLOCK_SIZE - 2], i); + /* S_i = E(K, A_i) */ + aes_encrypt(aes, a, out); + xor_aes_block(out, in); + out += AES_BLOCK_SIZE; + in += AES_BLOCK_SIZE; + } + if (last) { + WPA_PUT_BE16(&a[AES_BLOCK_SIZE - 2], i); + aes_encrypt(aes, a, out); + /* XOR zero-padded last block */ + for (i = 0; i < last; i++) + *out++ ^= *in++; + } +} + + +static void aes_ccm_encr_auth(void *aes, size_t M, u8 *x, u8 *a, u8 *auth) +{ + size_t i; + u8 tmp[AES_BLOCK_SIZE]; + + wpa_hexdump_key(_MSG_EXCESSIVE_, "CCM T", x, M); + /* U = T XOR S_0; S_0 = E(K, A_0) */ + WPA_PUT_BE16(&a[AES_BLOCK_SIZE - 2], 0); + aes_encrypt(aes, a, tmp); + for (i = 0; i < M; i++) + auth[i] = x[i] ^ tmp[i]; + wpa_hexdump_key(_MSG_EXCESSIVE_, "CCM U", auth, M); +} + + +static void aes_ccm_decr_auth(void *aes, size_t M, u8 *a, const u8 *auth, u8 *t) +{ + size_t i; + u8 tmp[AES_BLOCK_SIZE]; + + wpa_hexdump_key(_MSG_EXCESSIVE_, "CCM U", auth, M); + /* U = T XOR S_0; S_0 = E(K, A_0) */ + WPA_PUT_BE16(&a[AES_BLOCK_SIZE - 2], 0); + aes_encrypt(aes, a, tmp); + for (i = 0; i < M; i++) + t[i] = auth[i] ^ tmp[i]; + wpa_hexdump_key(_MSG_EXCESSIVE_, "CCM T", t, M); +} + + +/* AES-CCM with fixed L=2 and aad_len <= 30 assumption */ +int aes_ccm_ae(const u8 *key, size_t key_len, const u8 *nonce, + size_t M, const u8 *plain, size_t plain_len, + const u8 *aad, size_t aad_len, u8 *crypt, u8 *auth) +{ + const size_t L = 2; + void *aes; + u8 x[AES_BLOCK_SIZE], a[AES_BLOCK_SIZE]; + + if (aad_len > 30 || M > AES_BLOCK_SIZE) + return -1; + + aes = aes_encrypt_init(key, key_len); + if (aes == NULL) + return -1; + + aes_ccm_auth_start(aes, M, L, nonce, aad, aad_len, plain_len, x); + aes_ccm_auth(aes, plain, plain_len, x); + + /* Encryption */ + aes_ccm_encr_start(L, nonce, a); + aes_ccm_encr(aes, L, plain, plain_len, crypt, a); + aes_ccm_encr_auth(aes, M, x, a, auth); + + aes_encrypt_deinit(aes); + + return 0; +} + + +/* AES-CCM with fixed L=2 and aad_len <= 30 assumption */ +int aes_ccm_ad(const u8 *key, size_t key_len, const u8 *nonce, + size_t M, const u8 *crypt, size_t crypt_len, + const u8 *aad, size_t aad_len, const u8 *auth, u8 *plain) +{ + const size_t L = 2; + void *aes; + u8 x[AES_BLOCK_SIZE], a[AES_BLOCK_SIZE]; + u8 t[AES_BLOCK_SIZE]; + + if (aad_len > 30 || M > AES_BLOCK_SIZE) + return -1; + + aes = aes_encrypt_init(key, key_len); + if (aes == NULL) + return -1; + + /* Decryption */ + aes_ccm_encr_start(L, nonce, a); + aes_ccm_decr_auth(aes, M, a, auth, t); + + /* plaintext = msg XOR (S_1 | S_2 | ... | S_n) */ + aes_ccm_encr(aes, L, crypt, crypt_len, plain, a); + + aes_ccm_auth_start(aes, M, L, nonce, aad, aad_len, crypt_len, x); + aes_ccm_auth(aes, plain, crypt_len, x); + + aes_encrypt_deinit(aes); + + if (os_memcmp_const(x, t, M) != 0) { + wpa_printf(_MSG_EXCESSIVE_, "CCM: Auth mismatch"); + return -1; + } + + return 0; +} diff --git a/core/crypto/aes-ctr.c b/core/crypto/aes-ctr.c new file mode 100644 index 0000000..6c06851 --- /dev/null +++ b/core/crypto/aes-ctr.c @@ -0,0 +1,70 @@ +/* + * AES-128/192/256 CTR + * + * Copyright (c) 2003-2007, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" + +/** + * aes_ctr_encrypt - AES-128/192/256 CTR mode encryption + * @key: Key for encryption (key_len bytes) + * @key_len: Length of the key (16, 24, or 32 bytes) + * @nonce: Nonce for counter mode (16 bytes) + * @data: Data to encrypt in-place + * @data_len: Length of data in bytes + * Returns: 0 on success, -1 on failure + */ +int aes_ctr_encrypt(const u8 *key, size_t key_len, const u8 *nonce, + u8 *data, size_t data_len) +{ + void *ctx; + size_t j, len, left = data_len; + int i; + u8 *pos = data; + u8 counter[AES_BLOCK_SIZE], buf[AES_BLOCK_SIZE]; + + ctx = aes_encrypt_init(key, key_len); + if (ctx == NULL) + return -1; + os_memcpy(counter, nonce, AES_BLOCK_SIZE); + + while (left > 0) { + aes_encrypt(ctx, counter, buf); + + len = (left < AES_BLOCK_SIZE) ? left : AES_BLOCK_SIZE; + for (j = 0; j < len; j++) + pos[j] ^= buf[j]; + pos += len; + left -= len; + + for (i = AES_BLOCK_SIZE - 1; i >= 0; i--) { + counter[i]++; + if (counter[i]) + break; + } + } + aes_encrypt_deinit(ctx); + return 0; +} + + +/** + * aes_128_ctr_encrypt - AES-128 CTR mode encryption + * @key: Key for encryption (key_len bytes) + * @nonce: Nonce for counter mode (16 bytes) + * @data: Data to encrypt in-place + * @data_len: Length of data in bytes + * Returns: 0 on success, -1 on failure + */ +int aes_128_ctr_encrypt(const u8 *key, const u8 *nonce, + u8 *data, size_t data_len) +{ + return aes_ctr_encrypt(key, 16, nonce, data, data_len); +} diff --git a/core/crypto/aes-gcm.c b/core/crypto/aes-gcm.c new file mode 100644 index 0000000..4708810 --- /dev/null +++ b/core/crypto/aes-gcm.c @@ -0,0 +1,326 @@ +/* + * Galois/Counter Mode (GCM) and GMAC with AES + * + * Copyright (c) 2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" + +static void inc32(u8 *block) +{ + u32 val; + val = WPA_GET_BE32(block + AES_BLOCK_SIZE - 4); + val++; + WPA_PUT_BE32(block + AES_BLOCK_SIZE - 4, val); +} + + +static void xor_block(u8 *dst, const u8 *src) +{ + u32 *d = (u32 *) dst; + u32 *s = (u32 *) src; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; + *d++ ^= *s++; +} + + +static void shift_right_block(u8 *v) +{ + u32 val; + + val = WPA_GET_BE32(v + 12); + val >>= 1; + if (v[11] & 0x01) + val |= 0x80000000; + WPA_PUT_BE32(v + 12, val); + + val = WPA_GET_BE32(v + 8); + val >>= 1; + if (v[7] & 0x01) + val |= 0x80000000; + WPA_PUT_BE32(v + 8, val); + + val = WPA_GET_BE32(v + 4); + val >>= 1; + if (v[3] & 0x01) + val |= 0x80000000; + WPA_PUT_BE32(v + 4, val); + + val = WPA_GET_BE32(v); + val >>= 1; + WPA_PUT_BE32(v, val); +} + + +/* Multiplication in GF(2^128) */ +static void gf_mult(const u8 *x, const u8 *y, u8 *z) +{ + u8 v[16]; + int i, j; + + os_memset(z, 0, 16); /* Z_0 = 0^128 */ + os_memcpy(v, y, 16); /* V_0 = Y */ + + for (i = 0; i < 16; i++) { + for (j = 0; j < 8; j++) { + if (x[i] & BIT(7 - j)) { + /* Z_(i + 1) = Z_i XOR V_i */ + xor_block(z, v); + } else { + /* Z_(i + 1) = Z_i */ + } + + if (v[15] & 0x01) { + /* V_(i + 1) = (V_i >> 1) XOR R */ + shift_right_block(v); + /* R = 11100001 || 0^120 */ + v[0] ^= 0xe1; + } else { + /* V_(i + 1) = V_i >> 1 */ + shift_right_block(v); + } + } + } +} + + +static void ghash_start(u8 *y) +{ + /* Y_0 = 0^128 */ + os_memset(y, 0, 16); +} + + +static void ghash(const u8 *h, const u8 *x, size_t xlen, u8 *y) +{ + size_t m, i; + const u8 *xpos = x; + u8 tmp[16]; + + m = xlen / 16; + + for (i = 0; i < m; i++) { + /* Y_i = (Y^(i-1) XOR X_i) dot H */ + xor_block(y, xpos); + xpos += 16; + + /* dot operation: + * multiplication operation for binary Galois (finite) field of + * 2^128 elements */ + gf_mult(y, h, tmp); + os_memcpy(y, tmp, 16); + } + + if (x + xlen > xpos) { + /* Add zero padded last block */ + size_t last = x + xlen - xpos; + os_memcpy(tmp, xpos, last); + os_memset(tmp + last, 0, sizeof(tmp) - last); + + /* Y_i = (Y^(i-1) XOR X_i) dot H */ + xor_block(y, tmp); + + /* dot operation: + * multiplication operation for binary Galois (finite) field of + * 2^128 elements */ + gf_mult(y, h, tmp); + os_memcpy(y, tmp, 16); + } + + /* Return Y_m */ +} + + +static void aes_gctr(void *aes, const u8 *icb, const u8 *x, size_t xlen, u8 *y) +{ + size_t i, n, last; + u8 cb[AES_BLOCK_SIZE], tmp[AES_BLOCK_SIZE]; + const u8 *xpos = x; + u8 *ypos = y; + + if (xlen == 0) + return; + + n = xlen / 16; + + os_memcpy(cb, icb, AES_BLOCK_SIZE); + /* Full blocks */ + for (i = 0; i < n; i++) { + aes_encrypt(aes, cb, ypos); + xor_block(ypos, xpos); + xpos += AES_BLOCK_SIZE; + ypos += AES_BLOCK_SIZE; + inc32(cb); + } + + last = x + xlen - xpos; + if (last) { + /* Last, partial block */ + aes_encrypt(aes, cb, tmp); + for (i = 0; i < last; i++) + *ypos++ = *xpos++ ^ tmp[i]; + } +} + + +static void * aes_gcm_init_hash_subkey(const u8 *key, size_t key_len, u8 *H) +{ + void *aes; + + aes = aes_encrypt_init(key, key_len); + if (aes == NULL) + return NULL; + + /* Generate hash subkey H = AES_K(0^128) */ + os_memset(H, 0, AES_BLOCK_SIZE); + aes_encrypt(aes, H, H); + wpa_hexdump_key(_MSG_EXCESSIVE_, "Hash subkey H for GHASH", + H, AES_BLOCK_SIZE); + return aes; +} + + +static void aes_gcm_prepare_j0(const u8 *iv, size_t iv_len, const u8 *H, u8 *J0) +{ + u8 len_buf[16]; + + if (iv_len == 12) { + /* Prepare block J_0 = IV || 0^31 || 1 [len(IV) = 96] */ + os_memcpy(J0, iv, iv_len); + os_memset(J0 + iv_len, 0, AES_BLOCK_SIZE - iv_len); + J0[AES_BLOCK_SIZE - 1] = 0x01; + } else { + /* + * s = 128 * ceil(len(IV)/128) - len(IV) + * J_0 = GHASH_H(IV || 0^(s+64) || [len(IV)]_64) + */ + ghash_start(J0); + ghash(H, iv, iv_len, J0); + WPA_PUT_BE64(len_buf, 0); + WPA_PUT_BE64(len_buf + 8, iv_len * 8); + ghash(H, len_buf, sizeof(len_buf), J0); + } +} + + +static void aes_gcm_gctr(void *aes, const u8 *J0, const u8 *in, size_t len, + u8 *out) +{ + u8 J0inc[AES_BLOCK_SIZE]; + + if (len == 0) + return; + + os_memcpy(J0inc, J0, AES_BLOCK_SIZE); + inc32(J0inc); + aes_gctr(aes, J0inc, in, len, out); +} + + +static void aes_gcm_ghash(const u8 *H, const u8 *aad, size_t aad_len, + const u8 *crypt, size_t crypt_len, u8 *S) +{ + u8 len_buf[16]; + + /* + * u = 128 * ceil[len(C)/128] - len(C) + * v = 128 * ceil[len(A)/128] - len(A) + * S = GHASH_H(A || 0^v || C || 0^u || [len(A)]64 || [len(C)]64) + * (i.e., zero padded to block size A || C and lengths of each in bits) + */ + ghash_start(S); + ghash(H, aad, aad_len, S); + ghash(H, crypt, crypt_len, S); + WPA_PUT_BE64(len_buf, aad_len * 8); + WPA_PUT_BE64(len_buf + 8, crypt_len * 8); + ghash(H, len_buf, sizeof(len_buf), S); + + wpa_hexdump_key(_MSG_EXCESSIVE_, "S = GHASH_H(...)", S, 16); +} + + +/** + * aes_gcm_ae - GCM-AE_K(IV, P, A) + */ +int aes_gcm_ae(const u8 *key, size_t key_len, const u8 *iv, size_t iv_len, + const u8 *plain, size_t plain_len, + const u8 *aad, size_t aad_len, u8 *crypt, u8 *tag) +{ + u8 H[AES_BLOCK_SIZE]; + u8 J0[AES_BLOCK_SIZE]; + u8 S[16]; + void *aes; + + aes = aes_gcm_init_hash_subkey(key, key_len, H); + if (aes == NULL) + return -1; + + aes_gcm_prepare_j0(iv, iv_len, H, J0); + + /* C = GCTR_K(inc_32(J_0), P) */ + aes_gcm_gctr(aes, J0, plain, plain_len, crypt); + + aes_gcm_ghash(H, aad, aad_len, crypt, plain_len, S); + + /* T = MSB_t(GCTR_K(J_0, S)) */ + aes_gctr(aes, J0, S, sizeof(S), tag); + + /* Return (C, T) */ + + aes_encrypt_deinit(aes); + + return 0; +} + + +/** + * aes_gcm_ad - GCM-AD_K(IV, C, A, T) + */ +int aes_gcm_ad(const u8 *key, size_t key_len, const u8 *iv, size_t iv_len, + const u8 *crypt, size_t crypt_len, + const u8 *aad, size_t aad_len, const u8 *tag, u8 *plain) +{ + u8 H[AES_BLOCK_SIZE]; + u8 J0[AES_BLOCK_SIZE]; + u8 S[16], T[16]; + void *aes; + + aes = aes_gcm_init_hash_subkey(key, key_len, H); + if (aes == NULL) + return -1; + + aes_gcm_prepare_j0(iv, iv_len, H, J0); + + /* P = GCTR_K(inc_32(J_0), C) */ + aes_gcm_gctr(aes, J0, crypt, crypt_len, plain); + + aes_gcm_ghash(H, aad, aad_len, crypt, crypt_len, S); + + /* T' = MSB_t(GCTR_K(J_0, S)) */ + aes_gctr(aes, J0, S, sizeof(S), T); + + aes_encrypt_deinit(aes); + + if (os_memcmp_const(tag, T, 16) != 0) { + wpa_printf(_MSG_EXCESSIVE_, "GCM: Tag mismatch"); + return -1; + } + + return 0; +} + + +int aes_gmac(const u8 *key, size_t key_len, const u8 *iv, size_t iv_len, + const u8 *aad, size_t aad_len, u8 *tag) +{ + return aes_gcm_ae(key, key_len, iv, iv_len, NULL, 0, aad, aad_len, NULL, + tag); +} diff --git a/core/crypto/aes-internal-enc.c b/core/crypto/aes-internal-enc.c new file mode 100644 index 0000000..69e256b --- /dev/null +++ b/core/crypto/aes-internal-enc.c @@ -0,0 +1,129 @@ +/* + * AES (Rijndael) cipher - encrypt + * + * Modifications to public domain implementation: + * - cleanup + * - use C pre-processor to make it easier to change S table access + * - added option (AES_SMALL_TABLES) for reducing code size by about 8 kB at + * cost of reduced throughput (quite small difference on Pentium 4, + * 10-25% when using -O1 or -O2 optimization) + * + * Copyright (c) 2003-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes_i.h" + +static void rijndaelEncrypt(const u32 rk[], int Nr, const u8 pt[16], u8 ct[16]) +{ + u32 s0, s1, s2, s3, t0, t1, t2, t3; +#ifndef FULL_UNROLL + int r; +#endif /* ?FULL_UNROLL */ + + /* + * map byte array block to cipher state + * and add initial round key: + */ + s0 = GETU32(pt ) ^ rk[0]; + s1 = GETU32(pt + 4) ^ rk[1]; + s2 = GETU32(pt + 8) ^ rk[2]; + s3 = GETU32(pt + 12) ^ rk[3]; + +#define ROUND(i,d,s) \ +d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \ +d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \ +d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \ +d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3] + +#ifdef FULL_UNROLL + + ROUND(1,t,s); + ROUND(2,s,t); + ROUND(3,t,s); + ROUND(4,s,t); + ROUND(5,t,s); + ROUND(6,s,t); + ROUND(7,t,s); + ROUND(8,s,t); + ROUND(9,t,s); + if (Nr > 10) { + ROUND(10,s,t); + ROUND(11,t,s); + if (Nr > 12) { + ROUND(12,s,t); + ROUND(13,t,s); + } + } + + rk += Nr << 2; + +#else /* !FULL_UNROLL */ + + /* Nr - 1 full rounds: */ + r = Nr >> 1; + for (;;) { + ROUND(1,t,s); + rk += 8; + if (--r == 0) + break; + ROUND(0,s,t); + } + +#endif /* ?FULL_UNROLL */ + +#undef ROUND + + /* + * apply last round and + * map cipher state to byte array block: + */ + s0 = TE41(t0) ^ TE42(t1) ^ TE43(t2) ^ TE44(t3) ^ rk[0]; + PUTU32(ct , s0); + s1 = TE41(t1) ^ TE42(t2) ^ TE43(t3) ^ TE44(t0) ^ rk[1]; + PUTU32(ct + 4, s1); + s2 = TE41(t2) ^ TE42(t3) ^ TE43(t0) ^ TE44(t1) ^ rk[2]; + PUTU32(ct + 8, s2); + s3 = TE41(t3) ^ TE42(t0) ^ TE43(t1) ^ TE44(t2) ^ rk[3]; + PUTU32(ct + 12, s3); +} + + +void * aes_encrypt_init(const u8 *key, size_t len) +{ + u32 *rk; + int res; + + if (TEST_FAIL()) + return NULL; + + rk = os_malloc(AES_PRIV_SIZE); + if (rk == NULL) + return NULL; + res = rijndaelKeySetupEnc(rk, key, len * 8); + if (res < 0) { + rtw_mfree(rk, AES_PRIV_SIZE); + return NULL; + } + rk[AES_PRIV_NR_POS] = res; + return rk; +} + + +int aes_encrypt(void *ctx, const u8 *plain, u8 *crypt) +{ + u32 *rk = ctx; + rijndaelEncrypt(ctx, rk[AES_PRIV_NR_POS], plain, crypt); + return 0; +} + + +void aes_encrypt_deinit(void *ctx) +{ + os_memset(ctx, 0, AES_PRIV_SIZE); + rtw_mfree(ctx, AES_PRIV_SIZE); +} diff --git a/core/crypto/aes-internal.c b/core/crypto/aes-internal.c new file mode 100644 index 0000000..57d6539 --- /dev/null +++ b/core/crypto/aes-internal.c @@ -0,0 +1,843 @@ +/* + * AES (Rijndael) cipher + * + * Modifications to public domain implementation: + * - cleanup + * - use C pre-processor to make it easier to change S table access + * - added option (AES_SMALL_TABLES) for reducing code size by about 8 kB at + * cost of reduced throughput (quite small difference on Pentium 4, + * 10-25% when using -O1 or -O2 optimization) + * + * Copyright (c) 2003-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes_i.h" + +/* + * rijndael-alg-fst.c + * + * @version 3.0 (December 2000) + * + * Optimised ANSI C code for the Rijndael cipher (now AES) + * + * @author Vincent Rijmen + * @author Antoon Bosselaers + * @author Paulo Barreto + * + * This code is hereby placed in the public domain. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ''AS IS'' AND ANY EXPRESS + * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + +/* +Te0[x] = S [x].[02, 01, 01, 03]; +Te1[x] = S [x].[03, 02, 01, 01]; +Te2[x] = S [x].[01, 03, 02, 01]; +Te3[x] = S [x].[01, 01, 03, 02]; +Te4[x] = S [x].[01, 01, 01, 01]; + +Td0[x] = Si[x].[0e, 09, 0d, 0b]; +Td1[x] = Si[x].[0b, 0e, 09, 0d]; +Td2[x] = Si[x].[0d, 0b, 0e, 09]; +Td3[x] = Si[x].[09, 0d, 0b, 0e]; +Td4[x] = Si[x].[01, 01, 01, 01]; +*/ + +const u32 Te0[256] = { + 0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU, + 0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U, + 0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU, + 0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU, + 0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U, + 0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU, + 0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU, + 0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU, + 0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU, + 0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU, + 0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U, + 0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU, + 0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU, + 0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U, + 0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU, + 0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU, + 0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU, + 0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU, + 0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU, + 0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U, + 0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU, + 0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU, + 0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU, + 0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU, + 0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U, + 0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U, + 0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U, + 0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U, + 0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU, + 0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U, + 0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U, + 0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU, + 0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU, + 0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U, + 0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U, + 0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U, + 0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU, + 0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U, + 0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU, + 0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U, + 0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU, + 0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U, + 0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U, + 0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU, + 0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U, + 0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U, + 0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U, + 0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U, + 0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U, + 0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U, + 0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U, + 0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U, + 0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU, + 0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U, + 0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U, + 0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U, + 0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U, + 0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U, + 0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U, + 0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU, + 0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U, + 0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U, + 0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U, + 0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU, +}; +#ifndef AES_SMALL_TABLES +const u32 Te1[256] = { + 0xa5c66363U, 0x84f87c7cU, 0x99ee7777U, 0x8df67b7bU, + 0x0dfff2f2U, 0xbdd66b6bU, 0xb1de6f6fU, 0x5491c5c5U, + 0x50603030U, 0x03020101U, 0xa9ce6767U, 0x7d562b2bU, + 0x19e7fefeU, 0x62b5d7d7U, 0xe64dababU, 0x9aec7676U, + 0x458fcacaU, 0x9d1f8282U, 0x4089c9c9U, 0x87fa7d7dU, + 0x15effafaU, 0xebb25959U, 0xc98e4747U, 0x0bfbf0f0U, + 0xec41adadU, 0x67b3d4d4U, 0xfd5fa2a2U, 0xea45afafU, + 0xbf239c9cU, 0xf753a4a4U, 0x96e47272U, 0x5b9bc0c0U, + 0xc275b7b7U, 0x1ce1fdfdU, 0xae3d9393U, 0x6a4c2626U, + 0x5a6c3636U, 0x417e3f3fU, 0x02f5f7f7U, 0x4f83ccccU, + 0x5c683434U, 0xf451a5a5U, 0x34d1e5e5U, 0x08f9f1f1U, + 0x93e27171U, 0x73abd8d8U, 0x53623131U, 0x3f2a1515U, + 0x0c080404U, 0x5295c7c7U, 0x65462323U, 0x5e9dc3c3U, + 0x28301818U, 0xa1379696U, 0x0f0a0505U, 0xb52f9a9aU, + 0x090e0707U, 0x36241212U, 0x9b1b8080U, 0x3ddfe2e2U, + 0x26cdebebU, 0x694e2727U, 0xcd7fb2b2U, 0x9fea7575U, + 0x1b120909U, 0x9e1d8383U, 0x74582c2cU, 0x2e341a1aU, + 0x2d361b1bU, 0xb2dc6e6eU, 0xeeb45a5aU, 0xfb5ba0a0U, + 0xf6a45252U, 0x4d763b3bU, 0x61b7d6d6U, 0xce7db3b3U, + 0x7b522929U, 0x3edde3e3U, 0x715e2f2fU, 0x97138484U, + 0xf5a65353U, 0x68b9d1d1U, 0x00000000U, 0x2cc1ededU, + 0x60402020U, 0x1fe3fcfcU, 0xc879b1b1U, 0xedb65b5bU, + 0xbed46a6aU, 0x468dcbcbU, 0xd967bebeU, 0x4b723939U, + 0xde944a4aU, 0xd4984c4cU, 0xe8b05858U, 0x4a85cfcfU, + 0x6bbbd0d0U, 0x2ac5efefU, 0xe54faaaaU, 0x16edfbfbU, + 0xc5864343U, 0xd79a4d4dU, 0x55663333U, 0x94118585U, + 0xcf8a4545U, 0x10e9f9f9U, 0x06040202U, 0x81fe7f7fU, + 0xf0a05050U, 0x44783c3cU, 0xba259f9fU, 0xe34ba8a8U, + 0xf3a25151U, 0xfe5da3a3U, 0xc0804040U, 0x8a058f8fU, + 0xad3f9292U, 0xbc219d9dU, 0x48703838U, 0x04f1f5f5U, + 0xdf63bcbcU, 0xc177b6b6U, 0x75afdadaU, 0x63422121U, + 0x30201010U, 0x1ae5ffffU, 0x0efdf3f3U, 0x6dbfd2d2U, + 0x4c81cdcdU, 0x14180c0cU, 0x35261313U, 0x2fc3ececU, + 0xe1be5f5fU, 0xa2359797U, 0xcc884444U, 0x392e1717U, + 0x5793c4c4U, 0xf255a7a7U, 0x82fc7e7eU, 0x477a3d3dU, + 0xacc86464U, 0xe7ba5d5dU, 0x2b321919U, 0x95e67373U, + 0xa0c06060U, 0x98198181U, 0xd19e4f4fU, 0x7fa3dcdcU, + 0x66442222U, 0x7e542a2aU, 0xab3b9090U, 0x830b8888U, + 0xca8c4646U, 0x29c7eeeeU, 0xd36bb8b8U, 0x3c281414U, + 0x79a7dedeU, 0xe2bc5e5eU, 0x1d160b0bU, 0x76addbdbU, + 0x3bdbe0e0U, 0x56643232U, 0x4e743a3aU, 0x1e140a0aU, + 0xdb924949U, 0x0a0c0606U, 0x6c482424U, 0xe4b85c5cU, + 0x5d9fc2c2U, 0x6ebdd3d3U, 0xef43acacU, 0xa6c46262U, + 0xa8399191U, 0xa4319595U, 0x37d3e4e4U, 0x8bf27979U, + 0x32d5e7e7U, 0x438bc8c8U, 0x596e3737U, 0xb7da6d6dU, + 0x8c018d8dU, 0x64b1d5d5U, 0xd29c4e4eU, 0xe049a9a9U, + 0xb4d86c6cU, 0xfaac5656U, 0x07f3f4f4U, 0x25cfeaeaU, + 0xafca6565U, 0x8ef47a7aU, 0xe947aeaeU, 0x18100808U, + 0xd56fbabaU, 0x88f07878U, 0x6f4a2525U, 0x725c2e2eU, + 0x24381c1cU, 0xf157a6a6U, 0xc773b4b4U, 0x5197c6c6U, + 0x23cbe8e8U, 0x7ca1ddddU, 0x9ce87474U, 0x213e1f1fU, + 0xdd964b4bU, 0xdc61bdbdU, 0x860d8b8bU, 0x850f8a8aU, + 0x90e07070U, 0x427c3e3eU, 0xc471b5b5U, 0xaacc6666U, + 0xd8904848U, 0x05060303U, 0x01f7f6f6U, 0x121c0e0eU, + 0xa3c26161U, 0x5f6a3535U, 0xf9ae5757U, 0xd069b9b9U, + 0x91178686U, 0x5899c1c1U, 0x273a1d1dU, 0xb9279e9eU, + 0x38d9e1e1U, 0x13ebf8f8U, 0xb32b9898U, 0x33221111U, + 0xbbd26969U, 0x70a9d9d9U, 0x89078e8eU, 0xa7339494U, + 0xb62d9b9bU, 0x223c1e1eU, 0x92158787U, 0x20c9e9e9U, + 0x4987ceceU, 0xffaa5555U, 0x78502828U, 0x7aa5dfdfU, + 0x8f038c8cU, 0xf859a1a1U, 0x80098989U, 0x171a0d0dU, + 0xda65bfbfU, 0x31d7e6e6U, 0xc6844242U, 0xb8d06868U, + 0xc3824141U, 0xb0299999U, 0x775a2d2dU, 0x111e0f0fU, + 0xcb7bb0b0U, 0xfca85454U, 0xd66dbbbbU, 0x3a2c1616U, +}; +const u32 Te2[256] = { + 0x63a5c663U, 0x7c84f87cU, 0x7799ee77U, 0x7b8df67bU, + 0xf20dfff2U, 0x6bbdd66bU, 0x6fb1de6fU, 0xc55491c5U, + 0x30506030U, 0x01030201U, 0x67a9ce67U, 0x2b7d562bU, + 0xfe19e7feU, 0xd762b5d7U, 0xabe64dabU, 0x769aec76U, + 0xca458fcaU, 0x829d1f82U, 0xc94089c9U, 0x7d87fa7dU, + 0xfa15effaU, 0x59ebb259U, 0x47c98e47U, 0xf00bfbf0U, + 0xadec41adU, 0xd467b3d4U, 0xa2fd5fa2U, 0xafea45afU, + 0x9cbf239cU, 0xa4f753a4U, 0x7296e472U, 0xc05b9bc0U, + 0xb7c275b7U, 0xfd1ce1fdU, 0x93ae3d93U, 0x266a4c26U, + 0x365a6c36U, 0x3f417e3fU, 0xf702f5f7U, 0xcc4f83ccU, + 0x345c6834U, 0xa5f451a5U, 0xe534d1e5U, 0xf108f9f1U, + 0x7193e271U, 0xd873abd8U, 0x31536231U, 0x153f2a15U, + 0x040c0804U, 0xc75295c7U, 0x23654623U, 0xc35e9dc3U, + 0x18283018U, 0x96a13796U, 0x050f0a05U, 0x9ab52f9aU, + 0x07090e07U, 0x12362412U, 0x809b1b80U, 0xe23ddfe2U, + 0xeb26cdebU, 0x27694e27U, 0xb2cd7fb2U, 0x759fea75U, + 0x091b1209U, 0x839e1d83U, 0x2c74582cU, 0x1a2e341aU, + 0x1b2d361bU, 0x6eb2dc6eU, 0x5aeeb45aU, 0xa0fb5ba0U, + 0x52f6a452U, 0x3b4d763bU, 0xd661b7d6U, 0xb3ce7db3U, + 0x297b5229U, 0xe33edde3U, 0x2f715e2fU, 0x84971384U, + 0x53f5a653U, 0xd168b9d1U, 0x00000000U, 0xed2cc1edU, + 0x20604020U, 0xfc1fe3fcU, 0xb1c879b1U, 0x5bedb65bU, + 0x6abed46aU, 0xcb468dcbU, 0xbed967beU, 0x394b7239U, + 0x4ade944aU, 0x4cd4984cU, 0x58e8b058U, 0xcf4a85cfU, + 0xd06bbbd0U, 0xef2ac5efU, 0xaae54faaU, 0xfb16edfbU, + 0x43c58643U, 0x4dd79a4dU, 0x33556633U, 0x85941185U, + 0x45cf8a45U, 0xf910e9f9U, 0x02060402U, 0x7f81fe7fU, + 0x50f0a050U, 0x3c44783cU, 0x9fba259fU, 0xa8e34ba8U, + 0x51f3a251U, 0xa3fe5da3U, 0x40c08040U, 0x8f8a058fU, + 0x92ad3f92U, 0x9dbc219dU, 0x38487038U, 0xf504f1f5U, + 0xbcdf63bcU, 0xb6c177b6U, 0xda75afdaU, 0x21634221U, + 0x10302010U, 0xff1ae5ffU, 0xf30efdf3U, 0xd26dbfd2U, + 0xcd4c81cdU, 0x0c14180cU, 0x13352613U, 0xec2fc3ecU, + 0x5fe1be5fU, 0x97a23597U, 0x44cc8844U, 0x17392e17U, + 0xc45793c4U, 0xa7f255a7U, 0x7e82fc7eU, 0x3d477a3dU, + 0x64acc864U, 0x5de7ba5dU, 0x192b3219U, 0x7395e673U, + 0x60a0c060U, 0x81981981U, 0x4fd19e4fU, 0xdc7fa3dcU, + 0x22664422U, 0x2a7e542aU, 0x90ab3b90U, 0x88830b88U, + 0x46ca8c46U, 0xee29c7eeU, 0xb8d36bb8U, 0x143c2814U, + 0xde79a7deU, 0x5ee2bc5eU, 0x0b1d160bU, 0xdb76addbU, + 0xe03bdbe0U, 0x32566432U, 0x3a4e743aU, 0x0a1e140aU, + 0x49db9249U, 0x060a0c06U, 0x246c4824U, 0x5ce4b85cU, + 0xc25d9fc2U, 0xd36ebdd3U, 0xacef43acU, 0x62a6c462U, + 0x91a83991U, 0x95a43195U, 0xe437d3e4U, 0x798bf279U, + 0xe732d5e7U, 0xc8438bc8U, 0x37596e37U, 0x6db7da6dU, + 0x8d8c018dU, 0xd564b1d5U, 0x4ed29c4eU, 0xa9e049a9U, + 0x6cb4d86cU, 0x56faac56U, 0xf407f3f4U, 0xea25cfeaU, + 0x65afca65U, 0x7a8ef47aU, 0xaee947aeU, 0x08181008U, + 0xbad56fbaU, 0x7888f078U, 0x256f4a25U, 0x2e725c2eU, + 0x1c24381cU, 0xa6f157a6U, 0xb4c773b4U, 0xc65197c6U, + 0xe823cbe8U, 0xdd7ca1ddU, 0x749ce874U, 0x1f213e1fU, + 0x4bdd964bU, 0xbddc61bdU, 0x8b860d8bU, 0x8a850f8aU, + 0x7090e070U, 0x3e427c3eU, 0xb5c471b5U, 0x66aacc66U, + 0x48d89048U, 0x03050603U, 0xf601f7f6U, 0x0e121c0eU, + 0x61a3c261U, 0x355f6a35U, 0x57f9ae57U, 0xb9d069b9U, + 0x86911786U, 0xc15899c1U, 0x1d273a1dU, 0x9eb9279eU, + 0xe138d9e1U, 0xf813ebf8U, 0x98b32b98U, 0x11332211U, + 0x69bbd269U, 0xd970a9d9U, 0x8e89078eU, 0x94a73394U, + 0x9bb62d9bU, 0x1e223c1eU, 0x87921587U, 0xe920c9e9U, + 0xce4987ceU, 0x55ffaa55U, 0x28785028U, 0xdf7aa5dfU, + 0x8c8f038cU, 0xa1f859a1U, 0x89800989U, 0x0d171a0dU, + 0xbfda65bfU, 0xe631d7e6U, 0x42c68442U, 0x68b8d068U, + 0x41c38241U, 0x99b02999U, 0x2d775a2dU, 0x0f111e0fU, + 0xb0cb7bb0U, 0x54fca854U, 0xbbd66dbbU, 0x163a2c16U, +}; +const u32 Te3[256] = { + + 0x6363a5c6U, 0x7c7c84f8U, 0x777799eeU, 0x7b7b8df6U, + 0xf2f20dffU, 0x6b6bbdd6U, 0x6f6fb1deU, 0xc5c55491U, + 0x30305060U, 0x01010302U, 0x6767a9ceU, 0x2b2b7d56U, + 0xfefe19e7U, 0xd7d762b5U, 0xababe64dU, 0x76769aecU, + 0xcaca458fU, 0x82829d1fU, 0xc9c94089U, 0x7d7d87faU, + 0xfafa15efU, 0x5959ebb2U, 0x4747c98eU, 0xf0f00bfbU, + 0xadadec41U, 0xd4d467b3U, 0xa2a2fd5fU, 0xafafea45U, + 0x9c9cbf23U, 0xa4a4f753U, 0x727296e4U, 0xc0c05b9bU, + 0xb7b7c275U, 0xfdfd1ce1U, 0x9393ae3dU, 0x26266a4cU, + 0x36365a6cU, 0x3f3f417eU, 0xf7f702f5U, 0xcccc4f83U, + 0x34345c68U, 0xa5a5f451U, 0xe5e534d1U, 0xf1f108f9U, + 0x717193e2U, 0xd8d873abU, 0x31315362U, 0x15153f2aU, + 0x04040c08U, 0xc7c75295U, 0x23236546U, 0xc3c35e9dU, + 0x18182830U, 0x9696a137U, 0x05050f0aU, 0x9a9ab52fU, + 0x0707090eU, 0x12123624U, 0x80809b1bU, 0xe2e23ddfU, + 0xebeb26cdU, 0x2727694eU, 0xb2b2cd7fU, 0x75759feaU, + 0x09091b12U, 0x83839e1dU, 0x2c2c7458U, 0x1a1a2e34U, + 0x1b1b2d36U, 0x6e6eb2dcU, 0x5a5aeeb4U, 0xa0a0fb5bU, + 0x5252f6a4U, 0x3b3b4d76U, 0xd6d661b7U, 0xb3b3ce7dU, + 0x29297b52U, 0xe3e33eddU, 0x2f2f715eU, 0x84849713U, + 0x5353f5a6U, 0xd1d168b9U, 0x00000000U, 0xeded2cc1U, + 0x20206040U, 0xfcfc1fe3U, 0xb1b1c879U, 0x5b5bedb6U, + 0x6a6abed4U, 0xcbcb468dU, 0xbebed967U, 0x39394b72U, + 0x4a4ade94U, 0x4c4cd498U, 0x5858e8b0U, 0xcfcf4a85U, + 0xd0d06bbbU, 0xefef2ac5U, 0xaaaae54fU, 0xfbfb16edU, + 0x4343c586U, 0x4d4dd79aU, 0x33335566U, 0x85859411U, + 0x4545cf8aU, 0xf9f910e9U, 0x02020604U, 0x7f7f81feU, + 0x5050f0a0U, 0x3c3c4478U, 0x9f9fba25U, 0xa8a8e34bU, + 0x5151f3a2U, 0xa3a3fe5dU, 0x4040c080U, 0x8f8f8a05U, + 0x9292ad3fU, 0x9d9dbc21U, 0x38384870U, 0xf5f504f1U, + 0xbcbcdf63U, 0xb6b6c177U, 0xdada75afU, 0x21216342U, + 0x10103020U, 0xffff1ae5U, 0xf3f30efdU, 0xd2d26dbfU, + 0xcdcd4c81U, 0x0c0c1418U, 0x13133526U, 0xecec2fc3U, + 0x5f5fe1beU, 0x9797a235U, 0x4444cc88U, 0x1717392eU, + 0xc4c45793U, 0xa7a7f255U, 0x7e7e82fcU, 0x3d3d477aU, + 0x6464acc8U, 0x5d5de7baU, 0x19192b32U, 0x737395e6U, + 0x6060a0c0U, 0x81819819U, 0x4f4fd19eU, 0xdcdc7fa3U, + 0x22226644U, 0x2a2a7e54U, 0x9090ab3bU, 0x8888830bU, + 0x4646ca8cU, 0xeeee29c7U, 0xb8b8d36bU, 0x14143c28U, + 0xdede79a7U, 0x5e5ee2bcU, 0x0b0b1d16U, 0xdbdb76adU, + 0xe0e03bdbU, 0x32325664U, 0x3a3a4e74U, 0x0a0a1e14U, + 0x4949db92U, 0x06060a0cU, 0x24246c48U, 0x5c5ce4b8U, + 0xc2c25d9fU, 0xd3d36ebdU, 0xacacef43U, 0x6262a6c4U, + 0x9191a839U, 0x9595a431U, 0xe4e437d3U, 0x79798bf2U, + 0xe7e732d5U, 0xc8c8438bU, 0x3737596eU, 0x6d6db7daU, + 0x8d8d8c01U, 0xd5d564b1U, 0x4e4ed29cU, 0xa9a9e049U, + 0x6c6cb4d8U, 0x5656faacU, 0xf4f407f3U, 0xeaea25cfU, + 0x6565afcaU, 0x7a7a8ef4U, 0xaeaee947U, 0x08081810U, + 0xbabad56fU, 0x787888f0U, 0x25256f4aU, 0x2e2e725cU, + 0x1c1c2438U, 0xa6a6f157U, 0xb4b4c773U, 0xc6c65197U, + 0xe8e823cbU, 0xdddd7ca1U, 0x74749ce8U, 0x1f1f213eU, + 0x4b4bdd96U, 0xbdbddc61U, 0x8b8b860dU, 0x8a8a850fU, + 0x707090e0U, 0x3e3e427cU, 0xb5b5c471U, 0x6666aaccU, + 0x4848d890U, 0x03030506U, 0xf6f601f7U, 0x0e0e121cU, + 0x6161a3c2U, 0x35355f6aU, 0x5757f9aeU, 0xb9b9d069U, + 0x86869117U, 0xc1c15899U, 0x1d1d273aU, 0x9e9eb927U, + 0xe1e138d9U, 0xf8f813ebU, 0x9898b32bU, 0x11113322U, + 0x6969bbd2U, 0xd9d970a9U, 0x8e8e8907U, 0x9494a733U, + 0x9b9bb62dU, 0x1e1e223cU, 0x87879215U, 0xe9e920c9U, + 0xcece4987U, 0x5555ffaaU, 0x28287850U, 0xdfdf7aa5U, + 0x8c8c8f03U, 0xa1a1f859U, 0x89898009U, 0x0d0d171aU, + 0xbfbfda65U, 0xe6e631d7U, 0x4242c684U, 0x6868b8d0U, + 0x4141c382U, 0x9999b029U, 0x2d2d775aU, 0x0f0f111eU, + 0xb0b0cb7bU, 0x5454fca8U, 0xbbbbd66dU, 0x16163a2cU, +}; +const u32 Te4[256] = { + 0x63636363U, 0x7c7c7c7cU, 0x77777777U, 0x7b7b7b7bU, + 0xf2f2f2f2U, 0x6b6b6b6bU, 0x6f6f6f6fU, 0xc5c5c5c5U, + 0x30303030U, 0x01010101U, 0x67676767U, 0x2b2b2b2bU, + 0xfefefefeU, 0xd7d7d7d7U, 0xababababU, 0x76767676U, + 0xcacacacaU, 0x82828282U, 0xc9c9c9c9U, 0x7d7d7d7dU, + 0xfafafafaU, 0x59595959U, 0x47474747U, 0xf0f0f0f0U, + 0xadadadadU, 0xd4d4d4d4U, 0xa2a2a2a2U, 0xafafafafU, + 0x9c9c9c9cU, 0xa4a4a4a4U, 0x72727272U, 0xc0c0c0c0U, + 0xb7b7b7b7U, 0xfdfdfdfdU, 0x93939393U, 0x26262626U, + 0x36363636U, 0x3f3f3f3fU, 0xf7f7f7f7U, 0xccccccccU, + 0x34343434U, 0xa5a5a5a5U, 0xe5e5e5e5U, 0xf1f1f1f1U, + 0x71717171U, 0xd8d8d8d8U, 0x31313131U, 0x15151515U, + 0x04040404U, 0xc7c7c7c7U, 0x23232323U, 0xc3c3c3c3U, + 0x18181818U, 0x96969696U, 0x05050505U, 0x9a9a9a9aU, + 0x07070707U, 0x12121212U, 0x80808080U, 0xe2e2e2e2U, + 0xebebebebU, 0x27272727U, 0xb2b2b2b2U, 0x75757575U, + 0x09090909U, 0x83838383U, 0x2c2c2c2cU, 0x1a1a1a1aU, + 0x1b1b1b1bU, 0x6e6e6e6eU, 0x5a5a5a5aU, 0xa0a0a0a0U, + 0x52525252U, 0x3b3b3b3bU, 0xd6d6d6d6U, 0xb3b3b3b3U, + 0x29292929U, 0xe3e3e3e3U, 0x2f2f2f2fU, 0x84848484U, + 0x53535353U, 0xd1d1d1d1U, 0x00000000U, 0xededededU, + 0x20202020U, 0xfcfcfcfcU, 0xb1b1b1b1U, 0x5b5b5b5bU, + 0x6a6a6a6aU, 0xcbcbcbcbU, 0xbebebebeU, 0x39393939U, + 0x4a4a4a4aU, 0x4c4c4c4cU, 0x58585858U, 0xcfcfcfcfU, + 0xd0d0d0d0U, 0xefefefefU, 0xaaaaaaaaU, 0xfbfbfbfbU, + 0x43434343U, 0x4d4d4d4dU, 0x33333333U, 0x85858585U, + 0x45454545U, 0xf9f9f9f9U, 0x02020202U, 0x7f7f7f7fU, + 0x50505050U, 0x3c3c3c3cU, 0x9f9f9f9fU, 0xa8a8a8a8U, + 0x51515151U, 0xa3a3a3a3U, 0x40404040U, 0x8f8f8f8fU, + 0x92929292U, 0x9d9d9d9dU, 0x38383838U, 0xf5f5f5f5U, + 0xbcbcbcbcU, 0xb6b6b6b6U, 0xdadadadaU, 0x21212121U, + 0x10101010U, 0xffffffffU, 0xf3f3f3f3U, 0xd2d2d2d2U, + 0xcdcdcdcdU, 0x0c0c0c0cU, 0x13131313U, 0xececececU, + 0x5f5f5f5fU, 0x97979797U, 0x44444444U, 0x17171717U, + 0xc4c4c4c4U, 0xa7a7a7a7U, 0x7e7e7e7eU, 0x3d3d3d3dU, + 0x64646464U, 0x5d5d5d5dU, 0x19191919U, 0x73737373U, + 0x60606060U, 0x81818181U, 0x4f4f4f4fU, 0xdcdcdcdcU, + 0x22222222U, 0x2a2a2a2aU, 0x90909090U, 0x88888888U, + 0x46464646U, 0xeeeeeeeeU, 0xb8b8b8b8U, 0x14141414U, + 0xdedededeU, 0x5e5e5e5eU, 0x0b0b0b0bU, 0xdbdbdbdbU, + 0xe0e0e0e0U, 0x32323232U, 0x3a3a3a3aU, 0x0a0a0a0aU, + 0x49494949U, 0x06060606U, 0x24242424U, 0x5c5c5c5cU, + 0xc2c2c2c2U, 0xd3d3d3d3U, 0xacacacacU, 0x62626262U, + 0x91919191U, 0x95959595U, 0xe4e4e4e4U, 0x79797979U, + 0xe7e7e7e7U, 0xc8c8c8c8U, 0x37373737U, 0x6d6d6d6dU, + 0x8d8d8d8dU, 0xd5d5d5d5U, 0x4e4e4e4eU, 0xa9a9a9a9U, + 0x6c6c6c6cU, 0x56565656U, 0xf4f4f4f4U, 0xeaeaeaeaU, + 0x65656565U, 0x7a7a7a7aU, 0xaeaeaeaeU, 0x08080808U, + 0xbabababaU, 0x78787878U, 0x25252525U, 0x2e2e2e2eU, + 0x1c1c1c1cU, 0xa6a6a6a6U, 0xb4b4b4b4U, 0xc6c6c6c6U, + 0xe8e8e8e8U, 0xddddddddU, 0x74747474U, 0x1f1f1f1fU, + 0x4b4b4b4bU, 0xbdbdbdbdU, 0x8b8b8b8bU, 0x8a8a8a8aU, + 0x70707070U, 0x3e3e3e3eU, 0xb5b5b5b5U, 0x66666666U, + 0x48484848U, 0x03030303U, 0xf6f6f6f6U, 0x0e0e0e0eU, + 0x61616161U, 0x35353535U, 0x57575757U, 0xb9b9b9b9U, + 0x86868686U, 0xc1c1c1c1U, 0x1d1d1d1dU, 0x9e9e9e9eU, + 0xe1e1e1e1U, 0xf8f8f8f8U, 0x98989898U, 0x11111111U, + 0x69696969U, 0xd9d9d9d9U, 0x8e8e8e8eU, 0x94949494U, + 0x9b9b9b9bU, 0x1e1e1e1eU, 0x87878787U, 0xe9e9e9e9U, + 0xcecececeU, 0x55555555U, 0x28282828U, 0xdfdfdfdfU, + 0x8c8c8c8cU, 0xa1a1a1a1U, 0x89898989U, 0x0d0d0d0dU, + 0xbfbfbfbfU, 0xe6e6e6e6U, 0x42424242U, 0x68686868U, + 0x41414141U, 0x99999999U, 0x2d2d2d2dU, 0x0f0f0f0fU, + 0xb0b0b0b0U, 0x54545454U, 0xbbbbbbbbU, 0x16161616U, +}; +#endif /* AES_SMALL_TABLES */ +const u32 Td0[256] = { + 0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U, + 0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U, + 0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U, + 0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU, + 0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U, + 0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U, + 0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU, + 0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U, + 0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU, + 0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U, + 0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U, + 0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U, + 0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U, + 0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU, + 0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U, + 0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU, + 0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U, + 0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU, + 0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U, + 0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U, + 0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U, + 0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU, + 0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U, + 0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU, + 0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U, + 0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU, + 0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U, + 0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU, + 0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU, + 0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U, + 0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU, + 0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U, + 0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU, + 0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U, + 0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U, + 0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U, + 0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU, + 0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U, + 0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U, + 0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU, + 0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U, + 0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U, + 0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U, + 0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U, + 0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U, + 0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU, + 0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U, + 0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U, + 0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U, + 0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U, + 0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U, + 0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU, + 0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU, + 0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU, + 0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU, + 0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U, + 0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U, + 0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU, + 0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU, + 0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U, + 0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU, + 0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U, + 0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U, + 0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U, +}; +#ifndef AES_SMALL_TABLES +const u32 Td1[256] = { + 0x5051f4a7U, 0x537e4165U, 0xc31a17a4U, 0x963a275eU, + 0xcb3bab6bU, 0xf11f9d45U, 0xabacfa58U, 0x934be303U, + 0x552030faU, 0xf6ad766dU, 0x9188cc76U, 0x25f5024cU, + 0xfc4fe5d7U, 0xd7c52acbU, 0x80263544U, 0x8fb562a3U, + 0x49deb15aU, 0x6725ba1bU, 0x9845ea0eU, 0xe15dfec0U, + 0x02c32f75U, 0x12814cf0U, 0xa38d4697U, 0xc66bd3f9U, + 0xe7038f5fU, 0x9515929cU, 0xebbf6d7aU, 0xda955259U, + 0x2dd4be83U, 0xd3587421U, 0x2949e069U, 0x448ec9c8U, + 0x6a75c289U, 0x78f48e79U, 0x6b99583eU, 0xdd27b971U, + 0xb6bee14fU, 0x17f088adU, 0x66c920acU, 0xb47dce3aU, + 0x1863df4aU, 0x82e51a31U, 0x60975133U, 0x4562537fU, + 0xe0b16477U, 0x84bb6baeU, 0x1cfe81a0U, 0x94f9082bU, + 0x58704868U, 0x198f45fdU, 0x8794de6cU, 0xb7527bf8U, + 0x23ab73d3U, 0xe2724b02U, 0x57e31f8fU, 0x2a6655abU, + 0x07b2eb28U, 0x032fb5c2U, 0x9a86c57bU, 0xa5d33708U, + 0xf2302887U, 0xb223bfa5U, 0xba02036aU, 0x5ced1682U, + 0x2b8acf1cU, 0x92a779b4U, 0xf0f307f2U, 0xa14e69e2U, + 0xcd65daf4U, 0xd50605beU, 0x1fd13462U, 0x8ac4a6feU, + 0x9d342e53U, 0xa0a2f355U, 0x32058ae1U, 0x75a4f6ebU, + 0x390b83ecU, 0xaa4060efU, 0x065e719fU, 0x51bd6e10U, + 0xf93e218aU, 0x3d96dd06U, 0xaedd3e05U, 0x464de6bdU, + 0xb591548dU, 0x0571c45dU, 0x6f0406d4U, 0xff605015U, + 0x241998fbU, 0x97d6bde9U, 0xcc894043U, 0x7767d99eU, + 0xbdb0e842U, 0x8807898bU, 0x38e7195bU, 0xdb79c8eeU, + 0x47a17c0aU, 0xe97c420fU, 0xc9f8841eU, 0x00000000U, + 0x83098086U, 0x48322bedU, 0xac1e1170U, 0x4e6c5a72U, + 0xfbfd0effU, 0x560f8538U, 0x1e3daed5U, 0x27362d39U, + 0x640a0fd9U, 0x21685ca6U, 0xd19b5b54U, 0x3a24362eU, + 0xb10c0a67U, 0x0f9357e7U, 0xd2b4ee96U, 0x9e1b9b91U, + 0x4f80c0c5U, 0xa261dc20U, 0x695a774bU, 0x161c121aU, + 0x0ae293baU, 0xe5c0a02aU, 0x433c22e0U, 0x1d121b17U, + 0x0b0e090dU, 0xadf28bc7U, 0xb92db6a8U, 0xc8141ea9U, + 0x8557f119U, 0x4caf7507U, 0xbbee99ddU, 0xfda37f60U, + 0x9ff70126U, 0xbc5c72f5U, 0xc544663bU, 0x345bfb7eU, + 0x768b4329U, 0xdccb23c6U, 0x68b6edfcU, 0x63b8e4f1U, + 0xcad731dcU, 0x10426385U, 0x40139722U, 0x2084c611U, + 0x7d854a24U, 0xf8d2bb3dU, 0x11aef932U, 0x6dc729a1U, + 0x4b1d9e2fU, 0xf3dcb230U, 0xec0d8652U, 0xd077c1e3U, + 0x6c2bb316U, 0x99a970b9U, 0xfa119448U, 0x2247e964U, + 0xc4a8fc8cU, 0x1aa0f03fU, 0xd8567d2cU, 0xef223390U, + 0xc787494eU, 0xc1d938d1U, 0xfe8ccaa2U, 0x3698d40bU, + 0xcfa6f581U, 0x28a57adeU, 0x26dab78eU, 0xa43fadbfU, + 0xe42c3a9dU, 0x0d507892U, 0x9b6a5fccU, 0x62547e46U, + 0xc2f68d13U, 0xe890d8b8U, 0x5e2e39f7U, 0xf582c3afU, + 0xbe9f5d80U, 0x7c69d093U, 0xa96fd52dU, 0xb3cf2512U, + 0x3bc8ac99U, 0xa710187dU, 0x6ee89c63U, 0x7bdb3bbbU, + 0x09cd2678U, 0xf46e5918U, 0x01ec9ab7U, 0xa8834f9aU, + 0x65e6956eU, 0x7eaaffe6U, 0x0821bccfU, 0xe6ef15e8U, + 0xd9bae79bU, 0xce4a6f36U, 0xd4ea9f09U, 0xd629b07cU, + 0xaf31a4b2U, 0x312a3f23U, 0x30c6a594U, 0xc035a266U, + 0x37744ebcU, 0xa6fc82caU, 0xb0e090d0U, 0x1533a7d8U, + 0x4af10498U, 0xf741ecdaU, 0x0e7fcd50U, 0x2f1791f6U, + 0x8d764dd6U, 0x4d43efb0U, 0x54ccaa4dU, 0xdfe49604U, + 0xe39ed1b5U, 0x1b4c6a88U, 0xb8c12c1fU, 0x7f466551U, + 0x049d5eeaU, 0x5d018c35U, 0x73fa8774U, 0x2efb0b41U, + 0x5ab3671dU, 0x5292dbd2U, 0x33e91056U, 0x136dd647U, + 0x8c9ad761U, 0x7a37a10cU, 0x8e59f814U, 0x89eb133cU, + 0xeecea927U, 0x35b761c9U, 0xede11ce5U, 0x3c7a47b1U, + 0x599cd2dfU, 0x3f55f273U, 0x791814ceU, 0xbf73c737U, + 0xea53f7cdU, 0x5b5ffdaaU, 0x14df3d6fU, 0x867844dbU, + 0x81caaff3U, 0x3eb968c4U, 0x2c382434U, 0x5fc2a340U, + 0x72161dc3U, 0x0cbce225U, 0x8b283c49U, 0x41ff0d95U, + 0x7139a801U, 0xde080cb3U, 0x9cd8b4e4U, 0x906456c1U, + 0x617bcb84U, 0x70d532b6U, 0x74486c5cU, 0x42d0b857U, +}; +const u32 Td2[256] = { + 0xa75051f4U, 0x65537e41U, 0xa4c31a17U, 0x5e963a27U, + 0x6bcb3babU, 0x45f11f9dU, 0x58abacfaU, 0x03934be3U, + 0xfa552030U, 0x6df6ad76U, 0x769188ccU, 0x4c25f502U, + 0xd7fc4fe5U, 0xcbd7c52aU, 0x44802635U, 0xa38fb562U, + 0x5a49deb1U, 0x1b6725baU, 0x0e9845eaU, 0xc0e15dfeU, + 0x7502c32fU, 0xf012814cU, 0x97a38d46U, 0xf9c66bd3U, + 0x5fe7038fU, 0x9c951592U, 0x7aebbf6dU, 0x59da9552U, + 0x832dd4beU, 0x21d35874U, 0x692949e0U, 0xc8448ec9U, + 0x896a75c2U, 0x7978f48eU, 0x3e6b9958U, 0x71dd27b9U, + 0x4fb6bee1U, 0xad17f088U, 0xac66c920U, 0x3ab47dceU, + 0x4a1863dfU, 0x3182e51aU, 0x33609751U, 0x7f456253U, + 0x77e0b164U, 0xae84bb6bU, 0xa01cfe81U, 0x2b94f908U, + 0x68587048U, 0xfd198f45U, 0x6c8794deU, 0xf8b7527bU, + 0xd323ab73U, 0x02e2724bU, 0x8f57e31fU, 0xab2a6655U, + 0x2807b2ebU, 0xc2032fb5U, 0x7b9a86c5U, 0x08a5d337U, + 0x87f23028U, 0xa5b223bfU, 0x6aba0203U, 0x825ced16U, + 0x1c2b8acfU, 0xb492a779U, 0xf2f0f307U, 0xe2a14e69U, + 0xf4cd65daU, 0xbed50605U, 0x621fd134U, 0xfe8ac4a6U, + 0x539d342eU, 0x55a0a2f3U, 0xe132058aU, 0xeb75a4f6U, + 0xec390b83U, 0xefaa4060U, 0x9f065e71U, 0x1051bd6eU, + + 0x8af93e21U, 0x063d96ddU, 0x05aedd3eU, 0xbd464de6U, + 0x8db59154U, 0x5d0571c4U, 0xd46f0406U, 0x15ff6050U, + 0xfb241998U, 0xe997d6bdU, 0x43cc8940U, 0x9e7767d9U, + 0x42bdb0e8U, 0x8b880789U, 0x5b38e719U, 0xeedb79c8U, + 0x0a47a17cU, 0x0fe97c42U, 0x1ec9f884U, 0x00000000U, + 0x86830980U, 0xed48322bU, 0x70ac1e11U, 0x724e6c5aU, + 0xfffbfd0eU, 0x38560f85U, 0xd51e3daeU, 0x3927362dU, + 0xd9640a0fU, 0xa621685cU, 0x54d19b5bU, 0x2e3a2436U, + 0x67b10c0aU, 0xe70f9357U, 0x96d2b4eeU, 0x919e1b9bU, + 0xc54f80c0U, 0x20a261dcU, 0x4b695a77U, 0x1a161c12U, + 0xba0ae293U, 0x2ae5c0a0U, 0xe0433c22U, 0x171d121bU, + 0x0d0b0e09U, 0xc7adf28bU, 0xa8b92db6U, 0xa9c8141eU, + 0x198557f1U, 0x074caf75U, 0xddbbee99U, 0x60fda37fU, + 0x269ff701U, 0xf5bc5c72U, 0x3bc54466U, 0x7e345bfbU, + 0x29768b43U, 0xc6dccb23U, 0xfc68b6edU, 0xf163b8e4U, + 0xdccad731U, 0x85104263U, 0x22401397U, 0x112084c6U, + 0x247d854aU, 0x3df8d2bbU, 0x3211aef9U, 0xa16dc729U, + 0x2f4b1d9eU, 0x30f3dcb2U, 0x52ec0d86U, 0xe3d077c1U, + 0x166c2bb3U, 0xb999a970U, 0x48fa1194U, 0x642247e9U, + 0x8cc4a8fcU, 0x3f1aa0f0U, 0x2cd8567dU, 0x90ef2233U, + 0x4ec78749U, 0xd1c1d938U, 0xa2fe8ccaU, 0x0b3698d4U, + 0x81cfa6f5U, 0xde28a57aU, 0x8e26dab7U, 0xbfa43fadU, + 0x9de42c3aU, 0x920d5078U, 0xcc9b6a5fU, 0x4662547eU, + 0x13c2f68dU, 0xb8e890d8U, 0xf75e2e39U, 0xaff582c3U, + 0x80be9f5dU, 0x937c69d0U, 0x2da96fd5U, 0x12b3cf25U, + 0x993bc8acU, 0x7da71018U, 0x636ee89cU, 0xbb7bdb3bU, + 0x7809cd26U, 0x18f46e59U, 0xb701ec9aU, 0x9aa8834fU, + 0x6e65e695U, 0xe67eaaffU, 0xcf0821bcU, 0xe8e6ef15U, + 0x9bd9bae7U, 0x36ce4a6fU, 0x09d4ea9fU, 0x7cd629b0U, + 0xb2af31a4U, 0x23312a3fU, 0x9430c6a5U, 0x66c035a2U, + 0xbc37744eU, 0xcaa6fc82U, 0xd0b0e090U, 0xd81533a7U, + 0x984af104U, 0xdaf741ecU, 0x500e7fcdU, 0xf62f1791U, + 0xd68d764dU, 0xb04d43efU, 0x4d54ccaaU, 0x04dfe496U, + 0xb5e39ed1U, 0x881b4c6aU, 0x1fb8c12cU, 0x517f4665U, + 0xea049d5eU, 0x355d018cU, 0x7473fa87U, 0x412efb0bU, + 0x1d5ab367U, 0xd25292dbU, 0x5633e910U, 0x47136dd6U, + 0x618c9ad7U, 0x0c7a37a1U, 0x148e59f8U, 0x3c89eb13U, + 0x27eecea9U, 0xc935b761U, 0xe5ede11cU, 0xb13c7a47U, + 0xdf599cd2U, 0x733f55f2U, 0xce791814U, 0x37bf73c7U, + 0xcdea53f7U, 0xaa5b5ffdU, 0x6f14df3dU, 0xdb867844U, + 0xf381caafU, 0xc43eb968U, 0x342c3824U, 0x405fc2a3U, + 0xc372161dU, 0x250cbce2U, 0x498b283cU, 0x9541ff0dU, + 0x017139a8U, 0xb3de080cU, 0xe49cd8b4U, 0xc1906456U, + 0x84617bcbU, 0xb670d532U, 0x5c74486cU, 0x5742d0b8U, +}; +const u32 Td3[256] = { + 0xf4a75051U, 0x4165537eU, 0x17a4c31aU, 0x275e963aU, + 0xab6bcb3bU, 0x9d45f11fU, 0xfa58abacU, 0xe303934bU, + 0x30fa5520U, 0x766df6adU, 0xcc769188U, 0x024c25f5U, + 0xe5d7fc4fU, 0x2acbd7c5U, 0x35448026U, 0x62a38fb5U, + 0xb15a49deU, 0xba1b6725U, 0xea0e9845U, 0xfec0e15dU, + 0x2f7502c3U, 0x4cf01281U, 0x4697a38dU, 0xd3f9c66bU, + 0x8f5fe703U, 0x929c9515U, 0x6d7aebbfU, 0x5259da95U, + 0xbe832dd4U, 0x7421d358U, 0xe0692949U, 0xc9c8448eU, + 0xc2896a75U, 0x8e7978f4U, 0x583e6b99U, 0xb971dd27U, + 0xe14fb6beU, 0x88ad17f0U, 0x20ac66c9U, 0xce3ab47dU, + 0xdf4a1863U, 0x1a3182e5U, 0x51336097U, 0x537f4562U, + 0x6477e0b1U, 0x6bae84bbU, 0x81a01cfeU, 0x082b94f9U, + 0x48685870U, 0x45fd198fU, 0xde6c8794U, 0x7bf8b752U, + 0x73d323abU, 0x4b02e272U, 0x1f8f57e3U, 0x55ab2a66U, + 0xeb2807b2U, 0xb5c2032fU, 0xc57b9a86U, 0x3708a5d3U, + 0x2887f230U, 0xbfa5b223U, 0x036aba02U, 0x16825cedU, + 0xcf1c2b8aU, 0x79b492a7U, 0x07f2f0f3U, 0x69e2a14eU, + 0xdaf4cd65U, 0x05bed506U, 0x34621fd1U, 0xa6fe8ac4U, + 0x2e539d34U, 0xf355a0a2U, 0x8ae13205U, 0xf6eb75a4U, + 0x83ec390bU, 0x60efaa40U, 0x719f065eU, 0x6e1051bdU, + 0x218af93eU, 0xdd063d96U, 0x3e05aeddU, 0xe6bd464dU, + 0x548db591U, 0xc45d0571U, 0x06d46f04U, 0x5015ff60U, + 0x98fb2419U, 0xbde997d6U, 0x4043cc89U, 0xd99e7767U, + 0xe842bdb0U, 0x898b8807U, 0x195b38e7U, 0xc8eedb79U, + 0x7c0a47a1U, 0x420fe97cU, 0x841ec9f8U, 0x00000000U, + 0x80868309U, 0x2bed4832U, 0x1170ac1eU, 0x5a724e6cU, + 0x0efffbfdU, 0x8538560fU, 0xaed51e3dU, 0x2d392736U, + 0x0fd9640aU, 0x5ca62168U, 0x5b54d19bU, 0x362e3a24U, + 0x0a67b10cU, 0x57e70f93U, 0xee96d2b4U, 0x9b919e1bU, + 0xc0c54f80U, 0xdc20a261U, 0x774b695aU, 0x121a161cU, + 0x93ba0ae2U, 0xa02ae5c0U, 0x22e0433cU, 0x1b171d12U, + 0x090d0b0eU, 0x8bc7adf2U, 0xb6a8b92dU, 0x1ea9c814U, + 0xf1198557U, 0x75074cafU, 0x99ddbbeeU, 0x7f60fda3U, + 0x01269ff7U, 0x72f5bc5cU, 0x663bc544U, 0xfb7e345bU, + 0x4329768bU, 0x23c6dccbU, 0xedfc68b6U, 0xe4f163b8U, + 0x31dccad7U, 0x63851042U, 0x97224013U, 0xc6112084U, + 0x4a247d85U, 0xbb3df8d2U, 0xf93211aeU, 0x29a16dc7U, + 0x9e2f4b1dU, 0xb230f3dcU, 0x8652ec0dU, 0xc1e3d077U, + 0xb3166c2bU, 0x70b999a9U, 0x9448fa11U, 0xe9642247U, + 0xfc8cc4a8U, 0xf03f1aa0U, 0x7d2cd856U, 0x3390ef22U, + 0x494ec787U, 0x38d1c1d9U, 0xcaa2fe8cU, 0xd40b3698U, + 0xf581cfa6U, 0x7ade28a5U, 0xb78e26daU, 0xadbfa43fU, + 0x3a9de42cU, 0x78920d50U, 0x5fcc9b6aU, 0x7e466254U, + 0x8d13c2f6U, 0xd8b8e890U, 0x39f75e2eU, 0xc3aff582U, + 0x5d80be9fU, 0xd0937c69U, 0xd52da96fU, 0x2512b3cfU, + 0xac993bc8U, 0x187da710U, 0x9c636ee8U, 0x3bbb7bdbU, + 0x267809cdU, 0x5918f46eU, 0x9ab701ecU, 0x4f9aa883U, + 0x956e65e6U, 0xffe67eaaU, 0xbccf0821U, 0x15e8e6efU, + 0xe79bd9baU, 0x6f36ce4aU, 0x9f09d4eaU, 0xb07cd629U, + 0xa4b2af31U, 0x3f23312aU, 0xa59430c6U, 0xa266c035U, + 0x4ebc3774U, 0x82caa6fcU, 0x90d0b0e0U, 0xa7d81533U, + 0x04984af1U, 0xecdaf741U, 0xcd500e7fU, 0x91f62f17U, + 0x4dd68d76U, 0xefb04d43U, 0xaa4d54ccU, 0x9604dfe4U, + 0xd1b5e39eU, 0x6a881b4cU, 0x2c1fb8c1U, 0x65517f46U, + 0x5eea049dU, 0x8c355d01U, 0x877473faU, 0x0b412efbU, + 0x671d5ab3U, 0xdbd25292U, 0x105633e9U, 0xd647136dU, + 0xd7618c9aU, 0xa10c7a37U, 0xf8148e59U, 0x133c89ebU, + 0xa927eeceU, 0x61c935b7U, 0x1ce5ede1U, 0x47b13c7aU, + 0xd2df599cU, 0xf2733f55U, 0x14ce7918U, 0xc737bf73U, + 0xf7cdea53U, 0xfdaa5b5fU, 0x3d6f14dfU, 0x44db8678U, + 0xaff381caU, 0x68c43eb9U, 0x24342c38U, 0xa3405fc2U, + 0x1dc37216U, 0xe2250cbcU, 0x3c498b28U, 0x0d9541ffU, + 0xa8017139U, 0x0cb3de08U, 0xb4e49cd8U, 0x56c19064U, + 0xcb84617bU, 0x32b670d5U, 0x6c5c7448U, 0xb85742d0U, +}; +const u32 Td4[256] = { + 0x52525252U, 0x09090909U, 0x6a6a6a6aU, 0xd5d5d5d5U, + 0x30303030U, 0x36363636U, 0xa5a5a5a5U, 0x38383838U, + 0xbfbfbfbfU, 0x40404040U, 0xa3a3a3a3U, 0x9e9e9e9eU, + 0x81818181U, 0xf3f3f3f3U, 0xd7d7d7d7U, 0xfbfbfbfbU, + 0x7c7c7c7cU, 0xe3e3e3e3U, 0x39393939U, 0x82828282U, + 0x9b9b9b9bU, 0x2f2f2f2fU, 0xffffffffU, 0x87878787U, + 0x34343434U, 0x8e8e8e8eU, 0x43434343U, 0x44444444U, + 0xc4c4c4c4U, 0xdedededeU, 0xe9e9e9e9U, 0xcbcbcbcbU, + 0x54545454U, 0x7b7b7b7bU, 0x94949494U, 0x32323232U, + 0xa6a6a6a6U, 0xc2c2c2c2U, 0x23232323U, 0x3d3d3d3dU, + 0xeeeeeeeeU, 0x4c4c4c4cU, 0x95959595U, 0x0b0b0b0bU, + 0x42424242U, 0xfafafafaU, 0xc3c3c3c3U, 0x4e4e4e4eU, + 0x08080808U, 0x2e2e2e2eU, 0xa1a1a1a1U, 0x66666666U, + 0x28282828U, 0xd9d9d9d9U, 0x24242424U, 0xb2b2b2b2U, + 0x76767676U, 0x5b5b5b5bU, 0xa2a2a2a2U, 0x49494949U, + 0x6d6d6d6dU, 0x8b8b8b8bU, 0xd1d1d1d1U, 0x25252525U, + 0x72727272U, 0xf8f8f8f8U, 0xf6f6f6f6U, 0x64646464U, + 0x86868686U, 0x68686868U, 0x98989898U, 0x16161616U, + 0xd4d4d4d4U, 0xa4a4a4a4U, 0x5c5c5c5cU, 0xccccccccU, + 0x5d5d5d5dU, 0x65656565U, 0xb6b6b6b6U, 0x92929292U, + 0x6c6c6c6cU, 0x70707070U, 0x48484848U, 0x50505050U, + 0xfdfdfdfdU, 0xededededU, 0xb9b9b9b9U, 0xdadadadaU, + 0x5e5e5e5eU, 0x15151515U, 0x46464646U, 0x57575757U, + 0xa7a7a7a7U, 0x8d8d8d8dU, 0x9d9d9d9dU, 0x84848484U, + 0x90909090U, 0xd8d8d8d8U, 0xababababU, 0x00000000U, + 0x8c8c8c8cU, 0xbcbcbcbcU, 0xd3d3d3d3U, 0x0a0a0a0aU, + 0xf7f7f7f7U, 0xe4e4e4e4U, 0x58585858U, 0x05050505U, + 0xb8b8b8b8U, 0xb3b3b3b3U, 0x45454545U, 0x06060606U, + 0xd0d0d0d0U, 0x2c2c2c2cU, 0x1e1e1e1eU, 0x8f8f8f8fU, + 0xcacacacaU, 0x3f3f3f3fU, 0x0f0f0f0fU, 0x02020202U, + 0xc1c1c1c1U, 0xafafafafU, 0xbdbdbdbdU, 0x03030303U, + 0x01010101U, 0x13131313U, 0x8a8a8a8aU, 0x6b6b6b6bU, + 0x3a3a3a3aU, 0x91919191U, 0x11111111U, 0x41414141U, + 0x4f4f4f4fU, 0x67676767U, 0xdcdcdcdcU, 0xeaeaeaeaU, + 0x97979797U, 0xf2f2f2f2U, 0xcfcfcfcfU, 0xcecececeU, + 0xf0f0f0f0U, 0xb4b4b4b4U, 0xe6e6e6e6U, 0x73737373U, + 0x96969696U, 0xacacacacU, 0x74747474U, 0x22222222U, + 0xe7e7e7e7U, 0xadadadadU, 0x35353535U, 0x85858585U, + 0xe2e2e2e2U, 0xf9f9f9f9U, 0x37373737U, 0xe8e8e8e8U, + 0x1c1c1c1cU, 0x75757575U, 0xdfdfdfdfU, 0x6e6e6e6eU, + 0x47474747U, 0xf1f1f1f1U, 0x1a1a1a1aU, 0x71717171U, + 0x1d1d1d1dU, 0x29292929U, 0xc5c5c5c5U, 0x89898989U, + 0x6f6f6f6fU, 0xb7b7b7b7U, 0x62626262U, 0x0e0e0e0eU, + 0xaaaaaaaaU, 0x18181818U, 0xbebebebeU, 0x1b1b1b1bU, + 0xfcfcfcfcU, 0x56565656U, 0x3e3e3e3eU, 0x4b4b4b4bU, + 0xc6c6c6c6U, 0xd2d2d2d2U, 0x79797979U, 0x20202020U, + 0x9a9a9a9aU, 0xdbdbdbdbU, 0xc0c0c0c0U, 0xfefefefeU, + 0x78787878U, 0xcdcdcdcdU, 0x5a5a5a5aU, 0xf4f4f4f4U, + 0x1f1f1f1fU, 0xddddddddU, 0xa8a8a8a8U, 0x33333333U, + 0x88888888U, 0x07070707U, 0xc7c7c7c7U, 0x31313131U, + 0xb1b1b1b1U, 0x12121212U, 0x10101010U, 0x59595959U, + 0x27272727U, 0x80808080U, 0xececececU, 0x5f5f5f5fU, + 0x60606060U, 0x51515151U, 0x7f7f7f7fU, 0xa9a9a9a9U, + 0x19191919U, 0xb5b5b5b5U, 0x4a4a4a4aU, 0x0d0d0d0dU, + 0x2d2d2d2dU, 0xe5e5e5e5U, 0x7a7a7a7aU, 0x9f9f9f9fU, + 0x93939393U, 0xc9c9c9c9U, 0x9c9c9c9cU, 0xefefefefU, + 0xa0a0a0a0U, 0xe0e0e0e0U, 0x3b3b3b3bU, 0x4d4d4d4dU, + 0xaeaeaeaeU, 0x2a2a2a2aU, 0xf5f5f5f5U, 0xb0b0b0b0U, + 0xc8c8c8c8U, 0xebebebebU, 0xbbbbbbbbU, 0x3c3c3c3cU, + 0x83838383U, 0x53535353U, 0x99999999U, 0x61616161U, + 0x17171717U, 0x2b2b2b2bU, 0x04040404U, 0x7e7e7e7eU, + 0xbabababaU, 0x77777777U, 0xd6d6d6d6U, 0x26262626U, + 0xe1e1e1e1U, 0x69696969U, 0x14141414U, 0x63636363U, + 0x55555555U, 0x21212121U, 0x0c0c0c0cU, 0x7d7d7d7dU, +}; +const u32 rcon[] = { + 0x01000000, 0x02000000, 0x04000000, 0x08000000, + 0x10000000, 0x20000000, 0x40000000, 0x80000000, + 0x1B000000, 0x36000000, /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */ +}; +#else /* AES_SMALL_TABLES */ +const u8 Td4s[256] = { + 0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U, + 0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU, + 0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U, + 0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU, + 0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU, + 0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU, + 0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U, + 0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U, + 0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U, + 0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U, + 0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU, + 0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U, + 0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU, + 0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U, + 0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U, + 0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU, + 0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU, + 0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U, + 0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U, + 0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU, + 0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U, + 0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU, + 0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U, + 0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U, + 0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U, + 0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU, + 0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU, + 0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU, + 0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U, + 0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U, + 0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U, + 0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU, +}; +const u8 rcons[] = { + 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36 + /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */ +}; +#endif /* AES_SMALL_TABLES */ +/** + * Expand the cipher key into the encryption key schedule. + * + * @return the number of rounds for the given cipher key size. + */ +int rijndaelKeySetupEnc(u32 rk[], const u8 cipherKey[], int keyBits) +{ + int i; + u32 temp; + + rk[0] = GETU32(cipherKey ); + rk[1] = GETU32(cipherKey + 4); + rk[2] = GETU32(cipherKey + 8); + rk[3] = GETU32(cipherKey + 12); + + if (keyBits == 128) { + for (i = 0; i < 10; i++) { + temp = rk[3]; + rk[4] = rk[0] ^ TE421(temp) ^ TE432(temp) ^ + TE443(temp) ^ TE414(temp) ^ RCON(i); + rk[5] = rk[1] ^ rk[4]; + rk[6] = rk[2] ^ rk[5]; + rk[7] = rk[3] ^ rk[6]; + rk += 4; + } + return 10; + } + + rk[4] = GETU32(cipherKey + 16); + rk[5] = GETU32(cipherKey + 20); + + if (keyBits == 192) { + for (i = 0; i < 8; i++) { + temp = rk[5]; + rk[6] = rk[0] ^ TE421(temp) ^ TE432(temp) ^ + TE443(temp) ^ TE414(temp) ^ RCON(i); + rk[7] = rk[1] ^ rk[6]; + rk[8] = rk[2] ^ rk[7]; + rk[9] = rk[3] ^ rk[8]; + if (i == 7) + return 12; + rk[10] = rk[4] ^ rk[9]; + rk[11] = rk[5] ^ rk[10]; + rk += 6; + } + } + + rk[6] = GETU32(cipherKey + 24); + rk[7] = GETU32(cipherKey + 28); + + if (keyBits == 256) { + for (i = 0; i < 7; i++) { + temp = rk[7]; + rk[8] = rk[0] ^ TE421(temp) ^ TE432(temp) ^ + TE443(temp) ^ TE414(temp) ^ RCON(i); + rk[9] = rk[1] ^ rk[8]; + rk[10] = rk[2] ^ rk[9]; + rk[11] = rk[3] ^ rk[10]; + if (i == 6) + return 14; + temp = rk[11]; + rk[12] = rk[4] ^ TE411(temp) ^ TE422(temp) ^ + TE433(temp) ^ TE444(temp); + rk[13] = rk[5] ^ rk[12]; + rk[14] = rk[6] ^ rk[13]; + rk[15] = rk[7] ^ rk[14]; + rk += 8; + } + } + + return -1; +} diff --git a/core/crypto/aes-omac1.c b/core/crypto/aes-omac1.c new file mode 100644 index 0000000..30b5262 --- /dev/null +++ b/core/crypto/aes-omac1.c @@ -0,0 +1,172 @@ +/* + * One-key CBC MAC (OMAC1) hash with AES + * + * Copyright (c) 2003-2007, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" + +static void gf_mulx(u8 *pad) +{ + int i, carry; + + carry = pad[0] & 0x80; + for (i = 0; i < AES_BLOCK_SIZE - 1; i++) + pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7); + pad[AES_BLOCK_SIZE - 1] <<= 1; + if (carry) + pad[AES_BLOCK_SIZE - 1] ^= 0x87; +} + + +/** + * omac1_aes_vector - One-Key CBC MAC (OMAC1) hash with AES + * @key: Key for the hash operation + * @key_len: Key length in octets + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for MAC (128 bits, i.e., 16 bytes) + * Returns: 0 on success, -1 on failure + * + * This is a mode for using block cipher (AES in this case) for authentication. + * OMAC1 was standardized with the name CMAC by NIST in a Special Publication + * (SP) 800-38B. + */ +int omac1_aes_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac) +{ + void *ctx; + u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE]; + const u8 *pos, *end; + size_t i, e, left, total_len; + + if (TEST_FAIL()) + return -1; + + ctx = aes_encrypt_init(key, key_len); + if (ctx == NULL) + return -1; + os_memset(cbc, 0, AES_BLOCK_SIZE); + + total_len = 0; + for (e = 0; e < num_elem; e++) + total_len += len[e]; + left = total_len; + + e = 0; + pos = addr[0]; + end = pos + len[0]; + + while (left >= AES_BLOCK_SIZE) { + for (i = 0; i < AES_BLOCK_SIZE; i++) { + cbc[i] ^= *pos++; + if (pos >= end) { + /* + * Stop if there are no more bytes to process + * since there are no more entries in the array. + */ + if (i + 1 == AES_BLOCK_SIZE && + left == AES_BLOCK_SIZE) + break; + e++; + pos = addr[e]; + end = pos + len[e]; + } + } + if (left > AES_BLOCK_SIZE) + aes_encrypt(ctx, cbc, cbc); + left -= AES_BLOCK_SIZE; + } + + os_memset(pad, 0, AES_BLOCK_SIZE); + aes_encrypt(ctx, pad, pad); + gf_mulx(pad); + + if (left || total_len == 0) { + for (i = 0; i < left; i++) { + cbc[i] ^= *pos++; + if (pos >= end) { + /* + * Stop if there are no more bytes to process + * since there are no more entries in the array. + */ + if (i + 1 == left) + break; + e++; + pos = addr[e]; + end = pos + len[e]; + } + } + cbc[left] ^= 0x80; + gf_mulx(pad); + } + + for (i = 0; i < AES_BLOCK_SIZE; i++) + pad[i] ^= cbc[i]; + aes_encrypt(ctx, pad, mac); + aes_encrypt_deinit(ctx); + return 0; +} + + +/** + * omac1_aes_128_vector - One-Key CBC MAC (OMAC1) hash with AES-128 + * @key: 128-bit key for the hash operation + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for MAC (128 bits, i.e., 16 bytes) + * Returns: 0 on success, -1 on failure + * + * This is a mode for using block cipher (AES in this case) for authentication. + * OMAC1 was standardized with the name CMAC by NIST in a Special Publication + * (SP) 800-38B. + */ +int omac1_aes_128_vector(const u8 *key, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac) +{ + return omac1_aes_vector(key, 16, num_elem, addr, len, mac); +} + + +/** + * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC) + * @key: 128-bit key for the hash operation + * @data: Data buffer for which a MAC is determined + * @data_len: Length of data buffer in bytes + * @mac: Buffer for MAC (128 bits, i.e., 16 bytes) + * Returns: 0 on success, -1 on failure + * + * This is a mode for using block cipher (AES in this case) for authentication. + * OMAC1 was standardized with the name CMAC by NIST in a Special Publication + * (SP) 800-38B. + */ +int omac1_aes_128(const u8 *key, const u8 *data, size_t data_len, u8 *mac) +{ + return omac1_aes_128_vector(key, 1, &data, &data_len, mac); +} + + +/** + * omac1_aes_256 - One-Key CBC MAC (OMAC1) hash with AES-256 (aka AES-CMAC) + * @key: 256-bit key for the hash operation + * @data: Data buffer for which a MAC is determined + * @data_len: Length of data buffer in bytes + * @mac: Buffer for MAC (128 bits, i.e., 16 bytes) + * Returns: 0 on success, -1 on failure + * + * This is a mode for using block cipher (AES in this case) for authentication. + * OMAC1 was standardized with the name CMAC by NIST in a Special Publication + * (SP) 800-38B. + */ +int omac1_aes_256(const u8 *key, const u8 *data, size_t data_len, u8 *mac) +{ + return omac1_aes_vector(key, 32, 1, &data, &data_len, mac); +} diff --git a/core/crypto/aes-siv.c b/core/crypto/aes-siv.c new file mode 100644 index 0000000..58656f2 --- /dev/null +++ b/core/crypto/aes-siv.c @@ -0,0 +1,207 @@ +/* + * AES SIV (RFC 5297) + * Copyright (c) 2013 Cozybit, Inc. + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" +#include "aes_siv.h" + + +static const u8 zero[AES_BLOCK_SIZE]; + + +static void dbl(u8 *pad) +{ + int i, carry; + + carry = pad[0] & 0x80; + for (i = 0; i < AES_BLOCK_SIZE - 1; i++) + pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7); + pad[AES_BLOCK_SIZE - 1] <<= 1; + if (carry) + pad[AES_BLOCK_SIZE - 1] ^= 0x87; +} + + +static void xor(u8 *a, const u8 *b) +{ + int i; + + for (i = 0; i < AES_BLOCK_SIZE; i++) + *a++ ^= *b++; +} + + +static void xorend(u8 *a, int alen, const u8 *b, int blen) +{ + int i; + + if (alen < blen) + return; + + for (i = 0; i < blen; i++) + a[alen - blen + i] ^= b[i]; +} + + +static void pad_block(u8 *pad, const u8 *addr, size_t len) +{ + os_memset(pad, 0, AES_BLOCK_SIZE); + os_memcpy(pad, addr, len); + + if (len < AES_BLOCK_SIZE) + pad[len] = 0x80; +} + + +static int aes_s2v(const u8 *key, size_t key_len, + size_t num_elem, const u8 *addr[], size_t *len, u8 *mac) +{ + u8 tmp[AES_BLOCK_SIZE], tmp2[AES_BLOCK_SIZE]; + u8 *buf = NULL; + int ret; + size_t i; + const u8 *data[1]; + size_t data_len[1]; + + if (!num_elem) { + os_memcpy(tmp, zero, sizeof(zero)); + tmp[AES_BLOCK_SIZE - 1] = 1; + data[0] = tmp; + data_len[0] = sizeof(tmp); + return omac1_aes_vector(key, key_len, 1, data, data_len, mac); + } + + data[0] = zero; + data_len[0] = sizeof(zero); + ret = omac1_aes_vector(key, key_len, 1, data, data_len, tmp); + if (ret) + return ret; + + for (i = 0; i < num_elem - 1; i++) { + ret = omac1_aes_vector(key, key_len, 1, &addr[i], &len[i], + tmp2); + if (ret) + return ret; + + dbl(tmp); + xor(tmp, tmp2); + } + if (len[i] >= AES_BLOCK_SIZE) { + buf = os_memdup(addr[i], len[i]); + if (!buf) + return -ENOMEM; + + xorend(buf, len[i], tmp, AES_BLOCK_SIZE); + data[0] = buf; + ret = omac1_aes_vector(key, key_len, 1, data, &len[i], mac); + bin_clear_free(buf, len[i]); + return ret; + } + + dbl(tmp); + pad_block(tmp2, addr[i], len[i]); + xor(tmp, tmp2); + + data[0] = tmp; + data_len[0] = sizeof(tmp); + return omac1_aes_vector(key, key_len, 1, data, data_len, mac); +} + + +int aes_siv_encrypt(const u8 *key, size_t key_len, + const u8 *pw, size_t pwlen, + size_t num_elem, const u8 *addr[], const size_t *len, + u8 *out) +{ + const u8 *_addr[6]; + size_t _len[6]; + const u8 *k1, *k2; + u8 v[AES_BLOCK_SIZE]; + size_t i; + u8 *iv, *crypt_pw; + + if (num_elem > ARRAY_SIZE(_addr) - 1 || + (key_len != 32 && key_len != 48 && key_len != 64)) + return -1; + + key_len /= 2; + k1 = key; + k2 = key + key_len; + + for (i = 0; i < num_elem; i++) { + _addr[i] = addr[i]; + _len[i] = len[i]; + } + _addr[num_elem] = pw; + _len[num_elem] = pwlen; + + if (aes_s2v(k1, key_len, num_elem + 1, _addr, _len, v)) + return -1; + + iv = out; + crypt_pw = out + AES_BLOCK_SIZE; + + os_memcpy(iv, v, AES_BLOCK_SIZE); + os_memcpy(crypt_pw, pw, pwlen); + + /* zero out 63rd and 31st bits of ctr (from right) */ + v[8] &= 0x7f; + v[12] &= 0x7f; + return aes_ctr_encrypt(k2, key_len, v, crypt_pw, pwlen); +} + + +int aes_siv_decrypt(const u8 *key, size_t key_len, + const u8 *iv_crypt, size_t iv_c_len, + size_t num_elem, const u8 *addr[], const size_t *len, + u8 *out) +{ + const u8 *_addr[6]; + size_t _len[6]; + const u8 *k1, *k2; + size_t crypt_len; + size_t i; + int ret; + u8 iv[AES_BLOCK_SIZE]; + u8 check[AES_BLOCK_SIZE]; + + if (iv_c_len < AES_BLOCK_SIZE || num_elem > ARRAY_SIZE(_addr) - 1 || + (key_len != 32 && key_len != 48 && key_len != 64)) + return -1; + crypt_len = iv_c_len - AES_BLOCK_SIZE; + key_len /= 2; + k1 = key; + k2 = key + key_len; + + for (i = 0; i < num_elem; i++) { + _addr[i] = addr[i]; + _len[i] = len[i]; + } + _addr[num_elem] = out; + _len[num_elem] = crypt_len; + + os_memcpy(iv, iv_crypt, AES_BLOCK_SIZE); + os_memcpy(out, iv_crypt + AES_BLOCK_SIZE, crypt_len); + + iv[8] &= 0x7f; + iv[12] &= 0x7f; + + ret = aes_ctr_encrypt(k2, key_len, iv, out, crypt_len); + if (ret) + return ret; + + ret = aes_s2v(k1, key_len, num_elem + 1, _addr, _len, check); + if (ret) + return ret; + if (os_memcmp(check, iv_crypt, AES_BLOCK_SIZE) == 0) + return 0; + + return -1; +} diff --git a/core/crypto/aes.h b/core/crypto/aes.h new file mode 100644 index 0000000..8ab3de2 --- /dev/null +++ b/core/crypto/aes.h @@ -0,0 +1,21 @@ +/* + * AES functions + * Copyright (c) 2003-2006, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef AES_H +#define AES_H + +#define AES_BLOCK_SIZE 16 + +void * aes_encrypt_init(const u8 *key, size_t len); +int aes_encrypt(void *ctx, const u8 *plain, u8 *crypt); +void aes_encrypt_deinit(void *ctx); +void * aes_decrypt_init(const u8 *key, size_t len); +int aes_decrypt(void *ctx, const u8 *crypt, u8 *plain); +void aes_decrypt_deinit(void *ctx); + +#endif /* AES_H */ diff --git a/core/crypto/aes_i.h b/core/crypto/aes_i.h new file mode 100644 index 0000000..54375cf --- /dev/null +++ b/core/crypto/aes_i.h @@ -0,0 +1,125 @@ +/* + * AES (Rijndael) cipher + * Copyright (c) 2003-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef AES_I_H +#define AES_I_H + +#include "aes.h" + +/* #define FULL_UNROLL */ +#define AES_SMALL_TABLES + +extern const u32 Te0[256]; +extern const u32 Te1[256]; +extern const u32 Te2[256]; +extern const u32 Te3[256]; +extern const u32 Te4[256]; +extern const u32 Td0[256]; +extern const u32 Td1[256]; +extern const u32 Td2[256]; +extern const u32 Td3[256]; +extern const u32 Td4[256]; +extern const u32 rcon[10]; +extern const u8 Td4s[256]; +extern const u8 rcons[10]; + +#ifndef AES_SMALL_TABLES + +#define RCON(i) rcon[(i)] + +#define TE0(i) Te0[((i) >> 24) & 0xff] +#define TE1(i) Te1[((i) >> 16) & 0xff] +#define TE2(i) Te2[((i) >> 8) & 0xff] +#define TE3(i) Te3[(i) & 0xff] +#define TE41(i) (Te4[((i) >> 24) & 0xff] & 0xff000000) +#define TE42(i) (Te4[((i) >> 16) & 0xff] & 0x00ff0000) +#define TE43(i) (Te4[((i) >> 8) & 0xff] & 0x0000ff00) +#define TE44(i) (Te4[(i) & 0xff] & 0x000000ff) +#define TE421(i) (Te4[((i) >> 16) & 0xff] & 0xff000000) +#define TE432(i) (Te4[((i) >> 8) & 0xff] & 0x00ff0000) +#define TE443(i) (Te4[(i) & 0xff] & 0x0000ff00) +#define TE414(i) (Te4[((i) >> 24) & 0xff] & 0x000000ff) +#define TE411(i) (Te4[((i) >> 24) & 0xff] & 0xff000000) +#define TE422(i) (Te4[((i) >> 16) & 0xff] & 0x00ff0000) +#define TE433(i) (Te4[((i) >> 8) & 0xff] & 0x0000ff00) +#define TE444(i) (Te4[(i) & 0xff] & 0x000000ff) +#define TE4(i) (Te4[(i)] & 0x000000ff) + +#define TD0(i) Td0[((i) >> 24) & 0xff] +#define TD1(i) Td1[((i) >> 16) & 0xff] +#define TD2(i) Td2[((i) >> 8) & 0xff] +#define TD3(i) Td3[(i) & 0xff] +#define TD41(i) (Td4[((i) >> 24) & 0xff] & 0xff000000) +#define TD42(i) (Td4[((i) >> 16) & 0xff] & 0x00ff0000) +#define TD43(i) (Td4[((i) >> 8) & 0xff] & 0x0000ff00) +#define TD44(i) (Td4[(i) & 0xff] & 0x000000ff) +#define TD0_(i) Td0[(i) & 0xff] +#define TD1_(i) Td1[(i) & 0xff] +#define TD2_(i) Td2[(i) & 0xff] +#define TD3_(i) Td3[(i) & 0xff] + +#else /* AES_SMALL_TABLES */ + +#define RCON(i) (rcons[(i)] << 24) + +static inline u32 rotr(u32 val, int bits) +{ + return (val >> bits) | (val << (32 - bits)); +} + +#define TE0(i) Te0[((i) >> 24) & 0xff] +#define TE1(i) rotr(Te0[((i) >> 16) & 0xff], 8) +#define TE2(i) rotr(Te0[((i) >> 8) & 0xff], 16) +#define TE3(i) rotr(Te0[(i) & 0xff], 24) +#define TE41(i) ((Te0[((i) >> 24) & 0xff] << 8) & 0xff000000) +#define TE42(i) (Te0[((i) >> 16) & 0xff] & 0x00ff0000) +#define TE43(i) (Te0[((i) >> 8) & 0xff] & 0x0000ff00) +#define TE44(i) ((Te0[(i) & 0xff] >> 8) & 0x000000ff) +#define TE421(i) ((Te0[((i) >> 16) & 0xff] << 8) & 0xff000000) +#define TE432(i) (Te0[((i) >> 8) & 0xff] & 0x00ff0000) +#define TE443(i) (Te0[(i) & 0xff] & 0x0000ff00) +#define TE414(i) ((Te0[((i) >> 24) & 0xff] >> 8) & 0x000000ff) +#define TE411(i) ((Te0[((i) >> 24) & 0xff] << 8) & 0xff000000) +#define TE422(i) (Te0[((i) >> 16) & 0xff] & 0x00ff0000) +#define TE433(i) (Te0[((i) >> 8) & 0xff] & 0x0000ff00) +#define TE444(i) ((Te0[(i) & 0xff] >> 8) & 0x000000ff) +#define TE4(i) ((Te0[(i)] >> 8) & 0x000000ff) + +#define TD0(i) Td0[((i) >> 24) & 0xff] +#define TD1(i) rotr(Td0[((i) >> 16) & 0xff], 8) +#define TD2(i) rotr(Td0[((i) >> 8) & 0xff], 16) +#define TD3(i) rotr(Td0[(i) & 0xff], 24) +#define TD41(i) (Td4s[((i) >> 24) & 0xff] << 24) +#define TD42(i) (Td4s[((i) >> 16) & 0xff] << 16) +#define TD43(i) (Td4s[((i) >> 8) & 0xff] << 8) +#define TD44(i) (Td4s[(i) & 0xff]) +#define TD0_(i) Td0[(i) & 0xff] +#define TD1_(i) rotr(Td0[(i) & 0xff], 8) +#define TD2_(i) rotr(Td0[(i) & 0xff], 16) +#define TD3_(i) rotr(Td0[(i) & 0xff], 24) + +#endif /* AES_SMALL_TABLES */ + +#ifdef _MSC_VER +#define SWAP(x) (_lrotl(x, 8) & 0x00ff00ff | _lrotr(x, 8) & 0xff00ff00) +#define GETU32(p) SWAP(*((u32 *)(p))) +#define PUTU32(ct, st) { *((u32 *)(ct)) = SWAP((st)); } +#else +#define GETU32(pt) (((u32)(pt)[0] << 24) ^ ((u32)(pt)[1] << 16) ^ \ +((u32)(pt)[2] << 8) ^ ((u32)(pt)[3])) +#define PUTU32(ct, st) { \ +(ct)[0] = (u8)((st) >> 24); (ct)[1] = (u8)((st) >> 16); \ +(ct)[2] = (u8)((st) >> 8); (ct)[3] = (u8)(st); } +#endif + +#define AES_PRIV_SIZE (4 * 4 * 15 + 4) +#define AES_PRIV_NR_POS (4 * 15) + +int rijndaelKeySetupEnc(u32 rk[], const u8 cipherKey[], int keyBits); + +#endif /* AES_I_H */ diff --git a/core/crypto/aes_siv.h b/core/crypto/aes_siv.h new file mode 100644 index 0000000..fb05d80 --- /dev/null +++ b/core/crypto/aes_siv.h @@ -0,0 +1,21 @@ +/* + * AES SIV (RFC 5297) + * Copyright (c) 2013 Cozybit, Inc. + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef AES_SIV_H +#define AES_SIV_H + +int aes_siv_encrypt(const u8 *key, size_t key_len, + const u8 *pw, size_t pwlen, + size_t num_elem, const u8 *addr[], const size_t *len, + u8 *out); +int aes_siv_decrypt(const u8 *key, size_t key_len, + const u8 *iv_crypt, size_t iv_c_len, + size_t num_elem, const u8 *addr[], const size_t *len, + u8 *out); + +#endif /* AES_SIV_H */ diff --git a/core/crypto/aes_wrap.h b/core/crypto/aes_wrap.h new file mode 100644 index 0000000..b70b1d2 --- /dev/null +++ b/core/crypto/aes_wrap.h @@ -0,0 +1,73 @@ +/* + * AES-based functions + * + * - AES Key Wrap Algorithm (RFC3394) + * - One-Key CBC MAC (OMAC1) hash with AES-128 and AES-256 + * - AES-128/192/256 CTR mode encryption + * - AES-128 EAX mode encryption/decryption + * - AES-128 CBC + * - AES-GCM + * - AES-CCM + * + * Copyright (c) 2003-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef AES_WRAP_H +#define AES_WRAP_H + +int __must_check aes_wrap(const u8 *kek, size_t kek_len, int n, const u8 *plain, + u8 *cipher); +int __must_check aes_unwrap(const u8 *kek, size_t kek_len, int n, + const u8 *cipher, u8 *plain); +int __must_check omac1_aes_vector(const u8 *key, size_t key_len, + size_t num_elem, const u8 *addr[], + const size_t *len, u8 *mac); +int __must_check omac1_aes_128_vector(const u8 *key, size_t num_elem, + const u8 *addr[], const size_t *len, + u8 *mac); +int __must_check omac1_aes_128(const u8 *key, const u8 *data, size_t data_len, + u8 *mac); +int __must_check omac1_aes_256(const u8 *key, const u8 *data, size_t data_len, + u8 *mac); +int __must_check aes_128_encrypt_block(const u8 *key, const u8 *in, u8 *out); +int __must_check aes_ctr_encrypt(const u8 *key, size_t key_len, const u8 *nonce, + u8 *data, size_t data_len); +int __must_check aes_128_ctr_encrypt(const u8 *key, const u8 *nonce, + u8 *data, size_t data_len); +int __must_check aes_128_eax_encrypt(const u8 *key, + const u8 *nonce, size_t nonce_len, + const u8 *hdr, size_t hdr_len, + u8 *data, size_t data_len, u8 *tag); +int __must_check aes_128_eax_decrypt(const u8 *key, + const u8 *nonce, size_t nonce_len, + const u8 *hdr, size_t hdr_len, + u8 *data, size_t data_len, const u8 *tag); +int __must_check aes_128_cbc_encrypt(const u8 *key, const u8 *iv, u8 *data, + size_t data_len); +int __must_check aes_128_cbc_decrypt(const u8 *key, const u8 *iv, u8 *data, + size_t data_len); +int __must_check aes_gcm_ae(const u8 *key, size_t key_len, + const u8 *iv, size_t iv_len, + const u8 *plain, size_t plain_len, + const u8 *aad, size_t aad_len, + u8 *crypt, u8 *tag); +int __must_check aes_gcm_ad(const u8 *key, size_t key_len, + const u8 *iv, size_t iv_len, + const u8 *crypt, size_t crypt_len, + const u8 *aad, size_t aad_len, const u8 *tag, + u8 *plain); +int __must_check aes_gmac(const u8 *key, size_t key_len, + const u8 *iv, size_t iv_len, + const u8 *aad, size_t aad_len, u8 *tag); +int __must_check aes_ccm_ae(const u8 *key, size_t key_len, const u8 *nonce, + size_t M, const u8 *plain, size_t plain_len, + const u8 *aad, size_t aad_len, u8 *crypt, u8 *auth); +int __must_check aes_ccm_ad(const u8 *key, size_t key_len, const u8 *nonce, + size_t M, const u8 *crypt, size_t crypt_len, + const u8 *aad, size_t aad_len, const u8 *auth, + u8 *plain); + +#endif /* AES_WRAP_H */ diff --git a/core/crypto/ccmp.c b/core/crypto/ccmp.c new file mode 100644 index 0000000..3ebd487 --- /dev/null +++ b/core/crypto/ccmp.c @@ -0,0 +1,385 @@ +/* + * CTR with CBC-MAC Protocol (CCMP) + * Copyright (c) 2010-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" +#include "wlancrypto_wrap.h" + + + +static void ccmp_aad_nonce(_adapter *padapter, const struct ieee80211_hdr *hdr, const u8 *data, + u8 *aad, size_t *aad_len, u8 *nonce) +{ + u16 fc, stype, seq; + int qos = 0, addr4 = 0; + u8 *pos; + + nonce[0] = 0; + + fc = le_to_host16(hdr->frame_control); + stype = WLAN_FC_GET_STYPE(fc); + if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) == + (WLAN_FC_TODS | WLAN_FC_FROMDS)) + addr4 = 1; + + if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) { + fc &= ~0x0070; /* Mask subtype bits */ + if (stype & WLAN_FC_STYPE_QOS_DATA) { + const u8 *qc; + qos = 1; + fc &= ~WLAN_FC_ORDER; + qc = (const u8 *)hdr + 24; + if (addr4) + qc += ETH_ALEN; + nonce[0] = qc[0] & 0x0f; + } + } else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT) + nonce[0] |= 0x10; /* Management */ + + fc &= ~(WLAN_FC_RETRY | WLAN_FC_PWRMGT | WLAN_FC_MOREDATA); + fc |= WLAN_FC_ISWEP; + WPA_PUT_LE16(aad, fc); + pos = aad + 2; + os_memcpy(pos, GetAddr1Ptr((u8 *)hdr), 3 * ETH_ALEN); + pos += 3 * ETH_ALEN; + seq = le_to_host16(hdr->seq_ctrl); + seq &= ~0xfff0; /* Mask Seq#; do not modify Frag# */ + WPA_PUT_LE16(pos, seq); + pos += 2; + + os_memcpy(pos, (u8 *)hdr + 24, addr4 * ETH_ALEN + qos * 2); + pos += addr4 * ETH_ALEN; + if (qos) { + pos[0] &= ~0x70; + /* only spp mode need to refer QoS bit7 */ + if (padapter->registrypriv.amsdu_mode != RTW_AMSDU_MODE_SPP) + pos[0] &= ~0x80; + pos++; + *pos++ = 0x00; + } + + *aad_len = pos - aad; + + os_memcpy(nonce + 1, hdr->addr2, ETH_ALEN); + nonce[7] = data[7]; /* PN5 */ + nonce[8] = data[6]; /* PN4 */ + nonce[9] = data[5]; /* PN3 */ + nonce[10] = data[4]; /* PN2 */ + nonce[11] = data[1]; /* PN1 */ + nonce[12] = data[0]; /* PN0 */ +} + + +static void ccmp_aad_nonce_pv1(const u8 *hdr, const u8 *a1, const u8 *a2, + const u8 *a3, const u8 *pn, + u8 *aad, size_t *aad_len, u8 *nonce) +{ + u16 fc, type; + u8 *pos; + + nonce[0] = BIT(5); /* PV1 */ + /* TODO: Priority for QMF; 0 is used for Data frames */ + + fc = WPA_GET_LE16(hdr); + type = (fc & (BIT(2) | BIT(3) | BIT(4))) >> 2; + + if (type == 1) + nonce[0] |= 0x10; /* Management */ + + fc &= ~(BIT(10) | BIT(11) | BIT(13) | BIT(14) | BIT(15)); + fc |= BIT(12); + WPA_PUT_LE16(aad, fc); + pos = aad + 2; + if (type == 0 || type == 3) { + const u8 *sc; + + os_memcpy(pos, a1, ETH_ALEN); + pos += ETH_ALEN; + os_memcpy(pos, a2, ETH_ALEN); + pos += ETH_ALEN; + + if (type == 0) { + /* Either A1 or A2 contains SID */ + sc = hdr + 2 + 2 + ETH_ALEN; + } else { + /* Both A1 and A2 contain full addresses */ + sc = hdr + 2 + 2 * ETH_ALEN; + } + /* SC with Sequence Number subfield (bits 4-15 of the Sequence + * Control field) masked to 0. */ + *pos++ = *sc & 0x0f; + *pos++ = 0; + + if (a3) { + os_memcpy(pos, a3, ETH_ALEN); + pos += ETH_ALEN; + } + } + + *aad_len = pos - aad; + + os_memcpy(nonce + 1, a2, ETH_ALEN); + nonce[7] = pn[5]; /* PN5 */ + nonce[8] = pn[4]; /* PN4 */ + nonce[9] = pn[3]; /* PN3 */ + nonce[10] = pn[2]; /* PN2 */ + nonce[11] = pn[1]; /* PN1 */ + nonce[12] = pn[0]; /* PN0 */ +} + + +u8 * ccmp_decrypt(_adapter *padapter, const u8 *tk, const struct ieee80211_hdr *hdr, + const u8 *data, size_t data_len, size_t *decrypted_len) +{ + u8 aad[30], nonce[13]; + size_t aad_len; + size_t mlen; + u8 *plain; + + if (data_len < 8 + 8) + return NULL; + + plain = os_malloc(data_len + AES_BLOCK_SIZE); + if (plain == NULL) + return NULL; + + mlen = data_len - 8 - 8; + + os_memset(aad, 0, sizeof(aad)); + ccmp_aad_nonce(padapter, hdr, data, aad, &aad_len, nonce); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP AAD", aad, aad_len); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP nonce", nonce, 13); + + if (aes_ccm_ad(tk, 16, nonce, 8, data + 8, mlen, aad, aad_len, + data + 8 + mlen, plain) < 0) { + u16 seq_ctrl = le_to_host16(hdr->seq_ctrl); + wpa_printf(_MSG_INFO_, "Invalid CCMP MIC in frame: A1=" MACSTR + " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u", + MAC2STR(hdr->addr1), MAC2STR(hdr->addr2), + MAC2STR(hdr->addr3), + WLAN_GET_SEQ_SEQ(seq_ctrl), + WLAN_GET_SEQ_FRAG(seq_ctrl)); + rtw_mfree(plain, data_len + AES_BLOCK_SIZE); + return NULL; + } + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP decrypted", plain, mlen); + + *decrypted_len = mlen; + return plain; +} + + +void ccmp_get_pn(u8 *pn, const u8 *data) +{ + pn[0] = data[7]; /* PN5 */ + pn[1] = data[6]; /* PN4 */ + pn[2] = data[5]; /* PN3 */ + pn[3] = data[4]; /* PN2 */ + pn[4] = data[1]; /* PN1 */ + pn[5] = data[0]; /* PN0 */ +} + + +u8 * ccmp_encrypt(_adapter *padapter, const u8 *tk, u8 *frame, size_t len, size_t hdrlen, u8 *qos, + u8 *pn, int keyid, size_t *encrypted_len) +{ + u8 aad[30], nonce[13]; + size_t aad_len, plen; + u8 *crypt, *pos, *pdata; + struct ieee80211_hdr *hdr; + + if (len < hdrlen || hdrlen < 24) + return NULL; + plen = len - hdrlen; + + crypt = os_malloc(hdrlen + 8 + plen + 8 + AES_BLOCK_SIZE); + if (crypt == NULL) + return NULL; + + if (pn == NULL) { + os_memcpy(crypt, frame, hdrlen + 8); + hdr = (struct ieee80211_hdr *) crypt; + hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP); + pos = crypt + hdrlen + 8; + pdata = frame + hdrlen + 8; + } else { + os_memcpy(crypt, frame, hdrlen); + hdr = (struct ieee80211_hdr *) crypt; + hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP); + pos = crypt + hdrlen; + *pos++ = pn[5]; /* PN0 */ + *pos++ = pn[4]; /* PN1 */ + *pos++ = 0x00; /* Rsvd */ + *pos++ = 0x20 | (keyid << 6); + *pos++ = pn[3]; /* PN2 */ + *pos++ = pn[2]; /* PN3 */ + *pos++ = pn[1]; /* PN4 */ + *pos++ = pn[0]; /* PN5 */ + pdata = frame + hdrlen; + } + + os_memset(aad, 0, sizeof(aad)); + ccmp_aad_nonce(padapter, hdr, crypt + hdrlen, aad, &aad_len, nonce); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP AAD", aad, aad_len); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP nonce", nonce, 13); + + if (aes_ccm_ae(tk, 16, nonce, 8, pdata, plen, aad, aad_len, + pos, pos + plen) < 0) { + rtw_mfree(crypt, hdrlen + 8 + plen + 8 + AES_BLOCK_SIZE); + return NULL; + } + + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP encrypted", crypt + hdrlen + 8, plen); + + *encrypted_len = hdrlen + 8 + plen + 8; + + return crypt; +} + + +u8 * ccmp_encrypt_pv1(const u8 *tk, const u8 *a1, const u8 *a2, const u8 *a3, + const u8 *frame, size_t len, + size_t hdrlen, const u8 *pn, int keyid, + size_t *encrypted_len) +{ + u8 aad[24], nonce[13]; + size_t aad_len, plen; + u8 *crypt, *pos; + struct ieee80211_hdr *hdr; + + if (len < hdrlen || hdrlen < 12) + return NULL; + plen = len - hdrlen; + + crypt = os_malloc(hdrlen + plen + 8 + AES_BLOCK_SIZE); + if (crypt == NULL) + return NULL; + + os_memcpy(crypt, frame, hdrlen); + hdr = (struct ieee80211_hdr *) crypt; + hdr->frame_control |= host_to_le16(BIT(12)); /* Protected Frame */ + pos = crypt + hdrlen; + + os_memset(aad, 0, sizeof(aad)); + ccmp_aad_nonce_pv1(crypt, a1, a2, a3, pn, aad, &aad_len, nonce); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP AAD", aad, aad_len); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP nonce", nonce, sizeof(nonce)); + + if (aes_ccm_ae(tk, 16, nonce, 8, frame + hdrlen, plen, aad, aad_len, + pos, pos + plen) < 0) { + rtw_mfree(crypt, hdrlen + plen + 8 + AES_BLOCK_SIZE); + return NULL; + } + + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP encrypted", crypt + hdrlen, plen); + + *encrypted_len = hdrlen + plen + 8; + + return crypt; +} + + +u8 * ccmp_256_decrypt(_adapter *padapter, const u8 *tk, const struct ieee80211_hdr *hdr, + const u8 *data, size_t data_len, size_t *decrypted_len) +{ + u8 aad[30], nonce[13]; + size_t aad_len; + size_t mlen; + u8 *plain; + + if (data_len < 8 + 16) + return NULL; + + plain = os_malloc(data_len + AES_BLOCK_SIZE); + if (plain == NULL) + return NULL; + + mlen = data_len - 8 - 16; + + os_memset(aad, 0, sizeof(aad)); + ccmp_aad_nonce(padapter, 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 (aes_ccm_ad(tk, 32, nonce, 16, data + 8, mlen, aad, aad_len, + data + 8 + mlen, plain) < 0) { + u16 seq_ctrl = le_to_host16(hdr->seq_ctrl); + wpa_printf(_MSG_INFO_, "Invalid CCMP-256 MIC in frame: A1=" MACSTR + " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u", + MAC2STR(hdr->addr1), MAC2STR(hdr->addr2), + MAC2STR(hdr->addr3), + WLAN_GET_SEQ_SEQ(seq_ctrl), + WLAN_GET_SEQ_FRAG(seq_ctrl)); + rtw_mfree(plain, data_len + AES_BLOCK_SIZE); + return NULL; + } + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP-256 decrypted", plain, mlen); + + *decrypted_len = mlen; + return plain; +} + + +u8 * ccmp_256_encrypt(_adapter *padapter, const u8 *tk, u8 *frame, size_t len, size_t hdrlen, + u8 *qos, u8 *pn, int keyid, size_t *encrypted_len) +{ + u8 aad[30], nonce[13]; + size_t aad_len, plen; + u8 *crypt, *pos, *pdata; + struct ieee80211_hdr *hdr; + + if (len < hdrlen || hdrlen < 24) + return NULL; + plen = len - hdrlen; + + crypt = os_malloc(hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE); + if (crypt == NULL) + return NULL; + + if (pn == NULL) { + os_memcpy(crypt, frame, hdrlen + 8); + hdr = (struct ieee80211_hdr *) crypt; + hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP); + pos = crypt + hdrlen + 8; + pdata = frame + hdrlen + 8; + } else { + os_memcpy(crypt, frame, hdrlen); + hdr = (struct ieee80211_hdr *) crypt; + hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP); + pos = crypt + hdrlen; + *pos++ = pn[5]; /* PN0 */ + *pos++ = pn[4]; /* PN1 */ + *pos++ = 0x00; /* Rsvd */ + *pos++ = 0x20 | (keyid << 6); + *pos++ = pn[3]; /* PN2 */ + *pos++ = pn[2]; /* PN3 */ + *pos++ = pn[1]; /* PN4 */ + *pos++ = pn[0]; /* PN5 */ + pdata = frame + hdrlen; + } + + os_memset(aad, 0, sizeof(aad)); + ccmp_aad_nonce(padapter, hdr, crypt + hdrlen, aad, &aad_len, nonce); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP-256 AAD", aad, aad_len); + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP-256 nonce", nonce, 13); + + if (aes_ccm_ae(tk, 32, nonce, 16, pdata, plen, aad, aad_len, + pos, pos + plen) < 0) { + rtw_mfree(crypt, hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE); + return NULL; + } + + wpa_hexdump(_MSG_EXCESSIVE_, "CCMP-256 encrypted", crypt + hdrlen + 8, + plen); + + *encrypted_len = hdrlen + 8 + plen + 16; + + return crypt; +} diff --git a/core/crypto/gcmp.c b/core/crypto/gcmp.c new file mode 100644 index 0000000..9141d55 --- /dev/null +++ b/core/crypto/gcmp.c @@ -0,0 +1,194 @@ +/* + * GCM with GMAC Protocol (GCMP) + * Copyright (c) 2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "aes.h" +#include "aes_wrap.h" +#include "wlancrypto_wrap.h" + + +static void gcmp_aad_nonce(_adapter * padapter, const struct ieee80211_hdr *hdr, const u8 *data, + u8 *aad, size_t *aad_len, u8 *nonce) +{ + u16 fc, stype, seq; + int qos = 0, addr4 = 0; + u8 *pos; + + fc = le_to_host16(hdr->frame_control); + stype = WLAN_FC_GET_STYPE(fc); + if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) == + (WLAN_FC_TODS | WLAN_FC_FROMDS)) + addr4 = 1; + + if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) { + fc &= ~0x0070; /* Mask subtype bits */ + if (stype & WLAN_FC_STYPE_QOS_DATA) { + const u8 *qc; + qos = 1; + fc &= ~WLAN_FC_ORDER; + qc = (const u8 *)hdr + 24; + if (addr4) + qc += ETH_ALEN; + } + } + + fc &= ~(WLAN_FC_RETRY | WLAN_FC_PWRMGT | WLAN_FC_MOREDATA); + WPA_PUT_LE16(aad, fc); + pos = aad + 2; + os_memcpy(pos, GetAddr1Ptr((u8 *)hdr), 3 * ETH_ALEN); + pos += 3 * ETH_ALEN; + seq = le_to_host16(hdr->seq_ctrl); + seq &= ~0xfff0; /* Mask Seq#; do not modify Frag# */ + WPA_PUT_LE16(pos, seq); + pos += 2; + + wpa_printf(_MSG_INFO_, "pos - aad = %u, qos(%d)\n", (pos - aad), qos); + + os_memcpy(pos, (u8 *)hdr + 24, addr4 * ETH_ALEN + qos * 2); + pos += addr4 * ETH_ALEN; + if (qos) { + pos[0] &= ~0x70; + /* only spp mode need to refer QoS bit7 */ + if (padapter->registrypriv.amsdu_mode != RTW_AMSDU_MODE_SPP) + pos[0] &= ~0x80; + pos++; + *pos++ = 0x00; + } + + wpa_printf(_MSG_INFO_, "pos - aad = %u\n", (pos - aad)); + *aad_len = pos - aad; + + os_memcpy(nonce, hdr->addr2, ETH_ALEN); + nonce[6] = data[7]; /* PN5 */ + nonce[7] = data[6]; /* PN4 */ + nonce[8] = data[5]; /* PN3 */ + nonce[9] = data[4]; /* PN2 */ + nonce[10] = data[1]; /* PN1 */ + nonce[11] = data[0]; /* PN0 */ +} + +/** + * gcmp_decrypt - + * @tk: the temporal key + * @tk_len: length of @tk + * @hdr: the mac header + * @data: payload after mac header (PN + enc_data + MIC) + * @data_len: length of @data (PN + enc_data + MIC) + * @decrypted_len: length of the data decrypted + */ +u8 * gcmp_decrypt(_adapter *padapter, const u8 *tk, size_t tk_len, const struct ieee80211_hdr *hdr, + const u8 *data, size_t data_len, size_t *decrypted_len) +{ + u8 aad[30], nonce[12], *plain; + size_t aad_len, mlen; + const u8 *m; + + if (data_len < 8 + 16) + return NULL; + + plain = os_malloc(data_len + AES_BLOCK_SIZE); + if (plain == NULL) + return NULL; + + m = data + 8; + mlen = data_len - 8 - 16; + + os_memset(aad, 0, sizeof(aad)); + gcmp_aad_nonce(padapter, hdr, data, aad, &aad_len, nonce); + wpa_hexdump(_MSG_EXCESSIVE_, "GCMP AAD", aad, aad_len); + wpa_hexdump(_MSG_EXCESSIVE_, "GCMP nonce", nonce, sizeof(nonce)); + + if (aes_gcm_ad(tk, tk_len, nonce, sizeof(nonce), m, mlen, aad, aad_len, + m + mlen, plain) < 0) { + u16 seq_ctrl = le_to_host16(hdr->seq_ctrl); + wpa_printf(_MSG_INFO_, "Invalid GCMP frame: A1=" MACSTR + " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u", + MAC2STR(hdr->addr1), MAC2STR(hdr->addr2), + MAC2STR(hdr->addr3), + WLAN_GET_SEQ_SEQ(seq_ctrl), + WLAN_GET_SEQ_FRAG(seq_ctrl)); + rtw_mfree(plain, data_len + AES_BLOCK_SIZE); + return NULL; + } + + *decrypted_len = mlen; + return plain; +} + +/** + * gcmp_encrypt - + * @tk: the temporal key + * @tk_len: length of @tk + * @frame: the point to mac header, the frame including mac header and payload, + * if @pn is NULL, then the frame including pn + * @len: length of @frame + * length = mac header + payload + * @hdrlen: length of the mac header + * @qos: pointer to the QOS field of the frame + * @pn: packet number + * @keyid: key id + * @encrypted_len: length of the encrypted frame + * including mac header, pn, payload and MIC + */ +u8 * gcmp_encrypt(_adapter *padapter, const u8 *tk, size_t tk_len, const u8 *frame, size_t len, + size_t hdrlen, const u8 *qos, + const u8 *pn, int keyid, size_t *encrypted_len) +{ + u8 aad[30], nonce[12], *crypt, *pos; + const u8 *pdata; + size_t aad_len, plen; + struct ieee80211_hdr *hdr; + + if (len < hdrlen || hdrlen < 24) + return NULL; + plen = len - hdrlen; + + crypt = os_malloc(hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE); + if (crypt == NULL) + return NULL; + + if (pn == NULL) { + os_memcpy(crypt, frame, hdrlen + 8); + hdr = (struct ieee80211_hdr *)crypt; + pos = crypt + hdrlen + 8; + pdata = frame + hdrlen + 8; + } else { + os_memcpy(crypt, frame, hdrlen); + hdr = (struct ieee80211_hdr *)crypt; + pos = crypt + hdrlen; + + *pos++ = pn[5]; /* PN0 */ + *pos++ = pn[4]; /* PN1 */ + *pos++ = 0x00; /* Rsvd */ + *pos++ = 0x20 | (keyid << 6); + *pos++ = pn[3]; /* PN2 */ + *pos++ = pn[2]; /* PN3 */ + *pos++ = pn[1]; /* PN4 */ + *pos++ = pn[0]; /* PN5 */ + pdata = frame + hdrlen; + } + + os_memset(aad, 0, sizeof(aad)); + gcmp_aad_nonce(padapter, hdr, crypt + hdrlen, aad, &aad_len, nonce); + wpa_hexdump(_MSG_EXCESSIVE_, "GCMP AAD", aad, aad_len); + wpa_hexdump(_MSG_EXCESSIVE_, "GCMP nonce", nonce, sizeof(nonce)); + + if (aes_gcm_ae(tk, tk_len, nonce, sizeof(nonce), pdata, plen, + aad, aad_len, pos, pos + plen) < 0) { + rtw_mfree(crypt, hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE); + return NULL; + } + + wpa_hexdump(_MSG_EXCESSIVE_, "GCMP MIC", pos + plen, 16); + wpa_hexdump(_MSG_EXCESSIVE_, "GCMP encrypted", pos, plen); + + *encrypted_len = hdrlen + 8 + plen + 16; + + return crypt; +} diff --git a/core/crypto/rtw_crypto_wrap.c b/core/crypto/rtw_crypto_wrap.c new file mode 100644 index 0000000..8fdb3c9 --- /dev/null +++ b/core/crypto/rtw_crypto_wrap.c @@ -0,0 +1,85 @@ +#include "rtw_crypto_wrap.h" + +#ifndef DEBUG_CRYPTO +#define DEBUG_CRYPTO 0 +#endif /* DEBUG_CRYTO */ + +int os_memcmp(const void *s1, const void *s2, size_t n) +{ + return _rtw_memcmp2(s1, s2, n); +} + +int os_memcmp_const(const void *a, const void *b, size_t len) +{ + const u8 *aa = a; + const u8 *bb = b; + size_t i; + u8 res; + + for (res = 0, i = 0; i < len; i++) + res |= aa[i] ^ bb[i]; + + return res; +} + +void* os_memdup(const void *src, u32 sz) +{ + void *r = rtw_malloc(sz); + + if (r && src) + _rtw_memcpy(r, src, sz); + return r; +} + +size_t os_strlen(const char *s) +{ + const char *p = s; + while (*p) + p++; + return p - s; +} + + +void forced_memzero(void *ptr, size_t len) +{ + _rtw_memset(ptr, 0, len); +} + +void bin_clear_free(void *bin, size_t len) +{ + if (bin) { + forced_memzero(bin, len); + rtw_mfree(bin, len); + } +} + +void wpa_printf(int level, const char *fmt, ...) +{ +#if DEBUG_CRYPTO +#define MSG_LEN 100 + va_list args; + u8 buf[MSG_LEN] = { 0 }; + int err; + + va_start(args, fmt); + err = vsnprintf(buf, MSG_LEN, fmt, args); + va_end(args); + + RTW_INFO("%s", buf); +#undef MSG_LEN +#endif /* DEBUG_CRYPTO */ +} + +void wpa_hexdump(int level, const char *title, const void *buf, size_t len) +{ +#if DEBUG_CRYPTO + RTW_INFO_DUMP((u8 *)title, buf, len); +#endif /* DEBUG_CRYPTO */ +} + +void wpa_hexdump_key(int level, const char *title, const void *buf, size_t len) +{ +#if DEBUG_CRYPTO + RTW_INFO_DUMP((u8 *)title, buf, len); +#endif /* DEBUG_CRYPTO */ +} diff --git a/core/crypto/rtw_crypto_wrap.h b/core/crypto/rtw_crypto_wrap.h new file mode 100644 index 0000000..9b64a14 --- /dev/null +++ b/core/crypto/rtw_crypto_wrap.h @@ -0,0 +1,64 @@ +#ifndef RTW_CRYTO_WRAP_H +#define RTW_CRYTO_WRAP_H + +#include + +#define TEST_FAIL() 0 + +#define os_memset _rtw_memset +#define os_memcpy _rtw_memcpy +#define os_malloc rtw_malloc + +#define le_to_host16 le16_to_cpu +#define host_to_le16 cpu_to_le16 + +#define WPA_PUT_LE16 RTW_PUT_LE16 +#define WPA_GET_LE16 RTW_GET_LE16 +#define WPA_PUT_LE32 RTW_PUT_LE32 +#define WPA_GET_LE32 RTW_GET_LE32 +#define WPA_PUT_LE64 RTW_PUT_LE64 +#define WPA_GET_LE64 RTW_GET_LE64 +#define WPA_PUT_BE16 RTW_PUT_BE16 +#define WPA_GET_BE16 RTW_GET_BE16 +#define WPA_PUT_BE32 RTW_PUT_BE32 +#define WPA_GET_BE32 RTW_GET_BE32 +#define WPA_PUT_BE64 RTW_PUT_BE64 +#define WPA_GET_BE64 RTW_GET_BE64 + +#ifndef MAC2STR +#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5] +#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x" +#endif + +#define WLAN_FC_PVER 0x0003 +#define WLAN_FC_TODS 0x0100 +#define WLAN_FC_FROMDS 0x0200 +#define WLAN_FC_MOREFRAG 0x0400 +#define WLAN_FC_RETRY 0x0800 +#define WLAN_FC_PWRMGT 0x1000 +#define WLAN_FC_MOREDATA 0x2000 +#define WLAN_FC_ISWEP 0x4000 +#define WLAN_FC_ORDER 0x8000 + +#define WLAN_FC_TYPE_DATA RTW_IEEE80211_FTYPE_DATA +#define WLAN_FC_TYPE_MGMT RTW_IEEE80211_FTYPE_MGMT + +#define WLAN_FC_STYPE_QOS_DATA RTW_IEEE80211_STYPE_QOS_DATA + +enum { + _MSG_EXCESSIVE_, _MSG_MSGDUMP_, _MSG_DEBUG_, _MSG_INFO_, _MSG_WARNING_, _MSG_ERROR_ +}; + +int os_memcmp(const void *s1, const void *s2, size_t n); +int os_memcmp_const(const void *a, const void *b, size_t len); +void* os_memdup(const void *src, u32 sz); +size_t os_strlen(const char *s); + +void forced_memzero(void *ptr, size_t len); +void bin_clear_free(void *bin, size_t len); + +void wpa_printf(int level, const char *fmt, ...); +void wpa_hexdump(int level, const char *title, const void *buf, size_t len); +void wpa_hexdump_key(int level, const char *title, const void *buf, size_t len); + +#endif /* RTW_CRYTO_WRAP_H */ diff --git a/core/crypto/sha256-internal.c b/core/crypto/sha256-internal.c new file mode 100644 index 0000000..98228ea --- /dev/null +++ b/core/crypto/sha256-internal.c @@ -0,0 +1,230 @@ +/* + * SHA-256 hash implementation and interface functions + * Copyright (c) 2003-2011, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +//#include "common.h" +#include "sha256.h" +#include "sha256_i.h" +//#include "crypto.h" +#include "wlancrypto_wrap.h" + + +/** + * sha256_vector - SHA256 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 of failure + */ +int sha256_vector(size_t num_elem, const u8 *addr[], const size_t *len, + u8 *mac) +{ + struct _sha256_state ctx; + size_t i; + + if (TEST_FAIL()) + return -1; + + _sha256_init(&ctx); + for (i = 0; i < num_elem; i++) + if (sha256_process(&ctx, addr[i], len[i])) + return -1; + if (sha256_done(&ctx, mac)) + return -1; + return 0; +} + + +/* ===== start - public domain SHA256 implementation ===== */ + +/* This is based on SHA256 implementation in LibTomCrypt that was released into + * public domain by Tom St Denis. */ + +/* the K array */ +static const unsigned long K[64] = { + 0x428a2f98UL, 0x71374491UL, 0xb5c0fbcfUL, 0xe9b5dba5UL, 0x3956c25bUL, + 0x59f111f1UL, 0x923f82a4UL, 0xab1c5ed5UL, 0xd807aa98UL, 0x12835b01UL, + 0x243185beUL, 0x550c7dc3UL, 0x72be5d74UL, 0x80deb1feUL, 0x9bdc06a7UL, + 0xc19bf174UL, 0xe49b69c1UL, 0xefbe4786UL, 0x0fc19dc6UL, 0x240ca1ccUL, + 0x2de92c6fUL, 0x4a7484aaUL, 0x5cb0a9dcUL, 0x76f988daUL, 0x983e5152UL, + 0xa831c66dUL, 0xb00327c8UL, 0xbf597fc7UL, 0xc6e00bf3UL, 0xd5a79147UL, + 0x06ca6351UL, 0x14292967UL, 0x27b70a85UL, 0x2e1b2138UL, 0x4d2c6dfcUL, + 0x53380d13UL, 0x650a7354UL, 0x766a0abbUL, 0x81c2c92eUL, 0x92722c85UL, + 0xa2bfe8a1UL, 0xa81a664bUL, 0xc24b8b70UL, 0xc76c51a3UL, 0xd192e819UL, + 0xd6990624UL, 0xf40e3585UL, 0x106aa070UL, 0x19a4c116UL, 0x1e376c08UL, + 0x2748774cUL, 0x34b0bcb5UL, 0x391c0cb3UL, 0x4ed8aa4aUL, 0x5b9cca4fUL, + 0x682e6ff3UL, 0x748f82eeUL, 0x78a5636fUL, 0x84c87814UL, 0x8cc70208UL, + 0x90befffaUL, 0xa4506cebUL, 0xbef9a3f7UL, 0xc67178f2UL +}; + + +/* Various logical functions */ +#define RORc(x, y) \ +( ((((unsigned long) (x) & 0xFFFFFFFFUL) >> (unsigned long) ((y) & 31)) | \ + ((unsigned long) (x) << (unsigned long) (32 - ((y) & 31)))) & 0xFFFFFFFFUL) +#define Ch(x,y,z) (z ^ (x & (y ^ z))) +#define Maj(x,y,z) (((x | y) & z) | (x & y)) +#define S(x, n) RORc((x), (n)) +#define R(x, n) (((x)&0xFFFFFFFFUL)>>(n)) +#define Sigma0(x) (S(x, 2) ^ S(x, 13) ^ S(x, 22)) +#define Sigma1(x) (S(x, 6) ^ S(x, 11) ^ S(x, 25)) +#define Gamma0(x) (S(x, 7) ^ S(x, 18) ^ R(x, 3)) +#define Gamma1(x) (S(x, 17) ^ S(x, 19) ^ R(x, 10)) +#ifndef MIN +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) +#endif + +/* compress 512-bits */ +static int sha256_compress(struct _sha256_state *md, unsigned char *buf) +{ + u32 S[8], W[64], t0, t1; + u32 t; + int i; + + /* copy state into S */ + for (i = 0; i < 8; i++) { + S[i] = md->state[i]; + } + + /* copy the state into 512-bits into W[0..15] */ + for (i = 0; i < 16; i++) + W[i] = WPA_GET_BE32(buf + (4 * i)); + + /* fill W[16..63] */ + for (i = 16; i < 64; i++) { + W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) + + W[i - 16]; + } + + /* Compress */ +#define RND(a,b,c,d,e,f,g,h,i) \ + t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i]; \ + t1 = Sigma0(a) + Maj(a, b, c); \ + d += t0; \ + h = t0 + t1; + + for (i = 0; i < 64; ++i) { + RND(S[0], S[1], S[2], S[3], S[4], S[5], S[6], S[7], i); + t = S[7]; S[7] = S[6]; S[6] = S[5]; S[5] = S[4]; + S[4] = S[3]; S[3] = S[2]; S[2] = S[1]; S[1] = S[0]; S[0] = t; + } + + /* feedback */ + for (i = 0; i < 8; i++) { + md->state[i] = md->state[i] + S[i]; + } + return 0; +} + + +/* Initialize the hash state */ +void _sha256_init(struct _sha256_state *md) +{ + md->curlen = 0; + md->length = 0; + md->state[0] = 0x6A09E667UL; + md->state[1] = 0xBB67AE85UL; + md->state[2] = 0x3C6EF372UL; + md->state[3] = 0xA54FF53AUL; + md->state[4] = 0x510E527FUL; + md->state[5] = 0x9B05688CUL; + md->state[6] = 0x1F83D9ABUL; + md->state[7] = 0x5BE0CD19UL; +} + +/** + Process a block of memory though the hash + @param md The hash state + @param in The data to hash + @param inlen The length of the data (octets) + @return CRYPT_OK if successful +*/ +int sha256_process(struct _sha256_state *md, const unsigned char *in, + unsigned long inlen) +{ + unsigned long n; + + if (md->curlen >= sizeof(md->buf)) + return -1; + + while (inlen > 0) { + if (md->curlen == 0 && inlen >= SHA256_BLOCK_SIZE) { + if (sha256_compress(md, (unsigned char *) in) < 0) + return -1; + md->length += SHA256_BLOCK_SIZE * 8; + in += SHA256_BLOCK_SIZE; + inlen -= SHA256_BLOCK_SIZE; + } else { + n = MIN(inlen, (SHA256_BLOCK_SIZE - md->curlen)); + os_memcpy(md->buf + md->curlen, in, n); + md->curlen += n; + in += n; + inlen -= n; + if (md->curlen == SHA256_BLOCK_SIZE) { + if (sha256_compress(md, md->buf) < 0) + return -1; + md->length += 8 * SHA256_BLOCK_SIZE; + md->curlen = 0; + } + } + } + + return 0; +} + + +/** + Terminate the hash to get the digest + @param md The hash state + @param out [out] The destination of the hash (32 bytes) + @return CRYPT_OK if successful +*/ +int sha256_done(struct _sha256_state *md, unsigned char *out) +{ + int i; + + if (md->curlen >= sizeof(md->buf)) + return -1; + + /* increase the length of the message */ + md->length += md->curlen * 8; + + /* append the '1' bit */ + md->buf[md->curlen++] = (unsigned char) 0x80; + + /* if the length is currently above 56 bytes we append zeros + * then compress. Then we can fall back to padding zeros and length + * encoding like normal. + */ + if (md->curlen > 56) { + while (md->curlen < SHA256_BLOCK_SIZE) { + md->buf[md->curlen++] = (unsigned char) 0; + } + sha256_compress(md, md->buf); + md->curlen = 0; + } + + /* pad up to 56 bytes of zeroes */ + while (md->curlen < 56) { + md->buf[md->curlen++] = (unsigned char) 0; + } + + /* store length */ + WPA_PUT_BE64(md->buf + 56, md->length); + sha256_compress(md, md->buf); + + /* copy output */ + for (i = 0; i < 8; i++) + WPA_PUT_BE32(out + (4 * i), md->state[i]); + + return 0; +} + +/* ===== end - public domain SHA256 implementation ===== */ diff --git a/core/crypto/sha256-prf.c b/core/crypto/sha256-prf.c new file mode 100644 index 0000000..642b38f --- /dev/null +++ b/core/crypto/sha256-prf.c @@ -0,0 +1,109 @@ +/* + * SHA256-based PRF (IEEE 802.11r) + * Copyright (c) 2003-2016, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +//#include "common.h" +#include "sha256.h" +//#include "crypto.h" +#include "wlancrypto_wrap.h" + + +/** + * sha256_prf - SHA256-based Pseudo-Random Function (IEEE 802.11r, 8.5.1.5.2) + * @key: Key for PRF + * @key_len: Length of the key in bytes + * @label: A unique label for each purpose of the PRF + * @data: Extra data to bind into the key + * @data_len: Length of the data + * @buf: Buffer for the generated pseudo-random key + * @buf_len: Number of bytes of key to generate + * Returns: 0 on success, -1 on failure + * + * This function is used to derive new, cryptographically separate keys from a + * given key. + */ +int sha256_prf(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, size_t buf_len) +{ + return sha256_prf_bits(key, key_len, label, data, data_len, buf, + buf_len * 8); +} + + +/** + * sha256_prf_bits - IEEE Std 802.11-2012, 11.6.1.7.2 Key derivation function + * @key: Key for KDF + * @key_len: Length of the key in bytes + * @label: A unique label for each purpose of the PRF + * @data: Extra data to bind into the key + * @data_len: Length of the data + * @buf: Buffer for the generated pseudo-random key + * @buf_len: Number of bits of key to generate + * Returns: 0 on success, -1 on failure + * + * This function is used to derive new, cryptographically separate keys from a + * given key. If the requested buf_len is not divisible by eight, the least + * significant 1-7 bits of the last octet in the output are not part of the + * requested output. + */ +int sha256_prf_bits(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, + size_t buf_len_bits) +{ + u16 counter = 1; + size_t pos, plen; + u8 hash[SHA256_MAC_LEN]; + const u8 *addr[4]; + size_t len[4]; + u8 counter_le[2], length_le[2]; + size_t buf_len = (buf_len_bits + 7) / 8; + + addr[0] = counter_le; + len[0] = 2; + addr[1] = (u8 *) label; + len[1] = os_strlen(label); + addr[2] = data; + len[2] = data_len; + addr[3] = length_le; + len[3] = sizeof(length_le); + + WPA_PUT_LE16(length_le, buf_len_bits); + pos = 0; + while (pos < buf_len) { + plen = buf_len - pos; + WPA_PUT_LE16(counter_le, counter); + if (plen >= SHA256_MAC_LEN) { + if (hmac_sha256_vector(key, key_len, 4, addr, len, + &buf[pos]) < 0) + return -1; + pos += SHA256_MAC_LEN; + } else { + if (hmac_sha256_vector(key, key_len, 4, addr, len, + hash) < 0) + return -1; + os_memcpy(&buf[pos], hash, plen); + pos += plen; + break; + } + counter++; + } + + /* + * Mask out unused bits in the last octet if it does not use all the + * bits. + */ + if (buf_len_bits % 8) { + u8 mask = 0xff << (8 - buf_len_bits % 8); + buf[pos - 1] &= mask; + } + + forced_memzero(hash, sizeof(hash)); + + return 0; +} diff --git a/core/crypto/sha256.c b/core/crypto/sha256.c new file mode 100644 index 0000000..ea5d9e3 --- /dev/null +++ b/core/crypto/sha256.c @@ -0,0 +1,104 @@ +/* + * SHA-256 hash implementation and interface functions + * Copyright (c) 2003-2012, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "rtw_crypto_wrap.h" + +#include "sha256.h" +//#include "crypto.h" +#include "wlancrypto_wrap.h" + + +/** + * hmac_sha256_vector - HMAC-SHA256 over data vector (RFC 2104) + * @key: Key for HMAC operations + * @key_len: Length of the key in bytes + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash (32 bytes) + * Returns: 0 on success, -1 on failure + */ +int hmac_sha256_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac) +{ + unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */ + unsigned char tk[32]; + const u8 *_addr[6]; + size_t _len[6], i; + + if (num_elem > 5) { + /* + * Fixed limit on the number of fragments to avoid having to + * allocate memory (which could fail). + */ + return -1; + } + + /* if key is longer than 64 bytes reset it to key = SHA256(key) */ + if (key_len > 64) { + if (sha256_vector(1, &key, &key_len, tk) < 0) + return -1; + key = tk; + key_len = 32; + } + + /* the HMAC_SHA256 transform looks like: + * + * SHA256(K XOR opad, SHA256(K XOR ipad, text)) + * + * where K is an n byte key + * ipad is the byte 0x36 repeated 64 times + * opad is the byte 0x5c repeated 64 times + * and text is the data being protected */ + + /* start out by storing key in ipad */ + os_memset(k_pad, 0, sizeof(k_pad)); + os_memcpy(k_pad, key, key_len); + /* XOR key with ipad values */ + for (i = 0; i < 64; i++) + k_pad[i] ^= 0x36; + + /* perform inner SHA256 */ + _addr[0] = k_pad; + _len[0] = 64; + for (i = 0; i < num_elem; i++) { + _addr[i + 1] = addr[i]; + _len[i + 1] = len[i]; + } + if (sha256_vector(1 + num_elem, _addr, _len, mac) < 0) + return -1; + + os_memset(k_pad, 0, sizeof(k_pad)); + os_memcpy(k_pad, key, key_len); + /* XOR key with opad values */ + for (i = 0; i < 64; i++) + k_pad[i] ^= 0x5c; + + /* perform outer SHA256 */ + _addr[0] = k_pad; + _len[0] = 64; + _addr[1] = mac; + _len[1] = SHA256_MAC_LEN; + return sha256_vector(2, _addr, _len, mac); +} + + +/** + * hmac_sha256 - HMAC-SHA256 over data buffer (RFC 2104) + * @key: Key for HMAC operations + * @key_len: Length of the key in bytes + * @data: Pointers to the data area + * @data_len: Length of the data area + * @mac: Buffer for the hash (32 bytes) + * Returns: 0 on success, -1 on failure + */ +int hmac_sha256(const u8 *key, size_t key_len, const u8 *data, + size_t data_len, u8 *mac) +{ + return hmac_sha256_vector(key, key_len, 1, &data, &data_len, mac); +} diff --git a/core/crypto/sha256.h b/core/crypto/sha256.h new file mode 100644 index 0000000..5219022 --- /dev/null +++ b/core/crypto/sha256.h @@ -0,0 +1,30 @@ +/* + * SHA256 hash implementation and interface functions + * Copyright (c) 2003-2016, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef SHA256_H +#define SHA256_H + +#define SHA256_MAC_LEN 32 + +int hmac_sha256_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac); +int hmac_sha256(const u8 *key, size_t key_len, const u8 *data, + size_t data_len, u8 *mac); +int sha256_prf(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, size_t buf_len); +int sha256_prf_bits(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, + size_t buf_len_bits); +void tls_prf_sha256(const u8 *secret, size_t secret_len, + const char *label, const u8 *seed, size_t seed_len, + u8 *out, size_t outlen); +int hmac_sha256_kdf(const u8 *secret, size_t secret_len, + const char *label, const u8 *seed, size_t seed_len, + u8 *out, size_t outlen); + +#endif /* SHA256_H */ diff --git a/core/crypto/sha256_i.h b/core/crypto/sha256_i.h new file mode 100644 index 0000000..11ddd6b --- /dev/null +++ b/core/crypto/sha256_i.h @@ -0,0 +1,25 @@ +/* + * SHA-256 internal definitions + * Copyright (c) 2003-2011, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef SHA256_I_H +#define SHA256_I_H + +#define SHA256_BLOCK_SIZE 64 + +struct _sha256_state { + u64 length; + u32 state[8], curlen; + u8 buf[SHA256_BLOCK_SIZE]; +}; + +void _sha256_init(struct _sha256_state *md); +int sha256_process(struct _sha256_state *md, const unsigned char *in, + unsigned long inlen); +int sha256_done(struct _sha256_state *md, unsigned char *out); + +#endif /* SHA256_I_H */ diff --git a/core/crypto/wlancrypto_wrap.h b/core/crypto/wlancrypto_wrap.h new file mode 100644 index 0000000..b909a43 --- /dev/null +++ b/core/crypto/wlancrypto_wrap.h @@ -0,0 +1,34 @@ +/* + * wlantest - IEEE 802.11 protocol monitoring and testing tool + * Copyright (c) 2010-2013, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef WLANCRYPTO_WRAP_H +#define WLANCRYPTO_WRAP_H + +int sha256_vector(size_t num_elem, const u8 *addr[], const size_t *len, + u8 *mac); + +u8* ccmp_decrypt(_adapter *padapter, const u8 *tk, const struct ieee80211_hdr *hdr, + const u8 *data, size_t data_len, size_t *decrypted_len); +u8* ccmp_encrypt(_adapter *padapter, const u8 *tk, u8 *frame, size_t len, size_t hdrlen, u8 *qos, + u8 *pn, int keyid, size_t *encrypted_len); +u8* ccmp_encrypt_pv1(const u8 *tk, const u8 *a1, const u8 *a2, const u8 *a3, + const u8 *frame, size_t len, + size_t hdrlen, const u8 *pn, int keyid, + size_t *encrypted_len); +u8* ccmp_256_decrypt(_adapter *padapter, const u8 *tk, const struct ieee80211_hdr *hdr, + const u8 *data, size_t data_len, size_t *decrypted_len); +u8* ccmp_256_encrypt(_adapter *padapter, const u8 *tk, u8 *frame, size_t len, size_t hdrlen, + u8 *qos, u8 *pn, int keyid, size_t *encrypted_len); + +u8* gcmp_decrypt(_adapter *padapter, const u8 *tk, size_t tk_len, const struct ieee80211_hdr *hdr, + const u8 *data, size_t data_len, size_t *decrypted_len); +u8* gcmp_encrypt(_adapter *padapter, const u8 *tk, size_t tk_len, const u8 *frame, size_t len, + size_t hdrlen, const u8 *qos, + const u8 *pn, int keyid, size_t *encrypted_len); + +#endif /* WLANCRYPTO_WRAP_H */ diff --git a/core/efuse/rtw_efuse.c b/core/efuse/rtw_efuse.c new file mode 100644 index 0000000..613e91d --- /dev/null +++ b/core/efuse/rtw_efuse.c @@ -0,0 +1,3611 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_EFUSE_C_ + +#include +#include + +#include "../hal/efuse/efuse_mask.h" + +/*------------------------Define local variable------------------------------*/ +u8 fakeEfuseBank = {0}; +u32 fakeEfuseUsedBytes = {0}; +u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0}; +u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0}; +u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0}; + +u32 BTEfuseUsedBytes = {0}; +u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE]; +u8 BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0}; +u8 BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0}; + +u32 fakeBTEfuseUsedBytes = {0}; +u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE]; +u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0}; +u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0}; + +u8 maskfileBuffer[64]; +u8 btmaskfileBuffer[64]; + +/*------------------------Define local variable------------------------------*/ +BOOLEAN rtw_file_efuse_IsMasked(PADAPTER pAdapter, u16 Offset, u8 *maskbuf) +{ + int r = Offset / 16; + int c = (Offset % 16) / 2; + int result = 0; + + if (pAdapter->registrypriv.boffefusemask) + return FALSE; + + if (c < 4) /* Upper double word */ + result = (maskbuf[r] & (0x10 << c)); + else + result = (maskbuf[r] & (0x01 << (c - 4))); + + return (result > 0) ? 0 : 1; +} + +BOOLEAN efuse_IsBT_Masked(PADAPTER pAdapter, u16 Offset) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + if (pAdapter->registrypriv.boffefusemask) + return FALSE; + +#ifdef CONFIG_BT_EFUSE_MASK +#ifdef CONFIG_RTL8822C +#ifdef CONFIG_USB_HCI + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return (IS_BT_MASKED(8822C, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_PCI_HCI + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return (IS_BT_MASKED(8822C, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_SDIO_HCI + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return (IS_BT_MASKED(8822C, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#endif /*#ifdef CONFIG_RTL8822C*/ +#ifdef CONFIG_RTL8723F +#ifdef CONFIG_USB_HCI + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + return (IS_BT_MASKED(8723F, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_SDIO_HCI + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + return (IS_BT_MASKED(8723F, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#endif /*#ifdef CONFIG_RTL8723F*/ +#endif /* CONFIG_BT_EFUSE_MASK */ + return FALSE; +} + +void rtw_bt_efuse_mask_array(PADAPTER pAdapter, u8 *pArray) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + +#ifdef CONFIG_BT_EFUSE_MASK +#ifdef CONFIG_RTL8822C +#ifdef CONFIG_USB_HCI +if (IS_HARDWARE_TYPE_8822CU(pAdapter)) + GET_BT_MASK_ARRAY(8822C, _MUSB, pArray); +#endif +#ifdef CONFIG_PCI_HCI + if (IS_HARDWARE_TYPE_8822CE(pAdapter)) + GET_BT_MASK_ARRAY(8822C, _MPCIE, pArray); +#endif +#ifdef CONFIG_SDIO_HCI + if (IS_HARDWARE_TYPE_8822CS(pAdapter)) + GET_BT_MASK_ARRAY(8822C, _MSDIO, pArray); +#endif +#endif /*#ifdef CONFIG_RTL8822C*/ +#ifdef CONFIG_RTL8723F +#ifdef CONFIG_USB_HCI + if (IS_HARDWARE_TYPE_8723FU(pAdapter)) + GET_BT_MASK_ARRAY(8723F, _MUSB, pArray); +#endif +#ifdef CONFIG_SDIO_HCI + if (IS_HARDWARE_TYPE_8723FS(pAdapter)) + GET_BT_MASK_ARRAY(8723F, _MSDIO, pArray); +#endif +#endif /*#ifdef CONFIG_RTL8723F*/ +#endif /* CONFIG_BT_EFUSE_MASK */ + +} + +u16 rtw_get_bt_efuse_mask_arraylen(PADAPTER pAdapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(pAdapter); + +#ifdef CONFIG_BT_EFUSE_MASK +#ifdef CONFIG_RTL8822C +#ifdef CONFIG_USB_HCI + if (IS_HARDWARE_TYPE_8822CU(pAdapter)) + return GET_BT_MASK_ARRAY_LEN(8822C, _MUSB); +#endif +#ifdef CONFIG_PCI_HCI + if (IS_HARDWARE_TYPE_8822CE(pAdapter)) + return GET_BT_MASK_ARRAY_LEN(8822C, _MPCIE); +#endif +#ifdef CONFIG_SDIO_HCI + if (IS_HARDWARE_TYPE_8822CS(pAdapter)) + return GET_BT_MASK_ARRAY_LEN(8822C, _MSDIO); +#endif +#endif /*#ifdef CONFIG_RTL8822C*/ +#ifdef CONFIG_RTL8723F +#ifdef CONFIG_USB_HCI + if (IS_HARDWARE_TYPE_8723FU(pAdapter)) + return GET_BT_MASK_ARRAY_LEN(8723F, _MUSB); +#endif +#ifdef CONFIG_SDIO_HCI + if (IS_HARDWARE_TYPE_8723FS(pAdapter)) + return GET_BT_MASK_ARRAY_LEN(8723F, _MSDIO); +#endif +#endif /*CONFIG_RTL8723F*/ +#endif /* CONFIG_BT_EFUSE_MASK */ + + return 0; +} + +BOOLEAN efuse_IsMasked(PADAPTER pAdapter, u16 Offset) +{ + + if (pAdapter->registrypriv.boffefusemask) + return FALSE; + +#ifdef CONFIG_USB_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + return (IS_MASKED(8188E, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8812A) + if (IS_HARDWARE_TYPE_8812(pAdapter)) + return (IS_MASKED(8812A, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8821A) +#if 0 + if (IS_HARDWARE_TYPE_8811AU(pAdapter)) + return (IS_MASKED(8811A, _MUSB, Offset)) ? TRUE : FALSE; +#endif + if (IS_HARDWARE_TYPE_8821(pAdapter)) + return (IS_MASKED(8821A, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(pAdapter)) + return (IS_MASKED(8192E, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(pAdapter)) + return (IS_MASKED(8723B, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8703B) + if (IS_HARDWARE_TYPE_8703B(pAdapter)) + return (IS_MASKED(8703B, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(pAdapter)) + return (IS_MASKED(8814A, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8188F) + if (IS_HARDWARE_TYPE_8188F(pAdapter)) + return (IS_MASKED(8188F, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8188GTV) + if (IS_HARDWARE_TYPE_8188GTV(pAdapter)) + return (IS_MASKED(8188GTV, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + return (IS_MASKED(8822B, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8723D) + if (IS_HARDWARE_TYPE_8723D(pAdapter)) + return (IS_MASKED(8723D, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8710B) + if (IS_HARDWARE_TYPE_8710B(pAdapter)) + return (IS_MASKED(8710B, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CU(pAdapter)) + return (IS_MASKED(8821C, _MUSB, Offset)) ? TRUE : FALSE; +#endif + +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FU(pAdapter)) + return (IS_MASKED(8192F, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return (IS_MASKED(8822C, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(pAdapter)) + return (IS_MASKED(8814B, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + return (IS_MASKED(8723F, _MUSB, Offset)) ? TRUE : FALSE; +#endif +#endif /*CONFIG_USB_HCI*/ + +#ifdef CONFIG_PCI_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + return (IS_MASKED(8188E, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(pAdapter)) + return (IS_MASKED(8192E, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8812A) + if (IS_HARDWARE_TYPE_8812(pAdapter)) + return (IS_MASKED(8812A, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821(pAdapter)) + return (IS_MASKED(8821A, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(pAdapter)) + return (IS_MASKED(8723B, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(pAdapter)) + return (IS_MASKED(8814A, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + return (IS_MASKED(8822B, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CE(pAdapter)) + return (IS_MASKED(8821C, _MPCIE, Offset)) ? TRUE : FALSE; +#endif + +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FE(pAdapter)) + return (IS_MASKED(8192F, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return (IS_MASKED(8822C, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(pAdapter)) + return (IS_MASKED(8814B, _MPCIE, Offset)) ? TRUE : FALSE; +#endif +#endif /*CONFIG_PCI_HCI*/ + +#ifdef CONFIG_SDIO_HCI +#ifdef CONFIG_RTL8188E_SDIO + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + return (IS_MASKED(8188E, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_RTL8723B + if (IS_HARDWARE_TYPE_8723BS(pAdapter)) + return (IS_MASKED(8723B, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_RTL8188F + if (IS_HARDWARE_TYPE_8188F(pAdapter)) + return (IS_MASKED(8188F, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_RTL8188GTV + if (IS_HARDWARE_TYPE_8188GTV(pAdapter)) + return (IS_MASKED(8188GTV, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#ifdef CONFIG_RTL8192E + if (IS_HARDWARE_TYPE_8192ES(pAdapter)) + return (IS_MASKED(8192E, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821S(pAdapter)) + return (IS_MASKED(8821A, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CS(pAdapter)) + return (IS_MASKED(8821C, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + return (IS_MASKED(8822B, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FS(pAdapter)) + return (IS_MASKED(8192F, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return (IS_MASKED(8822C, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + return (IS_MASKED(8723F, _MSDIO, Offset)) ? TRUE : FALSE; +#endif +#endif /*CONFIG_SDIO_HCI*/ + + return FALSE; +} + +void rtw_efuse_mask_array(PADAPTER pAdapter, u8 *pArray) +{ + +#ifdef CONFIG_USB_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + GET_MASK_ARRAY(8188E, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8812A) + if (IS_HARDWARE_TYPE_8812(pAdapter)) + GET_MASK_ARRAY(8812A, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821(pAdapter)) + GET_MASK_ARRAY(8821A, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(pAdapter)) + GET_MASK_ARRAY(8192E, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(pAdapter)) + GET_MASK_ARRAY(8723B, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8703B) + if (IS_HARDWARE_TYPE_8703B(pAdapter)) + GET_MASK_ARRAY(8703B, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8188F) + if (IS_HARDWARE_TYPE_8188F(pAdapter)) + GET_MASK_ARRAY(8188F, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8188GTV) + if (IS_HARDWARE_TYPE_8188GTV(pAdapter)) + GET_MASK_ARRAY(8188GTV, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(pAdapter)) + GET_MASK_ARRAY(8814A, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + GET_MASK_ARRAY(8822B, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CU(pAdapter)) + GET_MASK_ARRAY(8821C, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FU(pAdapter)) + GET_MASK_ARRAY(8192F, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + GET_MASK_ARRAY(8822C, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(pAdapter)) + GET_MASK_ARRAY(8814B, _MUSB, pArray); +#endif +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + GET_MASK_ARRAY(8723F, _MUSB, pArray); +#endif +#endif /*CONFIG_USB_HCI*/ + +#ifdef CONFIG_PCI_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + GET_MASK_ARRAY(8188E, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(pAdapter)) + GET_MASK_ARRAY(8192E, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8812A) + if (IS_HARDWARE_TYPE_8812(pAdapter)) + GET_MASK_ARRAY(8812A, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821(pAdapter)) + GET_MASK_ARRAY(8821A, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(pAdapter)) + GET_MASK_ARRAY(8723B, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(pAdapter)) + GET_MASK_ARRAY(8814A, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + GET_MASK_ARRAY(8822B, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CE(pAdapter)) + GET_MASK_ARRAY(8821C, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FE(pAdapter)) + GET_MASK_ARRAY(8192F, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + GET_MASK_ARRAY(8822C, _MPCIE, pArray); +#endif +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(pAdapter)) + GET_MASK_ARRAY(8814B, _MPCIE, pArray); +#endif +#endif /*CONFIG_PCI_HCI*/ + +#ifdef CONFIG_SDIO_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + GET_MASK_ARRAY(8188E, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723BS(pAdapter)) + GET_MASK_ARRAY(8723B, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8188F) + if (IS_HARDWARE_TYPE_8188F(pAdapter)) + GET_MASK_ARRAY(8188F, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8188GTV) + if (IS_HARDWARE_TYPE_8188GTV(pAdapter)) + GET_MASK_ARRAY(8188GTV, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192ES(pAdapter)) + GET_MASK_ARRAY(8192E, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821S(pAdapter)) + GET_MASK_ARRAY(8821A, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CS(pAdapter)) + GET_MASK_ARRAY(8821C , _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + GET_MASK_ARRAY(8822B , _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FS(pAdapter)) + GET_MASK_ARRAY(8192F, _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + GET_MASK_ARRAY(8822C , _MSDIO, pArray); +#endif +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + GET_MASK_ARRAY(8723F, _MSDIO, pArray); +#endif +#endif /*CONFIG_SDIO_HCI*/ +} + +u16 rtw_get_efuse_mask_arraylen(PADAPTER pAdapter) +{ + +#ifdef CONFIG_USB_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + return GET_MASK_ARRAY_LEN(8188E, _MUSB); +#endif +#if defined(CONFIG_RTL8812A) + if (IS_HARDWARE_TYPE_8812(pAdapter)) + return GET_MASK_ARRAY_LEN(8812A, _MUSB); +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821(pAdapter)) + return GET_MASK_ARRAY_LEN(8821A, _MUSB); +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(pAdapter)) + return GET_MASK_ARRAY_LEN(8192E, _MUSB); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(pAdapter)) + return GET_MASK_ARRAY_LEN(8723B, _MUSB); +#endif +#if defined(CONFIG_RTL8703B) + if (IS_HARDWARE_TYPE_8703B(pAdapter)) + return GET_MASK_ARRAY_LEN(8703B, _MUSB); +#endif +#if defined(CONFIG_RTL8188F) + if (IS_HARDWARE_TYPE_8188F(pAdapter)) + return GET_MASK_ARRAY_LEN(8188F, _MUSB); +#endif +#if defined(CONFIG_RTL8188GTV) + if (IS_HARDWARE_TYPE_8188GTV(pAdapter)) + return GET_MASK_ARRAY_LEN(8188GTV, _MUSB); +#endif +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(pAdapter)) + return GET_MASK_ARRAY_LEN(8814A, _MUSB); +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + return GET_MASK_ARRAY_LEN(8822B, _MUSB); +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CU(pAdapter)) + return GET_MASK_ARRAY_LEN(8821C, _MUSB); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FU(pAdapter)) + return GET_MASK_ARRAY_LEN(8192F, _MUSB); +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return GET_MASK_ARRAY_LEN(8822C, _MUSB); +#endif +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(pAdapter)) { + return GET_MASK_ARRAY_LEN(8814B, _MUSB); + } +#endif +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + return GET_MASK_ARRAY_LEN(8723F, _MUSB); +#endif +#endif /*CONFIG_USB_HCI*/ + +#ifdef CONFIG_PCI_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + return GET_MASK_ARRAY_LEN(8188E, _MPCIE); +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(pAdapter)) + return GET_MASK_ARRAY_LEN(8192E, _MPCIE); +#endif +#if defined(CONFIG_RTL8812A) + if (IS_HARDWARE_TYPE_8812(pAdapter)) + return GET_MASK_ARRAY_LEN(8812A, _MPCIE); +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821(pAdapter)) + return GET_MASK_ARRAY_LEN(8821A, _MPCIE); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(pAdapter)) + return GET_MASK_ARRAY_LEN(8723B, _MPCIE); +#endif +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(pAdapter)) + return GET_MASK_ARRAY_LEN(8814A, _MPCIE); +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + return GET_MASK_ARRAY_LEN(8822B, _MPCIE); +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CE(pAdapter)) + return GET_MASK_ARRAY_LEN(8821C, _MPCIE); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FE(pAdapter)) + return GET_MASK_ARRAY_LEN(8192F, _MPCIE); +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return GET_MASK_ARRAY_LEN(8822C, _MPCIE); +#endif +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(pAdapter)) + return GET_MASK_ARRAY_LEN(8814B, _MPCIE); +#endif +#endif /*CONFIG_PCI_HCI*/ + +#ifdef CONFIG_SDIO_HCI +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(pAdapter)) + return GET_MASK_ARRAY_LEN(8188E, _MSDIO); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723BS(pAdapter)) + return GET_MASK_ARRAY_LEN(8723B, _MSDIO); +#endif +#if defined(CONFIG_RTL8188F) + if (IS_HARDWARE_TYPE_8188F(pAdapter)) + return GET_MASK_ARRAY_LEN(8188F, _MSDIO); +#endif +#if defined(CONFIG_RTL8188GTV) + if (IS_HARDWARE_TYPE_8188GTV(pAdapter)) + return GET_MASK_ARRAY_LEN(8188GTV, _MSDIO); +#endif +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192ES(pAdapter)) + return GET_MASK_ARRAY_LEN(8192E, _MSDIO); +#endif +#if defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8821S(pAdapter)) + return GET_MASK_ARRAY_LEN(8821A, _MSDIO); +#endif +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821CS(pAdapter)) + return GET_MASK_ARRAY_LEN(8821C, _MSDIO); +#endif +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(pAdapter)) + return GET_MASK_ARRAY_LEN(8822B, _MSDIO); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192FS(pAdapter)) + return GET_MASK_ARRAY_LEN(8192F, _MSDIO); +#endif +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(pAdapter)) + return GET_MASK_ARRAY_LEN(8822C, _MSDIO); +#endif +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(pAdapter)) + return GET_MASK_ARRAY_LEN(8723F, _MSDIO); +#endif +#endif/*CONFIG_SDIO_HCI*/ + return 0; +} + +static void rtw_mask_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ + u16 i = 0; + + if (padapter->registrypriv.boffefusemask == 0) { + for (i = 0; i < cnts; i++) { + if (padapter->registrypriv.bFileMaskEfuse == _TRUE) { + if (rtw_file_efuse_IsMasked(padapter, addr + i, maskfileBuffer)) /*use file efuse mask.*/ + data[i] = 0xff; + else + RTW_DBG("data[%x] = %x\n", i, data[i]); + } else { + if (efuse_IsMasked(padapter, addr + i)) + data[i] = 0xff; + else + RTW_DBG("data[%x] = %x\n", i, data[i]); + } + } + } +} + +u8 rtw_efuse_mask_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ + u8 ret = _SUCCESS; + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, _FALSE); + + ret = rtw_efuse_map_read(padapter, addr, cnts , data); + + rtw_mask_map_read(padapter, addr, cnts , data); + + return ret; + +} + +/* *********************************************************** + * Efuse related code + * *********************************************************** */ +static u8 hal_EfuseSwitchToBank( + PADAPTER padapter, + u8 bank, + u8 bPseudoTest) +{ + u8 bRet = _FALSE; + u32 value32 = 0; +#ifdef HAL_EFUSE_MEMORY + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); + PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal; +#endif + + + RTW_INFO("%s: Efuse switch bank to %d\n", __FUNCTION__, bank); + if (bPseudoTest) { +#ifdef HAL_EFUSE_MEMORY + pEfuseHal->fakeEfuseBank = bank; +#else + fakeEfuseBank = bank; +#endif + bRet = _TRUE; + } else { + value32 = rtw_read32(padapter, 0x34); + bRet = _TRUE; + switch (bank) { + case 0: + value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0); + break; + case 1: + value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_0); + break; + case 2: + value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_1); + break; + case 3: + value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_2); + break; + default: + value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0); + bRet = _FALSE; + break; + } + rtw_write32(padapter, 0x34, value32); + } + + return bRet; +} + +void rtw_efuse_analyze(PADAPTER padapter, u8 Type, u8 Fake) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + PEFUSE_HAL pEfuseHal = &(pHalData->EfuseHal); + u16 eFuse_Addr = 0; + u8 offset, wden; + u16 i, j; + u8 efuseHeader = 0, efuseExtHdr = 0, efuseData[EFUSE_MAX_WORD_UNIT*2] = {0}, dataCnt = 0; + u16 efuseHeader2Byte = 0; + u8 *eFuseWord = NULL;// [EFUSE_MAX_SECTION_NUM][EFUSE_MAX_WORD_UNIT]; + u8 offset_2_0 = 0; + u8 pgSectionCnt = 0; + u8 wd_cnt = 0; + u8 max_section = 64; + u16 mapLen = 0, maprawlen = 0; + boolean bExtHeader = _FALSE; + u8 efuseType = EFUSE_WIFI; + boolean bPseudoTest = _FALSE; + u8 bank = 0, startBank = 0, endBank = 1-1; + boolean bCheckNextBank = FALSE; + u8 protectBytesBank = 0; + u16 efuse_max = 0; + u8 ParseEfuseExtHdr, ParseEfuseHeader, ParseOffset, ParseWDEN, ParseOffset2_0; + + eFuseWord = rtw_zmalloc(EFUSE_MAX_SECTION_NUM * (EFUSE_MAX_WORD_UNIT * 2)); + + if (eFuseWord == NULL) { + RTW_INFO("%s:rtw_zmalloc eFuseWord = NULL !!\n", __func__); + return; + } + + RTW_INFO("\n"); + if (Type == 0) { + if (Fake == 0) { + RTW_INFO("\n\tEFUSE_Analyze Wifi Content\n"); + efuseType = EFUSE_WIFI; + bPseudoTest = FALSE; + startBank = 0; + endBank = 0; + } else { + RTW_INFO("\n\tEFUSE_Analyze Wifi Pseudo Content\n"); + efuseType = EFUSE_WIFI; + bPseudoTest = TRUE; + startBank = 0; + endBank = 0; + } + } else { + if (Fake == 0) { + RTW_INFO("\n\tEFUSE_Analyze BT Content\n"); + efuseType = EFUSE_BT; + bPseudoTest = FALSE; + startBank = 1; + endBank = EFUSE_MAX_BANK - 1; + } else { + RTW_INFO("\n\tEFUSE_Analyze BT Pseudo Content\n"); + efuseType = EFUSE_BT; + bPseudoTest = TRUE; + startBank = 1; + endBank = EFUSE_MAX_BANK - 1; + if (IS_HARDWARE_TYPE_8821(padapter)) + endBank = 3 - 1;/*EFUSE_MAX_BANK_8821A - 1;*/ + } + } + + RTW_INFO("\n\r 1Byte header, [7:4]=offset, [3:0]=word enable\n"); + RTW_INFO("\n\r 2Byte header, header[7:5]=offset[2:0], header[4:0]=0x0F\n"); + RTW_INFO("\n\r 2Byte header, extHeader[7:4]=offset[6:3], extHeader[3:0]=word enable\n"); + + EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest); + EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAX_SECTION, (void *)&max_section, bPseudoTest); + EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_PROTECT_BYTES_BANK, (void *)&protectBytesBank, bPseudoTest); + EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_CONTENT_LEN_BANK, (void *)&efuse_max, bPseudoTest); + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&maprawlen, _FALSE); + + _rtw_memset(eFuseWord, 0xff, EFUSE_MAX_SECTION_NUM * (EFUSE_MAX_WORD_UNIT * 2)); + _rtw_memset(pEfuseHal->fakeEfuseInitMap, 0xff, EFUSE_MAX_MAP_LEN); + + if (IS_HARDWARE_TYPE_8821(padapter)) + endBank = 3 - 1;/*EFUSE_MAX_BANK_8821A - 1;*/ + + for (bank = startBank; bank <= endBank; bank++) { + if (!hal_EfuseSwitchToBank(padapter, bank, bPseudoTest)) { + RTW_INFO("EFUSE_SwitchToBank() Fail!!\n"); + goto out_free_buffer; + } + + eFuse_Addr = bank * EFUSE_MAX_BANK_SIZE; + + efuse_OneByteRead(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest); + + if (efuseHeader == 0xFF && bank == startBank && Fake != TRUE) { + RTW_INFO("Non-PGed Efuse\n"); + goto out_free_buffer; + } + RTW_INFO("EFUSE_REAL_CONTENT_LEN = %d\n", maprawlen); + + while ((efuseHeader != 0xFF) && ((efuseType == EFUSE_WIFI && (eFuse_Addr < maprawlen)) || (efuseType == EFUSE_BT && (eFuse_Addr < (endBank + 1) * EFUSE_MAX_BANK_SIZE)))) { + + RTW_INFO("Analyzing: Offset: 0x%X\n", eFuse_Addr); + + /* Check PG header for section num.*/ + if (EXT_HEADER(efuseHeader)) { + bExtHeader = TRUE; + offset_2_0 = GET_HDR_OFFSET_2_0(efuseHeader); + efuse_OneByteRead(padapter, eFuse_Addr++, &efuseExtHdr, bPseudoTest); + + if (efuseExtHdr != 0xff) { + if (ALL_WORDS_DISABLED(efuseExtHdr)) { + /* Read next pg header*/ + efuse_OneByteRead(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest); + continue; + } else { + offset = ((efuseExtHdr & 0xF0) >> 1) | offset_2_0; + wden = (efuseExtHdr & 0x0F); + efuseHeader2Byte = (efuseExtHdr<<8)|efuseHeader; + RTW_INFO("Find efuseHeader2Byte = 0x%04X, offset=%d, wden=0x%x\n", + efuseHeader2Byte, offset, wden); + } + } else { + RTW_INFO("Error, efuse[%d]=0xff, efuseExtHdr=0xff\n", eFuse_Addr-1); + break; + } + } else { + offset = ((efuseHeader >> 4) & 0x0f); + wden = (efuseHeader & 0x0f); + } + + _rtw_memset(efuseData, '\0', EFUSE_MAX_WORD_UNIT * 2); + dataCnt = 0; + + if (offset < max_section) { + for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) { + /* Check word enable condition in the section */ + if (!(wden & (0x01<> 8; + ParseEfuseHeader = (efuseHeader2Byte & 0xff); + ParseOffset2_0 = GET_HDR_OFFSET_2_0(ParseEfuseHeader); + ParseOffset = ((ParseEfuseExtHdr & 0xF0) >> 1) | ParseOffset2_0; + ParseWDEN = (ParseEfuseExtHdr & 0x0F); + RTW_INFO("Header=0x%x, ExtHeader=0x%x, ", ParseEfuseHeader, ParseEfuseExtHdr); + } else { + ParseEfuseHeader = efuseHeader; + ParseOffset = ((ParseEfuseHeader >> 4) & 0x0f); + ParseWDEN = (ParseEfuseHeader & 0x0f); + RTW_INFO("Header=0x%x, ", ParseEfuseHeader); + } + RTW_INFO("offset=0x%x(%d), word enable=0x%x\n", ParseOffset, ParseOffset, ParseWDEN); + + wd_cnt = 0; + for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) { + if (!(wden & (0x01 << i))) { + RTW_INFO("Map[ %02X ] = %02X %02X\n", ((offset * EFUSE_MAX_WORD_UNIT * 2) + (i * 2)), efuseData[wd_cnt * 2 + 0], efuseData[wd_cnt * 2 + 1]); + wd_cnt++; + } + } + + pgSectionCnt++; + bExtHeader = FALSE; + efuse_OneByteRead(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest); + if (efuseHeader == 0xFF) { + if ((eFuse_Addr + protectBytesBank) >= efuse_max) + bCheckNextBank = TRUE; + else + bCheckNextBank = FALSE; + } + } + if (!bCheckNextBank) { + RTW_INFO("Not need to check next bank, eFuse_Addr=%d, protectBytesBank=%d, efuse_max=%d\n", + eFuse_Addr, protectBytesBank, efuse_max); + break; + } + } + /* switch bank back to 0 for BT/wifi later use*/ + hal_EfuseSwitchToBank(padapter, 0, bPseudoTest); + + /* 3. Collect 16 sections and 4 word unit into Efuse map.*/ + for (i = 0; i < max_section; i++) { + for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) { + pEfuseHal->fakeEfuseInitMap[(i*8)+(j*2)] = (eFuseWord[(i*8)+(j*2)]); + pEfuseHal->fakeEfuseInitMap[(i*8)+((j*2)+1)] = (eFuseWord[(i*8)+((j*2)+1)]); + } + } + + RTW_INFO("\n\tEFUSE Analyze Map\n"); + i = 0; + j = 0; + + for (i = 0; i < mapLen; i++) { + if (i % 16 == 0) + RTW_PRINT_SEL(RTW_DBGDUMP, "0x%03x: ", i); + _RTW_PRINT_SEL(RTW_DBGDUMP, "%02X%s" + , pEfuseHal->fakeEfuseInitMap[i] + , ((i + 1) % 16 == 0) ? "\n" : (((i + 1) % 8 == 0) ? " " : " ") + ); + } + _RTW_PRINT_SEL(RTW_DBGDUMP, "\n"); + +out_free_buffer: + if (eFuseWord) + rtw_mfree((u8 *)eFuseWord, EFUSE_MAX_SECTION_NUM * (EFUSE_MAX_WORD_UNIT * 2)); +} + +void efuse_PreUpdateAction( + PADAPTER pAdapter, + u32 *BackupRegs) +{ + if (IS_HARDWARE_TYPE_8812AU(pAdapter) || IS_HARDWARE_TYPE_8822BU(pAdapter)) { + /* <20131115, Kordan> Turn off Rx to prevent from being busy when writing the EFUSE. (Asked by Chunchu.)*/ + BackupRegs[0] = phy_query_mac_reg(pAdapter, REG_RCR, bMaskDWord); + BackupRegs[1] = phy_query_mac_reg(pAdapter, REG_RXFLTMAP0, bMaskDWord); + BackupRegs[2] = phy_query_mac_reg(pAdapter, REG_RXFLTMAP0+4, bMaskDWord); +#ifdef CONFIG_RTL8812A + BackupRegs[3] = phy_query_mac_reg(pAdapter, REG_AFE_MISC, bMaskDWord); +#endif + PlatformEFIOWrite4Byte(pAdapter, REG_RCR, 0x1); + PlatformEFIOWrite1Byte(pAdapter, REG_RXFLTMAP0, 0); + PlatformEFIOWrite1Byte(pAdapter, REG_RXFLTMAP0+1, 0); + PlatformEFIOWrite1Byte(pAdapter, REG_RXFLTMAP0+2, 0); + PlatformEFIOWrite1Byte(pAdapter, REG_RXFLTMAP0+3, 0); + PlatformEFIOWrite1Byte(pAdapter, REG_RXFLTMAP0+4, 0); + PlatformEFIOWrite1Byte(pAdapter, REG_RXFLTMAP0+5, 0); +#ifdef CONFIG_RTL8812A + /* <20140410, Kordan> 0x11 = 0x4E, lower down LX_SPS0 voltage. (Asked by Chunchu)*/ + phy_set_mac_reg(pAdapter, REG_AFE_MISC, bMaskByte1, 0x4E); +#endif + RTW_INFO(" %s , done\n", __func__); + + } +} + + +void efuse_PostUpdateAction( + PADAPTER pAdapter, + u32 *BackupRegs) +{ + if (IS_HARDWARE_TYPE_8812AU(pAdapter) || IS_HARDWARE_TYPE_8822BU(pAdapter)) { + /* <20131115, Kordan> Turn on Rx and restore the registers. (Asked by Chunchu.)*/ + phy_set_mac_reg(pAdapter, REG_RCR, bMaskDWord, BackupRegs[0]); + phy_set_mac_reg(pAdapter, REG_RXFLTMAP0, bMaskDWord, BackupRegs[1]); + phy_set_mac_reg(pAdapter, REG_RXFLTMAP0+4, bMaskDWord, BackupRegs[2]); +#ifdef CONFIG_RTL8812A + phy_set_mac_reg(pAdapter, REG_AFE_MISC, bMaskDWord, BackupRegs[3]); +#endif + RTW_INFO(" %s , done\n", __func__); + } +} + + +#ifdef RTW_HALMAC +#include "../../hal/hal_halmac.h" + +void Efuse_PowerSwitch(PADAPTER adapter, u8 write, u8 pwrstate) +{ +} + +void BTEfuse_PowerSwitch(PADAPTER adapter, u8 write, u8 pwrstate) +{ +} + +u8 efuse_GetCurrentSize(PADAPTER adapter, u16 *size) +{ + *size = 0; + + return _FAIL; +} + +u16 efuse_GetMaxSize(PADAPTER adapter) +{ + struct dvobj_priv *d; + u32 size = 0; + int err; + + d = adapter_to_dvobj(adapter); + err = rtw_halmac_get_physical_efuse_size(d, &size); + if (err) + return 0; + + return size; +} + +u16 efuse_GetavailableSize(PADAPTER adapter) +{ + struct dvobj_priv *d; + u32 size = 0; + int err; + + d = adapter_to_dvobj(adapter); + err = rtw_halmac_get_available_efuse_size(d, &size); + if (err) + return 0; + + return size; +} + + +u8 efuse_bt_GetCurrentSize(PADAPTER adapter, u16 *usesize) +{ + u8 *efuse_map; + + *usesize = 0; + efuse_map = rtw_malloc(EFUSE_BT_MAP_LEN); + if (efuse_map == NULL) { + RTW_DBG("%s: malloc FAIL\n", __FUNCTION__); + return _FAIL; + } + + /* for get bt phy efuse last use byte */ + hal_ReadEFuse_BT_logic_map(adapter, 0x00, EFUSE_BT_MAP_LEN, efuse_map); + *usesize = fakeBTEfuseUsedBytes; + + if (efuse_map) + rtw_mfree(efuse_map, EFUSE_BT_MAP_LEN); + + return _SUCCESS; +} + +u16 efuse_bt_GetMaxSize(PADAPTER adapter) +{ + return EFUSE_BT_REAL_CONTENT_LEN - EFUSE_PROTECT_BYTES_BANK; +} + +void EFUSE_GetEfuseDefinition(PADAPTER adapter, u8 efusetype, u8 type, void *out, BOOLEAN test) +{ + struct dvobj_priv *d; + u32 v32 = 0; + + + d = adapter_to_dvobj(adapter); + + if (adapter->hal_func.EFUSEGetEfuseDefinition) { + adapter->hal_func.EFUSEGetEfuseDefinition(adapter, efusetype, type, out, test); + return; + } + + if (EFUSE_WIFI == efusetype) { + switch (type) { + case TYPE_EFUSE_MAP_LEN: + rtw_halmac_get_logical_efuse_size(d, &v32); + *(u16 *)out = (u16)v32; + return; + + case TYPE_EFUSE_REAL_CONTENT_LEN: + rtw_halmac_get_physical_efuse_size(d, &v32); + *(u16 *)out = (u16)v32; + return; + } + } else if (EFUSE_BT == efusetype) { + switch (type) { + case TYPE_EFUSE_MAP_LEN: + *(u16 *)out = EFUSE_BT_MAP_LEN; + return; + + case TYPE_EFUSE_REAL_CONTENT_LEN: + *(u16 *)out = EFUSE_BT_REAL_CONTENT_LEN; + return; + } + } +} + +/* + * read/write raw efuse data + */ +u8 rtw_efuse_access(PADAPTER adapter, u8 write, u16 addr, u16 cnts, u8 *data) +{ + struct dvobj_priv *d; + u8 *efuse = NULL; + u32 size; + int err; + + + d = adapter_to_dvobj(adapter); + err = rtw_halmac_get_physical_efuse_size(d, &size); + if (err){ + size = EFUSE_MAX_SIZE; + RTW_INFO(" physical_efuse_size err size %d\n", size); + } + + if ((addr + cnts) > size) + return _FAIL; + + if (_TRUE == write) { + err = rtw_halmac_write_physical_efuse(d, addr, cnts, data); + if (err) + return _FAIL; + } else { + if (cnts > 16) + efuse = rtw_zmalloc(size); + + if (efuse) { + err = rtw_halmac_read_physical_efuse_map(d, efuse, size); + if (err) { + rtw_mfree(efuse, size); + return _FAIL; + } + + _rtw_memcpy(data, efuse + addr, cnts); + rtw_mfree(efuse, size); + } else { + err = rtw_halmac_read_physical_efuse(d, addr, cnts, data); + if (err) + return _FAIL; + } + } + + return _SUCCESS; +} + +static inline void dump_buf(u8 *buf, u32 len) +{ + u32 i; + + RTW_INFO("-----------------Len %d----------------\n", len); + for (i = 0; i < len; i++) + printk("%2.2x-", *(buf + i)); + printk("\n"); +} + +/* + * read/write raw efuse data + */ +u8 rtw_efuse_bt_access(PADAPTER adapter, u8 write, u16 addr, u16 cnts, u8 *data) +{ + struct dvobj_priv *d; + u8 *efuse = NULL; + u32 size; + int err = _FAIL; + + + d = adapter_to_dvobj(adapter); + + size = EFUSE_BT_REAL_CONTENT_LEN; + + if ((addr + cnts) > size) + return _FAIL; + + if (_TRUE == write) { + err = rtw_halmac_write_bt_physical_efuse(d, addr, cnts, data); + if (err == -1) { + RTW_ERR("%s: rtw_halmac_write_bt_physical_efuse fail!\n", __FUNCTION__); + return _FAIL; + } + RTW_INFO("%s: rtw_halmac_write_bt_physical_efuse OK! data 0x%x\n", __FUNCTION__, *data); + } else { + efuse = rtw_zmalloc(size); + + if (efuse) { + err = rtw_halmac_read_bt_physical_efuse_map(d, efuse, size); + + if (err == -1) { + RTW_ERR("%s: rtw_halmac_read_bt_physical_efuse_map fail!\n", __FUNCTION__); + rtw_mfree(efuse, size); + return _FAIL; + } + dump_buf(efuse + addr, cnts); + + _rtw_memcpy(data, efuse + addr, cnts); + + RTW_INFO("%s: rtw_halmac_read_bt_physical_efuse_map ok! data 0x%x\n", __FUNCTION__, *data); + rtw_mfree(efuse, size); + } + } + + return _SUCCESS; +} + +u8 rtw_efuse_map_read(PADAPTER adapter, u16 addr, u16 cnts, u8 *data) +{ + struct dvobj_priv *d; + u8 *efuse = NULL; + u32 size, i; + int err; + u32 backupRegs[4] = {0}; + u8 status = _SUCCESS; + + efuse_PreUpdateAction(adapter, backupRegs); + + d = adapter_to_dvobj(adapter); + err = rtw_halmac_get_logical_efuse_size(d, &size); + if (err) { + status = _FAIL; + RTW_DBG("halmac_get_logical_efuse_size fail\n"); + goto exit; + } + /* size error handle */ + if ((addr + cnts) > size) { + if (addr < size) + cnts = size - addr; + else { + status = _FAIL; + RTW_DBG(" %s() ,addr + cnts) > size fail\n", __func__); + goto exit; + } + } + + if (cnts > 16) + efuse = rtw_zmalloc(size); + + if (efuse) { + err = rtw_halmac_read_logical_efuse_map(d, efuse, size, NULL, 0); + if (err) { + rtw_mfree(efuse, size); + status = _FAIL; + RTW_DBG(" %s() ,halmac_read_logical_efus map fail\n", __func__); + goto exit; + } + + _rtw_memcpy(data, efuse + addr, cnts); + rtw_mfree(efuse, size); + } else { + err = rtw_halmac_read_logical_efuse(d, addr, cnts, data); + if (err) { + status = _FAIL; + RTW_DBG(" %s() ,halmac_read_logical_efus data fail\n", __func__); + goto exit; + } + } + status = _SUCCESS; +exit: + efuse_PostUpdateAction(adapter, backupRegs); + + return status; +} + + +u8 rtw_efuse_map_write(PADAPTER adapter, u16 addr, u16 cnts, u8 *data) +{ + struct dvobj_priv *d; + u8 *efuse = NULL; + u32 size; + int err; + u8 mask_buf[64] = ""; + u16 mask_len = sizeof(u8) * rtw_get_efuse_mask_arraylen(adapter); + u32 backupRegs[4] = {0}; + u8 status = _SUCCESS;; + + efuse_PreUpdateAction(adapter, backupRegs); + + d = adapter_to_dvobj(adapter); + err = rtw_halmac_get_logical_efuse_size(d, &size); + if (err) { + status = _FAIL; + goto exit; + } + + if ((addr + cnts) > size) { + status = _FAIL; + goto exit; + } + + efuse = rtw_zmalloc(size); + if (!efuse) { + status = _FAIL; + goto exit; + } + + err = rtw_halmac_read_logical_efuse_map(d, efuse, size, NULL, 0); + if (err) { + rtw_mfree(efuse, size); + status = _FAIL; + goto exit; + } + + _rtw_memcpy(efuse + addr, data, cnts); + + if (adapter->registrypriv.boffefusemask == 0) { + RTW_INFO("Use mask Array Len: %d\n", mask_len); + + if (mask_len != 0) { + if (adapter->registrypriv.bFileMaskEfuse == _TRUE) + _rtw_memcpy(mask_buf, maskfileBuffer, mask_len); + else + rtw_efuse_mask_array(adapter, mask_buf); + + err = rtw_halmac_write_logical_efuse_map(d, efuse, size, mask_buf, mask_len); + } else + err = rtw_halmac_write_logical_efuse_map(d, efuse, size, NULL, 0); + } else { + _rtw_memset(mask_buf, 0xFF, sizeof(mask_buf)); + RTW_INFO("Efuse mask off\n"); + err = rtw_halmac_write_logical_efuse_map(d, efuse, size, mask_buf, size/16); + } + + if (err) { + rtw_mfree(efuse, size); + status = _FAIL; + goto exit; + } + + rtw_mfree(efuse, size); + status = _SUCCESS; +exit : + efuse_PostUpdateAction(adapter, backupRegs); + + return status; +} + +int Efuse_PgPacketRead(PADAPTER adapter, u8 offset, u8 *data, BOOLEAN test) +{ + return _FALSE; +} + +int Efuse_PgPacketWrite(PADAPTER adapter, u8 offset, u8 word_en, u8 *data, BOOLEAN test) +{ + return _FALSE; +} + +static void rtw_bt_mask_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ + u16 i = 0; + +#ifdef CONFIG_BT_EFUSE_MASK + if (padapter->registrypriv.boffefusemask == 0) { + for (i = 0; i < cnts; i++) { + if (padapter->registrypriv.bBTFileMaskEfuse == _TRUE) { + if (rtw_file_efuse_IsMasked(padapter, addr + i, btmaskfileBuffer)) /*use BT file efuse mask.*/ + data[i] = 0xff; + else + RTW_INFO("data[%x] = %x\n", i, data[i]); + } else { + if (efuse_IsBT_Masked(padapter, addr + i)) /*use drv internal efuse mask.*/ + data[i] = 0xff; + else + RTW_INFO("data[%x] = %x\n", i, data[i]); + } + } + } +#endif /*CONFIG_BT_EFUSE_MASK*/ +} + +u8 rtw_BT_efuse_map_read(PADAPTER adapter, u16 addr, u16 cnts, u8 *data) +{ + hal_ReadEFuse_BT_logic_map(adapter, addr, cnts, data); + + rtw_bt_mask_map_read(adapter, addr, cnts, data); + + return _SUCCESS; +} + + +static u16 +hal_EfuseGetCurrentSize_BT( + PADAPTER padapter, + u8 bPseudoTest) +{ +#ifdef HAL_EFUSE_MEMORY + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); + PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal; +#endif + u16 btusedbytes; + u16 efuse_addr; + u8 bank, startBank; + u8 hoffset = 0, hworden = 0; + u8 efuse_data, word_cnts = 0; + u16 retU2 = 0; + + + btusedbytes = fakeBTEfuseUsedBytes; + + efuse_addr = (u16)((btusedbytes % EFUSE_BT_REAL_BANK_CONTENT_LEN)); + startBank = (u8)(1 + (btusedbytes / EFUSE_BT_REAL_BANK_CONTENT_LEN)); + + RTW_INFO("%s: start from bank=%d addr=0x%X\n", __FUNCTION__, startBank, efuse_addr); + retU2 = EFUSE_BT_REAL_CONTENT_LEN - EFUSE_PROTECT_BYTES_BANK; + + for (bank = startBank; bank < 3; bank++) { + if (hal_EfuseSwitchToBank(padapter, bank, bPseudoTest) == _FALSE) { + RTW_ERR("%s: switch bank(%d) Fail!!\n", __FUNCTION__, bank); + /* bank = EFUSE_MAX_BANK; */ + break; + } + + /* only when bank is switched we have to reset the efuse_addr. */ + if (bank != startBank) + efuse_addr = 0; + + + while (AVAILABLE_EFUSE_ADDR(efuse_addr)) { + if (rtw_efuse_bt_access(padapter, _FALSE, efuse_addr, 1, &efuse_data) == _FALSE) { + RTW_ERR("%s: efuse_OneByteRead Fail! addr=0x%X !!\n", __FUNCTION__, efuse_addr); + /* bank = EFUSE_MAX_BANK; */ + break; + } + RTW_INFO("%s: efuse_OneByteRead ! addr=0x%X !efuse_data=0x%X! bank =%d\n", __FUNCTION__, efuse_addr, efuse_data, bank); + + if (efuse_data == 0xFF) + break; + + if (EXT_HEADER(efuse_data)) { + hoffset = GET_HDR_OFFSET_2_0(efuse_data); + efuse_addr++; + rtw_efuse_bt_access(padapter, _FALSE, efuse_addr, 1, &efuse_data); + RTW_INFO("%s: efuse_OneByteRead EXT_HEADER ! addr=0x%X !efuse_data=0x%X! bank =%d\n", __FUNCTION__, efuse_addr, efuse_data, bank); + + if (ALL_WORDS_DISABLED(efuse_data)) { + efuse_addr++; + continue; + } + + /* hoffset = ((hoffset & 0xE0) >> 5) | ((efuse_data & 0xF0) >> 1); */ + hoffset |= ((efuse_data & 0xF0) >> 1); + hworden = efuse_data & 0x0F; + } else { + hoffset = (efuse_data >> 4) & 0x0F; + hworden = efuse_data & 0x0F; + } + + RTW_INFO(FUNC_ADPT_FMT": Offset=%d Worden=%#X\n", + FUNC_ADPT_ARG(padapter), hoffset, hworden); + + word_cnts = Efuse_CalculateWordCnts(hworden); + /* read next header */ + efuse_addr += (word_cnts * 2) + 1; + } + /* Check if we need to check next bank efuse */ + if (efuse_addr < retU2) + break;/* don't need to check next bank. */ + } + retU2 = ((bank - 1) * EFUSE_BT_REAL_BANK_CONTENT_LEN) + efuse_addr; + + fakeBTEfuseUsedBytes = retU2; + RTW_INFO("%s: CurrentSize=%d\n", __FUNCTION__, retU2); + return retU2; +} + +#ifdef CONFIG_RTL8822C +void rtw_pre_bt_efuse(PADAPTER padapter) +{ + char pgdata[4] = {0x72, 0x80, 0x14, 0x90}; /*BT 5M PLL*/ + u8 status = 1; + u8 bkmask; + BOOLEAN bt_en; + + bkmask = padapter->registrypriv.boffefusemask; + padapter->registrypriv.boffefusemask = 1; + + bt_en = rtw_read8(padapter, 0x6A) & BIT2 ? _TRUE : _FALSE; + if (IS_HARDWARE_TYPE_8822C(padapter) && bt_en == _TRUE) { + status = rtw_BT_efuse_map_write(padapter, 0x1f8, 4, pgdata); + RTW_INFO("%s done!!!\n", __FUNCTION__); + } + if (status == _FAIL) + RTW_INFO("%s: fail\n", __FUNCTION__); + padapter->registrypriv.boffefusemask = bkmask; +} +#endif + +u8 rtw_BT_efuse_map_write(PADAPTER adapter, u16 addr, u16 cnts, u8 *data) +{ +#define RT_ASSERT_RET(expr) \ + if (!(expr)) { \ + printk("Assertion failed! %s at ......\n", #expr); \ + printk(" ......%s,%s, line=%d\n",__FILE__, __FUNCTION__, __LINE__); \ + return _FAIL; \ + } + + u8 offset, word_en; + u8 *efuse = NULL; + u8 *map; + u8 newdata[PGPKT_DATA_SIZE]; + s32 i = 0, j = 0, idx = 0, chk_total_byte = 0; + u8 ret = _SUCCESS; + u16 mapLen = 1024; + u16 startAddr = 0; + + if ((addr + cnts) > mapLen) + return _FAIL; + + RT_ASSERT_RET(PGPKT_DATA_SIZE == 8); /* have to be 8 byte alignment */ + RT_ASSERT_RET((mapLen & 0x7) == 0); /* have to be PGPKT_DATA_SIZE alignment for memcpy */ + + efuse = rtw_zmalloc(mapLen); + if (!efuse) + return _FAIL; + + map = rtw_zmalloc(mapLen); + if (map == NULL) { + rtw_mfree(efuse, mapLen); + return _FAIL; + } + + _rtw_memset(map, 0xFF, mapLen); + + ret = rtw_BT_efuse_map_read(adapter, 0, mapLen, map); + if (ret == _FAIL) + goto exit; + + _rtw_memcpy(efuse , map, mapLen); + _rtw_memcpy(efuse + addr, data, cnts); +#ifdef CONFIG_BT_EFUSE_MASK + if (adapter->registrypriv.boffefusemask == 0) { + for (i = 0; i < cnts; i++) { + if (adapter->registrypriv.bBTFileMaskEfuse == _TRUE) { + if (rtw_file_efuse_IsMasked(adapter, addr + i, btmaskfileBuffer)) /*use file efuse mask. */ + efuse[addr + i] = map[addr + i]; + } else { + if (efuse_IsBT_Masked(adapter, addr + i)) + efuse[addr + i] = map[addr + i]; + } + RTW_INFO("%s , efuse[%x] = %x, map = %x\n", __func__, addr + i, efuse[ addr + i], map[addr + i]); + } + } +#endif /*CONFIG_BT_EFUSE_MASK*/ + /* precheck pg efuse data byte*/ + chk_total_byte = 0; + idx = 0; + offset = (addr >> 3); + + while (idx < cnts) { + word_en = 0xF; + j = (addr + idx) & 0x7; + for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) { + if (efuse[addr + idx] != map[addr + idx]) + word_en &= ~BIT(i >> 1); + } + + if (word_en != 0xF) { + chk_total_byte += Efuse_CalculateWordCnts(word_en) * 2; + + if (offset >= EFUSE_MAX_SECTION_BASE) /* Over EFUSE_MAX_SECTION 16 for 2 ByteHeader */ + chk_total_byte += 2; + else + chk_total_byte += 1; + } + + offset++; + } + + RTW_INFO("Total PG bytes Count = %d\n", chk_total_byte); + startAddr = hal_EfuseGetCurrentSize_BT(adapter, _FALSE); + RTW_INFO("%s: startAddr=%#X\n", __func__, startAddr); + + if (!AVAILABLE_EFUSE_ADDR(startAddr + chk_total_byte)) { + RTW_INFO("%s: startAddr(0x%X) + PG data len %d >= efuse BT available offset (0x%X)\n", + __func__, startAddr, chk_total_byte, EFUSE_BT_REAL_CONTENT_LEN - EFUSE_PROTECT_BYTES_BANK); + ret = _FAIL; + goto exit; + } + + idx = 0; + offset = (addr >> 3); + while (idx < cnts) { + word_en = 0xF; + j = (addr + idx) & 0x7; + _rtw_memcpy(newdata, &map[offset << 3], PGPKT_DATA_SIZE); + for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) { + if (efuse[addr + idx] != map[addr + idx]) { + word_en &= ~BIT(i >> 1); + newdata[i] = efuse[addr + idx]; + } + } + + if (word_en != 0xF) { + ret = EfusePgPacketWrite_BT(adapter, offset, word_en, newdata, _FALSE); + RTW_INFO("offset=%x\n", offset); + RTW_INFO("word_en=%x\n", word_en); + RTW_INFO("%s: data=", __FUNCTION__); + for (i = 0; i < PGPKT_DATA_SIZE; i++) + RTW_INFO("0x%02X ", newdata[i]); + RTW_INFO("\n"); + if (ret == _FAIL) + break; + } + offset++; + } +exit: + if (efuse) + rtw_mfree(efuse, mapLen); + if (map) + rtw_mfree(map, mapLen); + return ret; +} + +void hal_ReadEFuse_BT_logic_map( + PADAPTER padapter, + u16 _offset, + u16 _size_byte, + u8 *pbuf +) +{ + + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); + + u8 *efuseTbl, *phyefuse; + u8 bank; + u16 eFuse_Addr = 0; + u8 efuseHeader, efuseExtHdr, efuseData; + u8 offset, wden; + u16 i, total, used; + u8 efuse_usage; + + + /* */ + /* Do NOT excess total size of EFuse table. Added by Roger, 2008.11.10. */ + /* */ + if ((_offset + _size_byte) > EFUSE_BT_MAP_LEN) { + RTW_INFO("%s: Invalid offset(%#x) with read bytes(%#x)!!\n", __FUNCTION__, _offset, _size_byte); + return; + } + + efuseTbl = rtw_malloc(EFUSE_BT_MAP_LEN); + phyefuse = rtw_malloc(EFUSE_BT_REAL_CONTENT_LEN); + if (efuseTbl == NULL || phyefuse == NULL) { + RTW_INFO("%s: efuseTbl or phyefuse malloc fail!\n", __FUNCTION__); + goto exit; + } + + /* 0xff will be efuse default value instead of 0x00. */ + _rtw_memset(efuseTbl, 0xFF, EFUSE_BT_MAP_LEN); + _rtw_memset(phyefuse, 0xFF, EFUSE_BT_REAL_CONTENT_LEN); + + if (rtw_efuse_bt_access(padapter, _FALSE, 0, EFUSE_BT_REAL_CONTENT_LEN, phyefuse)) + dump_buf(phyefuse, EFUSE_BT_REAL_BANK_CONTENT_LEN); + + total = BANK_NUM; + for (bank = 1; bank <= total; bank++) { /* 8723d Max bake 0~2 */ + eFuse_Addr = 0; + + while (AVAILABLE_EFUSE_ADDR(eFuse_Addr)) { + /* ReadEFuseByte(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest); */ + efuseHeader = phyefuse[eFuse_Addr++]; + + if (efuseHeader == 0xFF) + break; + RTW_INFO("%s: efuse[%#X]=0x%02x (header)\n", __FUNCTION__, (((bank - 1) * EFUSE_BT_REAL_CONTENT_LEN) + eFuse_Addr - 1), efuseHeader); + + /* Check PG header for section num. */ + if (EXT_HEADER(efuseHeader)) { /* extended header */ + offset = GET_HDR_OFFSET_2_0(efuseHeader); + RTW_INFO("%s: extended header offset_2_0=0x%X\n", __FUNCTION__, offset); + + /* ReadEFuseByte(padapter, eFuse_Addr++, &efuseExtHdr, bPseudoTest); */ + efuseExtHdr = phyefuse[eFuse_Addr++]; + + RTW_INFO("%s: efuse[%#X]=0x%02x (ext header)\n", __FUNCTION__, (((bank - 1) * EFUSE_BT_REAL_CONTENT_LEN) + eFuse_Addr - 1), efuseExtHdr); + if (ALL_WORDS_DISABLED(efuseExtHdr)) + continue; + + offset |= ((efuseExtHdr & 0xF0) >> 1); + wden = (efuseExtHdr & 0x0F); + } else { + offset = ((efuseHeader >> 4) & 0x0f); + wden = (efuseHeader & 0x0f); + } + + if (offset < EFUSE_BT_MAX_SECTION) { + u16 addr; + + /* Get word enable value from PG header */ + RTW_INFO("%s: Offset=%d Worden=%#X\n", __FUNCTION__, offset, wden); + + addr = offset * PGPKT_DATA_SIZE; + for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) { + /* Check word enable condition in the section */ + if (!(wden & (0x01 << i))) { + efuseData = 0; + /* ReadEFuseByte(padapter, eFuse_Addr++, &efuseData, bPseudoTest); */ + efuseData = phyefuse[eFuse_Addr++]; + + RTW_INFO("%s: efuse[%#X]=0x%02X\n", __FUNCTION__, eFuse_Addr - 1, efuseData); + efuseTbl[addr] = efuseData; + + efuseData = 0; + /* ReadEFuseByte(padapter, eFuse_Addr++, &efuseData, bPseudoTest); */ + efuseData = phyefuse[eFuse_Addr++]; + + RTW_INFO("%s: efuse[%#X]=0x%02X\n", __FUNCTION__, eFuse_Addr - 1, efuseData); + efuseTbl[addr + 1] = efuseData; + } + addr += 2; + } + } else { + RTW_INFO("%s: offset(%d) is illegal!!\n", __FUNCTION__, offset); + eFuse_Addr += Efuse_CalculateWordCnts(wden) * 2; + } + } + + if ((eFuse_Addr - 1) < total) { + RTW_INFO("%s: bank(%d) data end at %#x\n", __FUNCTION__, bank, eFuse_Addr - 1); + break; + } + } + + /* switch bank back to bank 0 for later BT and wifi use. */ + //hal_EfuseSwitchToBank(padapter, 0, bPseudoTest); + + /* Copy from Efuse map to output pointer memory!!! */ + for (i = 0; i < _size_byte; i++) + pbuf[i] = efuseTbl[_offset + i]; + /* Calculate Efuse utilization */ + total = EFUSE_BT_REAL_BANK_CONTENT_LEN; + + used = eFuse_Addr - 1; + + if (total) + efuse_usage = (u8)((used * 100) / total); + else + efuse_usage = 100; + + fakeBTEfuseUsedBytes = used; + RTW_INFO("%s: BTEfuseUsed last Bytes = %#x\n", __FUNCTION__, fakeBTEfuseUsedBytes); + +exit: + if (efuseTbl) + rtw_mfree(efuseTbl, EFUSE_BT_MAP_LEN); + if (phyefuse) + rtw_mfree(phyefuse, EFUSE_BT_REAL_BANK_CONTENT_LEN); +} + + +static u8 hal_EfusePartialWriteCheck( + PADAPTER padapter, + u8 efuseType, + u16 *pAddr, + PPGPKT_STRUCT pTargetPkt, + u8 bPseudoTest) +{ + u8 bRet = _FALSE; + u16 startAddr = 0, efuse_max_available_len = EFUSE_BT_REAL_BANK_CONTENT_LEN, efuse_max = EFUSE_BT_REAL_BANK_CONTENT_LEN; + u8 efuse_data = 0; + + startAddr = (u16)fakeBTEfuseUsedBytes; + + startAddr %= efuse_max; + RTW_INFO("%s: startAddr=%#X\n", __FUNCTION__, startAddr); + + while (1) { + if (startAddr >= efuse_max_available_len) { + bRet = _FALSE; + RTW_INFO("%s: startAddr(%d) >= efuse_max_available_len(%d)\n", + __FUNCTION__, startAddr, efuse_max_available_len); + break; + } + if (rtw_efuse_bt_access(padapter, _FALSE, startAddr, 1, &efuse_data)&& (efuse_data != 0xFF)) { + bRet = _FALSE; + RTW_INFO("%s: Something Wrong! last bytes(%#X=0x%02X) is not 0xFF\n", + __FUNCTION__, startAddr, efuse_data); + break; + } else { + /* not used header, 0xff */ + *pAddr = startAddr; + /* RTW_INFO("%s: Started from unused header offset=%d\n", __FUNCTION__, startAddr)); */ + bRet = _TRUE; + break; + } + } + + return bRet; +} + + +static u8 hal_EfusePgPacketWrite2ByteHeader( + PADAPTER padapter, + u8 efuseType, + u16 *pAddr, + PPGPKT_STRUCT pTargetPkt, + u8 bPseudoTest) +{ + u16 efuse_addr, efuse_max_available_len = EFUSE_BT_REAL_BANK_CONTENT_LEN; + u8 pg_header = 0, tmp_header = 0; + u8 repeatcnt = 0; + + /* RTW_INFO("%s\n", __FUNCTION__); */ + + efuse_addr = *pAddr; + if (efuse_addr >= efuse_max_available_len) { + RTW_INFO("%s: addr(%d) over avaliable(%d)!!\n", __FUNCTION__, efuse_addr, efuse_max_available_len); + return _FALSE; + } + + pg_header = ((pTargetPkt->offset & 0x07) << 5) | 0x0F; + /* RTW_INFO("%s: pg_header=0x%x\n", __FUNCTION__, pg_header); */ + + do { + + rtw_efuse_bt_access(padapter, _TRUE, efuse_addr, 1, &pg_header); + rtw_efuse_bt_access(padapter, _FALSE, efuse_addr, 1, &tmp_header); + + if (tmp_header != 0xFF) + break; + if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) { + RTW_INFO("%s: Repeat over limit for pg_header!!\n", __FUNCTION__); + return _FALSE; + } + } while (1); + + if (tmp_header != pg_header) { + RTW_ERR("%s: PG Header Fail!!(pg=0x%02X read=0x%02X)\n", __FUNCTION__, pg_header, tmp_header); + return _FALSE; + } + + /* to write ext_header */ + efuse_addr++; + pg_header = ((pTargetPkt->offset & 0x78) << 1) | pTargetPkt->word_en; + + do { + rtw_efuse_bt_access(padapter, _TRUE, efuse_addr, 1, &pg_header); + rtw_efuse_bt_access(padapter, _FALSE, efuse_addr, 1, &tmp_header); + + if (tmp_header != 0xFF) + break; + if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) { + RTW_INFO("%s: Repeat over limit for ext_header!!\n", __FUNCTION__); + return _FALSE; + } + } while (1); + + if (tmp_header != pg_header) { /* offset PG fail */ + RTW_ERR("%s: PG EXT Header Fail!!(pg=0x%02X read=0x%02X)\n", __FUNCTION__, pg_header, tmp_header); + return _FALSE; + } + + *pAddr = efuse_addr; + + return _TRUE; +} + + +static u8 hal_EfusePgPacketWrite1ByteHeader( + PADAPTER pAdapter, + u8 efuseType, + u16 *pAddr, + PPGPKT_STRUCT pTargetPkt, + u8 bPseudoTest) +{ + u8 pg_header = 0, tmp_header = 0; + u16 efuse_addr = *pAddr; + u8 repeatcnt = 0; + + + /* RTW_INFO("%s\n", __FUNCTION__); */ + pg_header = ((pTargetPkt->offset << 4) & 0xf0) | pTargetPkt->word_en; + + do { + rtw_efuse_bt_access(pAdapter, _TRUE, efuse_addr, 1, &pg_header); + rtw_efuse_bt_access(pAdapter, _FALSE, efuse_addr, 1, &tmp_header); + + if (tmp_header != 0xFF) + break; + if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) { + RTW_INFO("%s: Repeat over limit for pg_header!!\n", __FUNCTION__); + return _FALSE; + } + } while (1); + + if (tmp_header != pg_header) { + RTW_ERR("%s: PG Header Fail!!(pg=0x%02X read=0x%02X)\n", __FUNCTION__, pg_header, tmp_header); + return _FALSE; + } + + *pAddr = efuse_addr; + + return _TRUE; +} + +static u8 hal_EfusePgPacketWriteHeader( + PADAPTER padapter, + u8 efuseType, + u16 *pAddr, + PPGPKT_STRUCT pTargetPkt, + u8 bPseudoTest) +{ + u8 bRet = _FALSE; + + if (pTargetPkt->offset >= EFUSE_MAX_SECTION_BASE) + bRet = hal_EfusePgPacketWrite2ByteHeader(padapter, efuseType, pAddr, pTargetPkt, bPseudoTest); + else + bRet = hal_EfusePgPacketWrite1ByteHeader(padapter, efuseType, pAddr, pTargetPkt, bPseudoTest); + + return bRet; +} + + +static u8 +Hal_EfuseWordEnableDataWrite( + PADAPTER padapter, + u16 efuse_addr, + u8 word_en, + u8 *data, + u8 bPseudoTest) +{ + u16 tmpaddr = 0; + u16 start_addr = efuse_addr; + u8 badworden = 0x0F; + u8 tmpdata[PGPKT_DATA_SIZE]; + + + /* RTW_INFO("%s: efuse_addr=%#x word_en=%#x\n", __FUNCTION__, efuse_addr, word_en); */ + _rtw_memset(tmpdata, 0xFF, PGPKT_DATA_SIZE); + + if (!(word_en & BIT(0))) { + tmpaddr = start_addr; + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[0]); + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[1]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr, 1, &tmpdata[0]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr + 1, 1, &tmpdata[1]); + if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1])) + badworden &= (~BIT(0)); + } + if (!(word_en & BIT(1))) { + tmpaddr = start_addr; + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[2]); + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[3]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr, 1, &tmpdata[2]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr + 1, 1, &tmpdata[3]); + if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3])) + badworden &= (~BIT(1)); + } + if (!(word_en & BIT(2))) { + tmpaddr = start_addr; + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[4]); + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[5]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr, 1, &tmpdata[4]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr + 1, 1, &tmpdata[5]); + if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5])) + badworden &= (~BIT(2)); + } + if (!(word_en & BIT(3))) { + tmpaddr = start_addr; + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[6]); + rtw_efuse_bt_access(padapter, _TRUE, start_addr++, 1, &data[7]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr, 1, &tmpdata[6]); + rtw_efuse_bt_access(padapter, _FALSE, tmpaddr + 1, 1, &tmpdata[7]); + + if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7])) + badworden &= (~BIT(3)); + } + + return badworden; +} + +static void +hal_EfuseConstructPGPkt( + u8 offset, + u8 word_en, + u8 *pData, + PPGPKT_STRUCT pTargetPkt) +{ + _rtw_memset(pTargetPkt->data, 0xFF, PGPKT_DATA_SIZE); + pTargetPkt->offset = offset; + pTargetPkt->word_en = word_en; + efuse_WordEnableDataRead(word_en, pData, pTargetPkt->data); + pTargetPkt->word_cnts = Efuse_CalculateWordCnts(pTargetPkt->word_en); +} + +static u8 +hal_EfusePgPacketWriteData( + PADAPTER pAdapter, + u8 efuseType, + u16 *pAddr, + PPGPKT_STRUCT pTargetPkt, + u8 bPseudoTest) +{ + u16 efuse_addr; + u8 badworden; + + efuse_addr = *pAddr; + badworden = Hal_EfuseWordEnableDataWrite(pAdapter, efuse_addr + 1, pTargetPkt->word_en, pTargetPkt->data, bPseudoTest); + if (badworden != 0x0F) { + RTW_INFO("%s: Fail!!\n", __FUNCTION__); + return _FALSE; + } else + RTW_INFO("%s: OK!!\n", __FUNCTION__); + + return _TRUE; +} + +u8 efuse_OneByteRead(struct _ADAPTER *a, u16 addr, u8 *data, u8 bPseudoTest) +{ + struct dvobj_priv *d; + int err; + u8 ret = _TRUE; + + d = adapter_to_dvobj(a); + err = rtw_halmac_read_physical_efuse(d, addr, 1, data); + if (err) { + RTW_ERR("%s: addr=0x%x FAIL!!!\n", __FUNCTION__, addr); + ret = _FALSE; + } + + return ret; + +} + + +static u8 +hal_BT_EfusePgCheckAvailableAddr( + PADAPTER pAdapter, + u8 bPseudoTest) +{ + u16 max_available = EFUSE_BT_REAL_CONTENT_LEN - EFUSE_PROTECT_BYTES_BANK; + u16 current_size = 0; + + RTW_INFO("%s: max_available=%d\n", __FUNCTION__, max_available); + current_size = hal_EfuseGetCurrentSize_BT(pAdapter, bPseudoTest); + if (current_size >= max_available) { + RTW_INFO("%s: Error!! current_size(%d)>max_available(%d)\n", __FUNCTION__, current_size, max_available); + return _FALSE; + } + return _TRUE; +} + +u8 EfusePgPacketWrite_BT( + PADAPTER pAdapter, + u8 offset, + u8 word_en, + u8 *pData, + u8 bPseudoTest) +{ + PGPKT_STRUCT targetPkt; + u16 startAddr = 0; + u8 efuseType = EFUSE_BT; + + if (!hal_BT_EfusePgCheckAvailableAddr(pAdapter, bPseudoTest)) + return _FALSE; + + hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt); + + if (!hal_EfusePartialWriteCheck(pAdapter, efuseType, &startAddr, &targetPkt, bPseudoTest)) + return _FALSE; + + if (!hal_EfusePgPacketWriteHeader(pAdapter, efuseType, &startAddr, &targetPkt, bPseudoTest)) + return _FALSE; + + if (!hal_EfusePgPacketWriteData(pAdapter, efuseType, &startAddr, &targetPkt, bPseudoTest)) + return _FALSE; + + return _TRUE; +} + + +#else /* !RTW_HALMAC */ +/* ------------------------------------------------------------------------------ */ +#define REG_EFUSE_CTRL 0x0030 +#define EFUSE_CTRL REG_EFUSE_CTRL /* E-Fuse Control. */ +/* ------------------------------------------------------------------------------ */ + + +BOOLEAN +Efuse_Read1ByteFromFakeContent( + PADAPTER pAdapter, + u16 Offset, + u8 *Value); +BOOLEAN +Efuse_Read1ByteFromFakeContent( + PADAPTER pAdapter, + u16 Offset, + u8 *Value) +{ + if (Offset >= EFUSE_MAX_HW_SIZE) + return _FALSE; + /* DbgPrint("Read fake content, offset = %d\n", Offset); */ + if (fakeEfuseBank == 0) + *Value = fakeEfuseContent[Offset]; + else + *Value = fakeBTEfuseContent[fakeEfuseBank - 1][Offset]; + return _TRUE; +} + +BOOLEAN +Efuse_Write1ByteToFakeContent( + PADAPTER pAdapter, + u16 Offset, + u8 Value); +BOOLEAN +Efuse_Write1ByteToFakeContent( + PADAPTER pAdapter, + u16 Offset, + u8 Value) +{ + if (Offset >= EFUSE_MAX_HW_SIZE) + return _FALSE; + if (fakeEfuseBank == 0) + fakeEfuseContent[Offset] = Value; + else + fakeBTEfuseContent[fakeEfuseBank - 1][Offset] = Value; + return _TRUE; +} + +/*----------------------------------------------------------------------------- + * Function: Efuse_PowerSwitch + * + * Overview: When we want to enable write operation, we should change to + * pwr on state. When we stop write, we should switch to 500k mode + * and disable LDO 2.5V. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/17/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void +Efuse_PowerSwitch( + PADAPTER pAdapter, + u8 bWrite, + u8 PwrState) +{ + pAdapter->hal_func.EfusePowerSwitch(pAdapter, bWrite, PwrState); +} + +void +BTEfuse_PowerSwitch( + PADAPTER pAdapter, + u8 bWrite, + u8 PwrState) +{ + if (pAdapter->hal_func.BTEfusePowerSwitch) + pAdapter->hal_func.BTEfusePowerSwitch(pAdapter, bWrite, PwrState); +} + +/*----------------------------------------------------------------------------- + * Function: efuse_GetCurrentSize + * + * Overview: Get current efuse size!!! + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/16/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +u16 +Efuse_GetCurrentSize( + PADAPTER pAdapter, + u8 efuseType, + BOOLEAN bPseudoTest) +{ + u16 ret = 0; + + ret = pAdapter->hal_func.EfuseGetCurrentSize(pAdapter, efuseType, bPseudoTest); + + return ret; +} + +/* + * Description: + * Execute E-Fuse read byte operation. + * Refered from SD1 Richard. + * + * Assumption: + * 1. Boot from E-Fuse and successfully auto-load. + * 2. PASSIVE_LEVEL (USB interface) + * + * Created by Roger, 2008.10.21. + * */ +void +ReadEFuseByte( + PADAPTER Adapter, + u16 _offset, + u8 *pbuf, + BOOLEAN bPseudoTest) +{ + u32 value32; + u8 readbyte; + u16 retry; + /* systime start=rtw_get_current_time(); */ + + if (bPseudoTest) { + Efuse_Read1ByteFromFakeContent(Adapter, _offset, pbuf); + return; + } + if (IS_HARDWARE_TYPE_8723B(Adapter)) { + /* <20130121, Kordan> For SMIC S55 EFUSE specificatoin. */ + /* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */ + phy_set_mac_reg(Adapter, EFUSE_TEST, BIT11, 0); + } + /* Write Address */ + rtw_write8(Adapter, EFUSE_CTRL + 1, (_offset & 0xff)); + readbyte = rtw_read8(Adapter, EFUSE_CTRL + 2); + rtw_write8(Adapter, EFUSE_CTRL + 2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc)); + + /* Write bit 32 0 */ + readbyte = rtw_read8(Adapter, EFUSE_CTRL + 3); + rtw_write8(Adapter, EFUSE_CTRL + 3, (readbyte & 0x7f)); + + /* Check bit 32 read-ready */ + retry = 0; + value32 = rtw_read32(Adapter, EFUSE_CTRL); + /* while(!(((value32 >> 24) & 0xff) & 0x80) && (retry<10)) */ + while (!(((value32 >> 24) & 0xff) & 0x80) && (retry < 10000)) { + value32 = rtw_read32(Adapter, EFUSE_CTRL); + retry++; + } + + /* 20100205 Joseph: Add delay suggested by SD1 Victor. */ + /* This fix the problem that Efuse read error in high temperature condition. */ + /* Designer says that there shall be some delay after ready bit is set, or the */ + /* result will always stay on last data we read. */ + rtw_udelay_os(50); + value32 = rtw_read32(Adapter, EFUSE_CTRL); + + *pbuf = (u8)(value32 & 0xff); + /* RTW_INFO("ReadEFuseByte _offset:%08u, in %d ms\n",_offset ,rtw_get_passing_time_ms(start)); */ + +} + +/* + * Description: + * 1. Execute E-Fuse read byte operation according as map offset and + * save to E-Fuse table. + * 2. Refered from SD1 Richard. + * + * Assumption: + * 1. Boot from E-Fuse and successfully auto-load. + * 2. PASSIVE_LEVEL (USB interface) + * + * Created by Roger, 2008.10.21. + * + * 2008/12/12 MH 1. Reorganize code flow and reserve bytes. and add description. + * 2. Add efuse utilization collect. + * 2008/12/22 MH Read Efuse must check if we write section 1 data again!!! Sec1 + * write addr must be after sec5. + * */ + +void +efuse_ReadEFuse( + PADAPTER Adapter, + u8 efuseType, + u16 _offset, + u16 _size_byte, + u8 *pbuf, + BOOLEAN bPseudoTest +); +void +efuse_ReadEFuse( + PADAPTER Adapter, + u8 efuseType, + u16 _offset, + u16 _size_byte, + u8 *pbuf, + BOOLEAN bPseudoTest +) +{ + Adapter->hal_func.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, bPseudoTest); +} + +void +EFUSE_GetEfuseDefinition( + PADAPTER pAdapter, + u8 efuseType, + u8 type, + void *pOut, + BOOLEAN bPseudoTest +) +{ + pAdapter->hal_func.EFUSEGetEfuseDefinition(pAdapter, efuseType, type, pOut, bPseudoTest); +} + + +/* 11/16/2008 MH Read one byte from real Efuse. */ +u8 +efuse_OneByteRead( + PADAPTER pAdapter, + u16 addr, + u8 *data, + BOOLEAN bPseudoTest) +{ + u32 tmpidx = 0; + u8 bResult; + u8 readbyte; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(pAdapter); + + /* RTW_INFO("===> EFUSE_OneByteRead(), addr = %x\n", addr); */ + /* RTW_INFO("===> EFUSE_OneByteRead() start, 0x34 = 0x%X\n", rtw_read32(pAdapter, EFUSE_TEST)); */ + + if (bPseudoTest) { + bResult = Efuse_Read1ByteFromFakeContent(pAdapter, addr, data); + return bResult; + } + +#ifdef CONFIG_RTL8710B + /* <20171208, Peter>, Dont do the following write16(0x34) */ + if (IS_HARDWARE_TYPE_8710B(pAdapter)) { + bResult = pAdapter->hal_func.efuse_indirect_read4(pAdapter, addr, data); + return bResult; + } +#endif + + if (IS_HARDWARE_TYPE_8723B(pAdapter) || + (IS_HARDWARE_TYPE_8192E(pAdapter) && (!IS_A_CUT(pHalData->version_id))) || + (IS_VENDOR_8188E_I_CUT_SERIES(pAdapter)) || (IS_CHIP_VENDOR_SMIC(pHalData->version_id)) + ) { + /* <20130121, Kordan> For SMIC EFUSE specificatoin. */ + /* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */ + /* phy_set_mac_reg(pAdapter, 0x34, BIT11, 0); */ + rtw_write16(pAdapter, 0x34, rtw_read16(pAdapter, 0x34) & (~BIT11)); + } + + /* -----------------e-fuse reg ctrl --------------------------------- */ + /* address */ + rtw_write8(pAdapter, EFUSE_CTRL + 1, (u8)(addr & 0xff)); + rtw_write8(pAdapter, EFUSE_CTRL + 2, ((u8)((addr >> 8) & 0x03)) | + (rtw_read8(pAdapter, EFUSE_CTRL + 2) & 0xFC)); + + /* rtw_write8(pAdapter, EFUSE_CTRL+3, 0x72); */ /* read cmd */ + /* Write bit 32 0 */ + readbyte = rtw_read8(pAdapter, EFUSE_CTRL + 3); + rtw_write8(pAdapter, EFUSE_CTRL + 3, (readbyte & 0x7f)); + + while (!(0x80 & rtw_read8(pAdapter, EFUSE_CTRL + 3)) && (tmpidx < 1000)) { + rtw_mdelay_os(1); + tmpidx++; + } + if (tmpidx < 100) { + *data = rtw_read8(pAdapter, EFUSE_CTRL); + bResult = _TRUE; + } else { + *data = 0xff; + bResult = _FALSE; + RTW_INFO("%s: [ERROR] addr=0x%x bResult=%d time out 1s !!!\n", __FUNCTION__, addr, bResult); + RTW_INFO("%s: [ERROR] EFUSE_CTRL =0x%08x !!!\n", __FUNCTION__, rtw_read32(pAdapter, EFUSE_CTRL)); + } + + return bResult; +} + +/* 11/16/2008 MH Write one byte to reald Efuse. */ +u8 +efuse_OneByteWrite( + PADAPTER pAdapter, + u16 addr, + u8 data, + BOOLEAN bPseudoTest) +{ + u8 tmpidx = 0; + u8 bResult = _FALSE; + u32 efuseValue = 0; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(pAdapter); + + /* RTW_INFO("===> EFUSE_OneByteWrite(), addr = %x data=%x\n", addr, data); */ + /* RTW_INFO("===> EFUSE_OneByteWrite() start, 0x34 = 0x%X\n", rtw_read32(pAdapter, EFUSE_TEST)); */ + + if (bPseudoTest) { + bResult = Efuse_Write1ByteToFakeContent(pAdapter, addr, data); + return bResult; + } + + Efuse_PowerSwitch(pAdapter, _TRUE, _TRUE); + + /* -----------------e-fuse reg ctrl --------------------------------- */ + /* address */ + + + efuseValue = rtw_read32(pAdapter, EFUSE_CTRL); + efuseValue |= (BIT21 | BIT31); + efuseValue &= ~(0x3FFFF); + efuseValue |= ((addr << 8 | data) & 0x3FFFF); + + /* <20130227, Kordan> 8192E MP chip A-cut had better not set 0x34[11] until B-Cut. */ + if (IS_HARDWARE_TYPE_8723B(pAdapter) || + (IS_HARDWARE_TYPE_8192E(pAdapter) && (!IS_A_CUT(pHalData->version_id))) || + (IS_VENDOR_8188E_I_CUT_SERIES(pAdapter)) || (IS_CHIP_VENDOR_SMIC(pHalData->version_id)) + ) { + /* <20130121, Kordan> For SMIC EFUSE specificatoin. */ + /* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */ + /* phy_set_mac_reg(pAdapter, 0x34, BIT11, 1); */ + rtw_write16(pAdapter, 0x34, rtw_read16(pAdapter, 0x34) | (BIT11)); + rtw_write32(pAdapter, EFUSE_CTRL, 0x90600000 | ((addr << 8 | data))); + } else + rtw_write32(pAdapter, EFUSE_CTRL, efuseValue); + + rtw_mdelay_os(1); + + while ((0x80 & rtw_read8(pAdapter, EFUSE_CTRL + 3)) && (tmpidx < 100)) { + rtw_mdelay_os(1); + tmpidx++; + } + + if (tmpidx < 100) + bResult = _TRUE; + else { + bResult = _FALSE; + RTW_INFO("%s: [ERROR] addr=0x%x ,efuseValue=0x%x ,bResult=%d time out 1s !!!\n", + __FUNCTION__, addr, efuseValue, bResult); + RTW_INFO("%s: [ERROR] EFUSE_CTRL =0x%08x !!!\n", __FUNCTION__, rtw_read32(pAdapter, EFUSE_CTRL)); + } + + /* disable Efuse program enable */ + if (IS_HARDWARE_TYPE_8723B(pAdapter) || + (IS_HARDWARE_TYPE_8192E(pAdapter) && (!IS_A_CUT(pHalData->version_id))) || + (IS_VENDOR_8188E_I_CUT_SERIES(pAdapter)) || (IS_CHIP_VENDOR_SMIC(pHalData->version_id)) + ) + phy_set_mac_reg(pAdapter, EFUSE_TEST, BIT(11), 0); + + Efuse_PowerSwitch(pAdapter, _TRUE, _FALSE); + + return bResult; +} + +int +Efuse_PgPacketRead(PADAPTER pAdapter, + u8 offset, + u8 *data, + BOOLEAN bPseudoTest) +{ + int ret = 0; + + ret = pAdapter->hal_func.Efuse_PgPacketRead(pAdapter, offset, data, bPseudoTest); + + return ret; +} + +int +Efuse_PgPacketWrite(PADAPTER pAdapter, + u8 offset, + u8 word_en, + u8 *data, + BOOLEAN bPseudoTest) +{ + int ret; + + ret = pAdapter->hal_func.Efuse_PgPacketWrite(pAdapter, offset, word_en, data, bPseudoTest); + + return ret; +} + + +int +Efuse_PgPacketWrite_BT(PADAPTER pAdapter, + u8 offset, + u8 word_en, + u8 *data, + BOOLEAN bPseudoTest) +{ + int ret; + + ret = pAdapter->hal_func.Efuse_PgPacketWrite_BT(pAdapter, offset, word_en, data, bPseudoTest); + + return ret; +} + + +u8 +Efuse_WordEnableDataWrite(PADAPTER pAdapter, + u16 efuse_addr, + u8 word_en, + u8 *data, + BOOLEAN bPseudoTest) +{ + u8 ret = 0; + + ret = pAdapter->hal_func.Efuse_WordEnableDataWrite(pAdapter, efuse_addr, word_en, data, bPseudoTest); + + return ret; +} + +static u8 efuse_read8(PADAPTER padapter, u16 address, u8 *value) +{ + return efuse_OneByteRead(padapter, address, value, _FALSE); +} + +static u8 efuse_write8(PADAPTER padapter, u16 address, u8 *value) +{ + return efuse_OneByteWrite(padapter, address, *value, _FALSE); +} + +/* + * read/wirte raw efuse data + */ +u8 rtw_efuse_access(PADAPTER padapter, u8 bWrite, u16 start_addr, u16 cnts, u8 *data) +{ + int i = 0; + u16 real_content_len = 0, max_available_size = 0; + u8 res = _FAIL ; + u8(*rw8)(PADAPTER, u16, u8 *); + u32 backupRegs[4] = {0}; + + + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&real_content_len, _FALSE); + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_available_size, _FALSE); + + if (start_addr > real_content_len) + return _FAIL; + + if (_TRUE == bWrite) { + if ((start_addr + cnts) > max_available_size) + return _FAIL; + rw8 = &efuse_write8; + } else + rw8 = &efuse_read8; + + efuse_PreUpdateAction(padapter, backupRegs); + + Efuse_PowerSwitch(padapter, bWrite, _TRUE); + + /* e-fuse one byte read / write */ + for (i = 0; i < cnts; i++) { + if (start_addr >= real_content_len) { + res = _FAIL; + break; + } + + res = rw8(padapter, start_addr++, data++); + if (_FAIL == res) + break; + } + + Efuse_PowerSwitch(padapter, bWrite, _FALSE); + + efuse_PostUpdateAction(padapter, backupRegs); + + return res; +} +/* ------------------------------------------------------------------------------ */ +u16 efuse_GetMaxSize(PADAPTER padapter) +{ + u16 max_size; + + max_size = 0; + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI , TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_size, _FALSE); + return max_size; +} +/* ------------------------------------------------------------------------------ */ +u8 efuse_GetCurrentSize(PADAPTER padapter, u16 *size) +{ + Efuse_PowerSwitch(padapter, _FALSE, _TRUE); + *size = Efuse_GetCurrentSize(padapter, EFUSE_WIFI, _FALSE); + Efuse_PowerSwitch(padapter, _FALSE, _FALSE); + + return _SUCCESS; +} +/* ------------------------------------------------------------------------------ */ +u16 efuse_bt_GetMaxSize(PADAPTER padapter) +{ + u16 max_size; + + max_size = 0; + EFUSE_GetEfuseDefinition(padapter, EFUSE_BT , TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_size, _FALSE); + return max_size; +} + +u8 efuse_bt_GetCurrentSize(PADAPTER padapter, u16 *size) +{ + Efuse_PowerSwitch(padapter, _FALSE, _TRUE); + *size = Efuse_GetCurrentSize(padapter, EFUSE_BT, _FALSE); + Efuse_PowerSwitch(padapter, _FALSE, _FALSE); + + return _SUCCESS; +} + +u8 rtw_efuse_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, _FALSE); + + if ((addr + cnts) > mapLen) + return _FAIL; + + Efuse_PowerSwitch(padapter, _FALSE, _TRUE); + + efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data, _FALSE); + + Efuse_PowerSwitch(padapter, _FALSE, _FALSE); + + return _SUCCESS; +} + +u8 rtw_BT_efuse_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, _FALSE); + + if ((addr + cnts) > mapLen) + return _FAIL; + + Efuse_PowerSwitch(padapter, _FALSE, _TRUE); + + efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data, _FALSE); + + Efuse_PowerSwitch(padapter, _FALSE, _FALSE); + + return _SUCCESS; +} + +/* ------------------------------------------------------------------------------ */ +u8 rtw_efuse_map_write(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ +#define RT_ASSERT_RET(expr) \ + if (!(expr)) { \ + printk("Assertion failed! %s at ......\n", #expr); \ + printk(" ......%s,%s, line=%d\n",__FILE__, __FUNCTION__, __LINE__); \ + return _FAIL; \ + } + + u8 *efuse = NULL; + u8 offset, word_en; + u8 *map = NULL; + u8 newdata[PGPKT_DATA_SIZE]; + s32 i, j, idx, chk_total_byte; + u8 ret = _SUCCESS; + u16 mapLen = 0, startAddr = 0, efuse_max_available_len = 0; + u32 backupRegs[4] = {0}; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal; + + + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, _FALSE); + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &efuse_max_available_len, _FALSE); + + if ((addr + cnts) > mapLen) + return _FAIL; + + RT_ASSERT_RET(PGPKT_DATA_SIZE == 8); /* have to be 8 byte alignment */ + RT_ASSERT_RET((mapLen & 0x7) == 0); /* have to be PGPKT_DATA_SIZE alignment for memcpy */ + + efuse = rtw_zmalloc(mapLen); + if (!efuse) + return _FAIL; + + map = rtw_zmalloc(mapLen); + if (map == NULL) { + rtw_mfree(efuse, mapLen); + return _FAIL; + } + + _rtw_memset(map, 0xFF, mapLen); + + ret = rtw_efuse_map_read(padapter, 0, mapLen, map); + if (ret == _FAIL) + goto exit; + + _rtw_memcpy(efuse , map, mapLen); + _rtw_memcpy(efuse + addr, data, cnts); + + if (padapter->registrypriv.boffefusemask == 0) { + for (i = 0; i < cnts; i++) { + if (padapter->registrypriv.bFileMaskEfuse == _TRUE) { + if (rtw_file_efuse_IsMasked(padapter, addr + i, maskfileBuffer)) /*use file efuse mask. */ + efuse[addr + i] = map[addr + i]; + } else { + if (efuse_IsMasked(padapter, addr + i)) + efuse[addr + i] = map[addr + i]; + } + RTW_INFO("%s , data[%d] = %x, map[addr+i]= %x\n", __func__, addr + i, efuse[ addr + i], map[addr + i]); + } + } + /*Efuse_PowerSwitch(padapter, _TRUE, _TRUE);*/ + + chk_total_byte = 0; + idx = 0; + offset = (addr >> 3); + + while (idx < cnts) { + word_en = 0xF; + j = (addr + idx) & 0x7; + for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) { + if (efuse[addr + idx] != map[addr + idx]) + word_en &= ~BIT(i >> 1); + } + + if (word_en != 0xF) { + chk_total_byte += Efuse_CalculateWordCnts(word_en) * 2; + + if (offset >= EFUSE_MAX_SECTION_BASE) /* Over EFUSE_MAX_SECTION 16 for 2 ByteHeader */ + chk_total_byte += 2; + else + chk_total_byte += 1; + } + + offset++; + } + + RTW_INFO("Total PG bytes Count = %d\n", chk_total_byte); + rtw_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&startAddr); + + if (startAddr == 0) { + startAddr = Efuse_GetCurrentSize(padapter, EFUSE_WIFI, _FALSE); + RTW_INFO("%s: Efuse_GetCurrentSize startAddr=%#X\n", __func__, startAddr); + } + RTW_DBG("%s: startAddr=%#X\n", __func__, startAddr); + + if ((startAddr + chk_total_byte) >= efuse_max_available_len) { + RTW_INFO("%s: startAddr(0x%X) + PG data len %d >= efuse_max_available_len(0x%X)\n", + __func__, startAddr, chk_total_byte, efuse_max_available_len); + ret = _FAIL; + goto exit; + } + + efuse_PreUpdateAction(padapter, backupRegs); + + idx = 0; + offset = (addr >> 3); + while (idx < cnts) { + word_en = 0xF; + j = (addr + idx) & 0x7; + _rtw_memcpy(newdata, &map[offset << 3], PGPKT_DATA_SIZE); + for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) { + if (efuse[addr + idx] != map[addr + idx]) { + word_en &= ~BIT(i >> 1); + newdata[i] = efuse[addr + idx]; +#ifdef CONFIG_RTL8723B + if (addr + idx == 0x8) { + if (IS_C_CUT(pHalData->version_id) || IS_B_CUT(pHalData->version_id)) { + if (pHalData->adjuseVoltageVal == 6) { + newdata[i] = map[addr + idx]; + RTW_INFO(" %s ,\n adjuseVoltageVal = %d ,newdata[%d] = %x\n", __func__, pHalData->adjuseVoltageVal, i, newdata[i]); + } + } + } +#endif + } + } + + if (word_en != 0xF) { + ret = Efuse_PgPacketWrite(padapter, offset, word_en, newdata, _FALSE); + RTW_INFO("offset=%x\n", offset); + RTW_INFO("word_en=%x\n", word_en); + + for (i = 0; i < PGPKT_DATA_SIZE; i++) + RTW_INFO("data=%x \t", newdata[i]); + if (ret == _FAIL) + break; + } + + offset++; + } + + /*Efuse_PowerSwitch(padapter, _TRUE, _FALSE);*/ + + efuse_PostUpdateAction(padapter, backupRegs); + +exit: + + rtw_mfree(map, mapLen); + rtw_mfree(efuse, mapLen); + + return ret; +} + + +u8 rtw_BT_efuse_map_write(PADAPTER padapter, u16 addr, u16 cnts, u8 *data) +{ +#define RT_ASSERT_RET(expr) \ + if (!(expr)) { \ + printk("Assertion failed! %s at ......\n", #expr); \ + printk(" ......%s,%s, line=%d\n",__FILE__, __FUNCTION__, __LINE__); \ + return _FAIL; \ + } + + u8 offset, word_en; + u8 *map; + u8 newdata[PGPKT_DATA_SIZE]; + s32 i = 0, j = 0, idx; + u8 ret = _SUCCESS; + u16 mapLen = 0; + + EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, _FALSE); + + if ((addr + cnts) > mapLen) + return _FAIL; + + RT_ASSERT_RET(PGPKT_DATA_SIZE == 8); /* have to be 8 byte alignment */ + RT_ASSERT_RET((mapLen & 0x7) == 0); /* have to be PGPKT_DATA_SIZE alignment for memcpy */ + + map = rtw_zmalloc(mapLen); + if (map == NULL) + return _FAIL; + + ret = rtw_BT_efuse_map_read(padapter, 0, mapLen, map); + if (ret == _FAIL) + goto exit; + RTW_INFO("OFFSET\tVALUE(hex)\n"); + for (i = 0; i < 1024; i += 16) { /* set 512 because the iwpriv's extra size have limit 0x7FF */ + RTW_INFO("0x%03x\t", i); + for (j = 0; j < 8; j++) + RTW_INFO("%02X ", map[i + j]); + RTW_INFO("\t"); + for (; j < 16; j++) + RTW_INFO("%02X ", map[i + j]); + RTW_INFO("\n"); + } + RTW_INFO("\n"); + Efuse_PowerSwitch(padapter, _TRUE, _TRUE); + + idx = 0; + offset = (addr >> 3); + while (idx < cnts) { + word_en = 0xF; + j = (addr + idx) & 0x7; + _rtw_memcpy(newdata, &map[offset << 3], PGPKT_DATA_SIZE); + for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) { + if (data[idx] != map[addr + idx]) { + word_en &= ~BIT(i >> 1); + newdata[i] = data[idx]; + } + } + + if (word_en != 0xF) { + RTW_INFO("offset=%x\n", offset); + RTW_INFO("word_en=%x\n", word_en); + RTW_INFO("%s: data=", __FUNCTION__); + for (i = 0; i < PGPKT_DATA_SIZE; i++) + RTW_INFO("0x%02X ", newdata[i]); + RTW_INFO("\n"); + ret = Efuse_PgPacketWrite_BT(padapter, offset, word_en, newdata, _FALSE); + if (ret == _FAIL) + break; + } + + offset++; + } + + Efuse_PowerSwitch(padapter, _TRUE, _FALSE); + +exit: + + rtw_mfree(map, mapLen); + + return ret; +} + +/*----------------------------------------------------------------------------- + * Function: Efuse_ReadAllMap + * + * Overview: Read All Efuse content + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/11/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void +Efuse_ReadAllMap( + PADAPTER pAdapter, + u8 efuseType, + u8 *Efuse, + BOOLEAN bPseudoTest); +void +Efuse_ReadAllMap( + PADAPTER pAdapter, + u8 efuseType, + u8 *Efuse, + BOOLEAN bPseudoTest) +{ + u16 mapLen = 0; + + Efuse_PowerSwitch(pAdapter, _FALSE, _TRUE); + + EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest); + + efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse, bPseudoTest); + + Efuse_PowerSwitch(pAdapter, _FALSE, _FALSE); +} + +/*----------------------------------------------------------------------------- + * Function: efuse_ShadowWrite1Byte + * efuse_ShadowWrite2Byte + * efuse_ShadowWrite4Byte + * + * Overview: Write efuse modify map by one/two/four byte. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/12/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +#ifdef PLATFORM +static void +efuse_ShadowWrite1Byte( + PADAPTER pAdapter, + u16 Offset, + u8 Value); +#endif /* PLATFORM */ +static void +efuse_ShadowWrite1Byte( + PADAPTER pAdapter, + u16 Offset, + u8 Value) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + pHalData->efuse_eeprom_data[Offset] = Value; + +} /* efuse_ShadowWrite1Byte */ + +/* ---------------Write Two Bytes */ +static void +efuse_ShadowWrite2Byte( + PADAPTER pAdapter, + u16 Offset, + u16 Value) +{ + + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + + pHalData->efuse_eeprom_data[Offset] = Value & 0x00FF; + pHalData->efuse_eeprom_data[Offset + 1] = Value >> 8; + +} /* efuse_ShadowWrite1Byte */ + +/* ---------------Write Four Bytes */ +static void +efuse_ShadowWrite4Byte( + PADAPTER pAdapter, + u16 Offset, + u32 Value) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + pHalData->efuse_eeprom_data[Offset] = (u8)(Value & 0x000000FF); + pHalData->efuse_eeprom_data[Offset + 1] = (u8)((Value >> 8) & 0x0000FF); + pHalData->efuse_eeprom_data[Offset + 2] = (u8)((Value >> 16) & 0x00FF); + pHalData->efuse_eeprom_data[Offset + 3] = (u8)((Value >> 24) & 0xFF); + +} /* efuse_ShadowWrite1Byte */ + + +/*----------------------------------------------------------------------------- + * Function: EFUSE_ShadowWrite + * + * Overview: Write efuse modify map for later update operation to use!!!!! + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/12/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void +EFUSE_ShadowWrite( + PADAPTER pAdapter, + u8 Type, + u16 Offset, + u32 Value); +void +EFUSE_ShadowWrite( + PADAPTER pAdapter, + u8 Type, + u16 Offset, + u32 Value) +{ +#if (MP_DRIVER == 0) + return; +#endif + if (pAdapter->registrypriv.mp_mode == 0) + return; + + + if (Type == 1) + efuse_ShadowWrite1Byte(pAdapter, Offset, (u8)Value); + else if (Type == 2) + efuse_ShadowWrite2Byte(pAdapter, Offset, (u16)Value); + else if (Type == 4) + efuse_ShadowWrite4Byte(pAdapter, Offset, (u32)Value); + +} /* EFUSE_ShadowWrite */ + +#endif /* !RTW_HALMAC */ +/*----------------------------------------------------------------------------- + * Function: efuse_ShadowRead1Byte + * efuse_ShadowRead2Byte + * efuse_ShadowRead4Byte + * + * Overview: Read from efuse init map by one/two/four bytes !!!!! + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/12/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +static void +efuse_ShadowRead1Byte( + PADAPTER pAdapter, + u16 Offset, + u8 *Value) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + *Value = pHalData->efuse_eeprom_data[Offset]; + +} /* EFUSE_ShadowRead1Byte */ + +/* ---------------Read Two Bytes */ +static void +efuse_ShadowRead2Byte( + PADAPTER pAdapter, + u16 Offset, + u16 *Value) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + *Value = pHalData->efuse_eeprom_data[Offset]; + *Value |= pHalData->efuse_eeprom_data[Offset + 1] << 8; + +} /* EFUSE_ShadowRead2Byte */ + +/* ---------------Read Four Bytes */ +static void +efuse_ShadowRead4Byte( + PADAPTER pAdapter, + u16 Offset, + u32 *Value) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + + *Value = pHalData->efuse_eeprom_data[Offset]; + *Value |= pHalData->efuse_eeprom_data[Offset + 1] << 8; + *Value |= pHalData->efuse_eeprom_data[Offset + 2] << 16; + *Value |= pHalData->efuse_eeprom_data[Offset + 3] << 24; + +} /* efuse_ShadowRead4Byte */ + +/*----------------------------------------------------------------------------- + * Function: EFUSE_ShadowRead + * + * Overview: Read from pHalData->efuse_eeprom_data + *---------------------------------------------------------------------------*/ +void +EFUSE_ShadowRead( + PADAPTER pAdapter, + u8 Type, + u16 Offset, + u32 *Value) +{ + if (Type == 1) + efuse_ShadowRead1Byte(pAdapter, Offset, (u8 *)Value); + else if (Type == 2) + efuse_ShadowRead2Byte(pAdapter, Offset, (u16 *)Value); + else if (Type == 4) + efuse_ShadowRead4Byte(pAdapter, Offset, (u32 *)Value); + +} /* EFUSE_ShadowRead */ + +/* 11/16/2008 MH Add description. Get current efuse area enabled word!!. */ +u8 +Efuse_CalculateWordCnts(u8 word_en) +{ + u8 word_cnts = 0; + if (!(word_en & BIT(0))) + word_cnts++; /* 0 : write enable */ + if (!(word_en & BIT(1))) + word_cnts++; + if (!(word_en & BIT(2))) + word_cnts++; + if (!(word_en & BIT(3))) + word_cnts++; + return word_cnts; +} + +/*----------------------------------------------------------------------------- + * Function: efuse_WordEnableDataRead + * + * Overview: Read allowed word in current efuse section data. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/16/2008 MHC Create Version 0. + * 11/21/2008 MHC Fix Write bug when we only enable late word. + * + *---------------------------------------------------------------------------*/ +void +efuse_WordEnableDataRead(u8 word_en, + u8 *sourdata, + u8 *targetdata) +{ + if (!(word_en & BIT(0))) { + targetdata[0] = sourdata[0]; + targetdata[1] = sourdata[1]; + } + if (!(word_en & BIT(1))) { + targetdata[2] = sourdata[2]; + targetdata[3] = sourdata[3]; + } + if (!(word_en & BIT(2))) { + targetdata[4] = sourdata[4]; + targetdata[5] = sourdata[5]; + } + if (!(word_en & BIT(3))) { + targetdata[6] = sourdata[6]; + targetdata[7] = sourdata[7]; + } +} + +/*----------------------------------------------------------------------------- + * Function: EFUSE_ShadowMapUpdate + * + * Overview: Transfer current EFUSE content to shadow init and modify map. + * + * Input: NONE + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 11/13/2008 MHC Create Version 0. + * + *---------------------------------------------------------------------------*/ +void EFUSE_ShadowMapUpdate( + PADAPTER pAdapter, + u8 efuseType, + BOOLEAN bPseudoTest) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + u16 mapLen = 0; +#ifdef RTW_HALMAC + u8 *efuse_map = NULL; + int err; + + + mapLen = EEPROM_MAX_SIZE; + efuse_map = pHalData->efuse_eeprom_data; + /* efuse default content is 0xFF */ + _rtw_memset(efuse_map, 0xFF, EEPROM_MAX_SIZE); + + EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest); + if (!mapLen) { + RTW_WARN("%s: fail to get efuse size!\n", __FUNCTION__); + mapLen = EEPROM_MAX_SIZE; + } + if (mapLen > EEPROM_MAX_SIZE) { + RTW_WARN("%s: size of efuse data(%d) is large than expected(%d)!\n", + __FUNCTION__, mapLen, EEPROM_MAX_SIZE); + mapLen = EEPROM_MAX_SIZE; + } + + if (pHalData->bautoload_fail_flag == _FALSE) { + err = rtw_halmac_read_logical_efuse_map(adapter_to_dvobj(pAdapter), efuse_map, mapLen, NULL, 0); + if (err) + RTW_ERR("%s: fail to get efuse map!\n", __FUNCTION__); + } +#else /* !RTW_HALMAC */ + EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest); + + if (pHalData->bautoload_fail_flag == _TRUE) + _rtw_memset(pHalData->efuse_eeprom_data, 0xFF, mapLen); + else { +#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE + if (_SUCCESS != retriveAdaptorInfoFile(pAdapter->registrypriv.adaptor_info_caching_file_path, pHalData->efuse_eeprom_data)) { +#endif + + Efuse_ReadAllMap(pAdapter, efuseType, pHalData->efuse_eeprom_data, bPseudoTest); + +#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE + storeAdaptorInfoFile(pAdapter->registrypriv.adaptor_info_caching_file_path, pHalData->efuse_eeprom_data); + } +#endif + } + + /* PlatformMoveMemory((void *)&pHalData->EfuseMap[EFUSE_MODIFY_MAP][0], */ + /* (void *)&pHalData->EfuseMap[EFUSE_INIT_MAP][0], mapLen); */ +#endif /* !RTW_HALMAC */ +#ifdef CONFIG_MP_INCLUDED + if (rtw_mp_mode_check(pAdapter)) { + PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal; + + if (GET_EFUSE_UPDATE_ON(pAdapter)) + _rtw_memcpy(pHalData->efuse_eeprom_data, pEfuseHal->fakeEfuseModifiedMap, mapLen); + } +#endif + rtw_mask_map_read(pAdapter, 0x00, mapLen, pHalData->efuse_eeprom_data); + + rtw_dump_cur_efuse(pAdapter); +} /* EFUSE_ShadowMapUpdate */ + +const u8 _mac_hidden_max_bw_to_hal_bw_cap[MAC_HIDDEN_MAX_BW_NUM] = { + 0, + 0, + (BW_CAP_160M | BW_CAP_80M | BW_CAP_40M | BW_CAP_20M | BW_CAP_10M | BW_CAP_5M), + (BW_CAP_5M), + (BW_CAP_10M | BW_CAP_5M), + (BW_CAP_20M | BW_CAP_10M | BW_CAP_5M), + (BW_CAP_40M | BW_CAP_20M | BW_CAP_10M | BW_CAP_5M), + (BW_CAP_80M | BW_CAP_40M | BW_CAP_20M | BW_CAP_10M | BW_CAP_5M), +}; + +const u8 _mac_hidden_proto_to_hal_proto_cap[MAC_HIDDEN_PROTOCOL_NUM] = { + 0, + 0, + (PROTO_CAP_11N | PROTO_CAP_11G | PROTO_CAP_11B), + (PROTO_CAP_11AC | PROTO_CAP_11N | PROTO_CAP_11G | PROTO_CAP_11B), +}; + +u8 mac_hidden_wl_func_to_hal_wl_func(u8 func) +{ + u8 wl_func = 0; + + if (func & BIT0) + wl_func |= WL_FUNC_MIRACAST; + if (func & BIT1) + wl_func |= WL_FUNC_P2P; + if (func & BIT2) + wl_func |= WL_FUNC_TDLS; + if (func & BIT3) + wl_func |= WL_FUNC_FTM; + + return wl_func; +} + +#ifdef PLATFORM_LINUX +#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE +/* #include */ + +int isAdaptorInfoFileValid(void) +{ + return _TRUE; +} + +int storeAdaptorInfoFile(char *path, u8 *efuse_data) +{ + int ret = _SUCCESS; + + if (path && efuse_data) { + ret = rtw_store_to_file(path, efuse_data, EEPROM_MAX_SIZE_512); + if (ret == EEPROM_MAX_SIZE) + ret = _SUCCESS; + else + ret = _FAIL; + } else { + RTW_INFO("%s NULL pointer\n", __FUNCTION__); + ret = _FAIL; + } + return ret; +} + +int retriveAdaptorInfoFile(char *path, u8 *efuse_data) +{ + int ret = _SUCCESS; + mm_segment_t oldfs; + struct file *fp; + + if (path && efuse_data) { + + ret = rtw_retrieve_from_file(path, efuse_data, EEPROM_MAX_SIZE); + + if (ret == EEPROM_MAX_SIZE) + ret = _SUCCESS; + else + ret = _FAIL; + +#if 0 + if (isAdaptorInfoFileValid()) + return 0; + else + return _FAIL; +#endif + + } else { + RTW_INFO("%s NULL pointer\n", __FUNCTION__); + ret = _FAIL; + } + return ret; +} +#endif /* CONFIG_ADAPTOR_INFO_CACHING_FILE */ + +u8 rtw_efuse_file_read(PADAPTER padapter, u8 *filepath, u8 *buf, u32 len) +{ + char *ptmpbuf = NULL, *ptr; + u8 val8; + u32 count, i, j; + int err; + u32 bufsize = 4096; + + ptmpbuf = rtw_zmalloc(bufsize); + if (ptmpbuf == NULL) + return _FALSE; + + count = rtw_retrieve_from_file(filepath, ptmpbuf, bufsize); + if (count <= 90) { + rtw_mfree(ptmpbuf, bufsize); + RTW_ERR("%s, filepatch %s, size=%d, FAIL!!\n", __FUNCTION__, filepath, count); + return _FALSE; + } + i = 0; + j = 0; + ptr = ptmpbuf; + while ((j < len) && (i < count)) { + if (ptmpbuf[i] == '\0') + break; + ptr = strpbrk(&ptmpbuf[i], " \t\n\r"); + if (ptr) { + if (ptr == &ptmpbuf[i]) { + i++; + continue; + } + /* Add string terminating null */ + *ptr = 0; + } else { + ptr = &ptmpbuf[count-1]; + } + + err = sscanf(&ptmpbuf[i], "%hhx", &val8); + if (err != 1) { + RTW_WARN("Something wrong to parse efuse file, string=%s\n", &ptmpbuf[i]); + } else { + buf[j] = val8; + RTW_DBG("i=%d, j=%d, 0x%02x\n", i, j, buf[j]); + j++; + } + i = ptr - ptmpbuf + 1; + } + rtw_mfree(ptmpbuf, bufsize); + RTW_INFO("%s, filepatch %s, size=%d, done\n", __FUNCTION__, filepath, count); + return _TRUE; +} + + +u8 rtw_efuse_file_store(PADAPTER padapter, u8 *filepath, u8 *buf, u32 len) +{ + int err = 0, i = 0, j = 0, mapLen = 0 ; + char *cbuf, *pchr; + + cbuf = rtw_zmalloc(len * 3); + pchr = cbuf; + + if (filepath && buf) { + if (cbuf == NULL) { + RTW_INFO("%s, malloc cbuf _FAIL\n", __FUNCTION__); + err = _FAIL; + } else { + for (i = 0; i <= len; i += 16) { + for (j = 0; j < 16; j++) + pchr += sprintf(pchr, "%02X ", buf[i + j]); + pchr += sprintf(pchr, "\n"); + } + + err = rtw_store_to_file(filepath, cbuf, strlen(cbuf)); + RTW_INFO("%s, rtw_store_to_file len=%d,err =%d, len(cbuf)=%zd\n", __FUNCTION__, len, err, strlen(cbuf)); + if (err == strlen(cbuf)) { + err = _SUCCESS; + RTW_INFO("%s, filepatch %s, len=%d, done\n", __FUNCTION__, filepath, len); + } else { + err = _FAIL; + RTW_INFO("%s, filepatch %s, len=%d,err =%d, _FAIL\n", __FUNCTION__, filepath, len, err); + } + } + } + if (cbuf) + rtw_mfree(cbuf, len * 3); + + return err; +} + +#ifdef CONFIG_EFUSE_CONFIG_FILE +u32 rtw_read_efuse_from_file(const char *path, u8 *buf, int map_size) +{ + u32 i; + u8 c; + u8 temp[3]; + u8 temp_i; + u8 end = _FALSE; + u32 ret = _FAIL; + + u8 *file_data = NULL; + u32 file_size, read_size, pos = 0; + u8 *map = NULL; + + if (rtw_is_file_readable_with_size(path, &file_size) != _TRUE) { + RTW_PRINT("%s %s is not readable\n", __func__, path); + goto exit; + } + + file_data = rtw_vmalloc(file_size); + if (!file_data) { + RTW_ERR("%s rtw_vmalloc(%d) fail\n", __func__, file_size); + goto exit; + } + + read_size = rtw_retrieve_from_file(path, file_data, file_size); + if (read_size == 0) { + RTW_ERR("%s read from %s fail\n", __func__, path); + goto exit; + } + + map = rtw_vmalloc(map_size); + if (!map) { + RTW_ERR("%s rtw_vmalloc(%d) fail\n", __func__, map_size); + goto exit; + } + _rtw_memset(map, 0xff, map_size); + + temp[2] = 0; /* end of string '\0' */ + + for (i = 0 ; i < map_size ; i++) { + temp_i = 0; + + while (1) { + if (pos >= read_size) { + end = _TRUE; + break; + } + c = file_data[pos++]; + + /* bypass spece or eol or null before first hex digit */ + if (temp_i == 0 && (is_eol(c) == _TRUE || is_space(c) == _TRUE || is_null(c) == _TRUE)) + continue; + + if (IsHexDigit(c) == _FALSE) { + RTW_ERR("%s invalid 8-bit hex format for offset:0x%03x\n", __func__, i); + goto exit; + } + + temp[temp_i++] = c; + + if (temp_i == 2) { + /* parse value */ + if (sscanf(temp, "%hhx", &map[i]) != 1) { + RTW_ERR("%s sscanf fail for offset:0x%03x\n", __func__, i); + goto exit; + } + break; + } + } + + if (end == _TRUE) { + if (temp_i != 0) { + RTW_ERR("%s incomplete 8-bit hex format for offset:0x%03x\n", __func__, i); + goto exit; + } + break; + } + } + + RTW_PRINT("efuse file:%s, 0x%03x byte content read\n", path, i); + + _rtw_memcpy(buf, map, map_size); + + ret = _SUCCESS; + +exit: + if (file_data) + rtw_vmfree(file_data, file_size); + if (map) + rtw_vmfree(map, map_size); + + return ret; +} + +u32 rtw_read_macaddr_from_file(const char *path, u8 *buf) +{ + u32 i; + u8 temp[3]; + u32 ret = _FAIL; + + u8 file_data[17]; + u32 read_size; + u8 addr[ETH_ALEN]; + + if (rtw_is_file_readable(path) != _TRUE) { + RTW_PRINT("%s %s is not readable\n", __func__, path); + goto exit; + } + + read_size = rtw_retrieve_from_file(path, file_data, 17); + if (read_size != 17) { + RTW_ERR("%s read from %s fail\n", __func__, path); + goto exit; + } + + temp[2] = 0; /* end of string '\0' */ + + for (i = 0 ; i < ETH_ALEN ; i++) { + if (IsHexDigit(file_data[i * 3]) == _FALSE || IsHexDigit(file_data[i * 3 + 1]) == _FALSE) { + RTW_ERR("%s invalid 8-bit hex format for address offset:%u\n", __func__, i); + goto exit; + } + + if (i < ETH_ALEN - 1 && file_data[i * 3 + 2] != ':') { + RTW_ERR("%s invalid separator after address offset:%u\n", __func__, i); + goto exit; + } + + temp[0] = file_data[i * 3]; + temp[1] = file_data[i * 3 + 1]; + if (sscanf(temp, "%hhx", &addr[i]) != 1) { + RTW_ERR("%s sscanf fail for address offset:0x%03x\n", __func__, i); + goto exit; + } + } + + _rtw_memcpy(buf, addr, ETH_ALEN); + + RTW_PRINT("wifi_mac file: %s\n", path); +#ifdef CONFIG_RTW_DEBUG + RTW_INFO(MAC_FMT"\n", MAC_ARG(buf)); +#endif + + ret = _SUCCESS; + +exit: + return ret; +} +#endif /* CONFIG_EFUSE_CONFIG_FILE */ + +#endif /* PLATFORM_LINUX */ diff --git a/core/mesh/rtw_mesh.c b/core/mesh/rtw_mesh.c new file mode 100644 index 0000000..25c17cf --- /dev/null +++ b/core/mesh/rtw_mesh.c @@ -0,0 +1,4392 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_MESH_C_ + +#ifdef CONFIG_RTW_MESH +#include + +const char *_rtw_mesh_plink_str[] = { + "UNKNOWN", + "LISTEN", + "OPN_SNT", + "OPN_RCVD", + "CNF_RCVD", + "ESTAB", + "HOLDING", + "BLOCKED", +}; + +const char *_rtw_mesh_ps_str[] = { + "UNKNOWN", + "ACTIVE", + "LSLEEP", + "DSLEEP", +}; + +const char *_action_self_protected_str[] = { + "ACT_SELF_PROTECTED_RSVD", + "MESH_OPEN", + "MESH_CONF", + "MESH_CLOSE", + "MESH_GK_INFORM", + "MESH_GK_ACK", +}; + +inline u8 *rtw_set_ie_mesh_id(u8 *buf, u32 *buf_len, const char *mesh_id, u8 id_len) +{ + return rtw_set_ie(buf, WLAN_EID_MESH_ID, id_len, mesh_id, buf_len); +} + +inline u8 *rtw_set_ie_mesh_config(u8 *buf, u32 *buf_len + , u8 path_sel_proto, u8 path_sel_metric, u8 congest_ctl_mode, u8 sync_method, u8 auth_proto + , u8 num_of_peerings, bool cto_mgate, bool cto_as + , bool accept_peerings, bool mcca_sup, bool mcca_en, bool forwarding + , bool mbca_en, bool tbtt_adj, bool ps_level) +{ + + u8 conf[7] = {0}; + + SET_MESH_CONF_ELE_PATH_SEL_PROTO_ID(conf, path_sel_proto); + SET_MESH_CONF_ELE_PATH_SEL_METRIC_ID(conf, path_sel_metric); + SET_MESH_CONF_ELE_CONGEST_CTRL_MODE_ID(conf, congest_ctl_mode); + SET_MESH_CONF_ELE_SYNC_METHOD_ID(conf, sync_method); + SET_MESH_CONF_ELE_AUTH_PROTO_ID(conf, auth_proto); + + SET_MESH_CONF_ELE_CTO_MGATE(conf, cto_mgate); + SET_MESH_CONF_ELE_NUM_OF_PEERINGS(conf, num_of_peerings); + SET_MESH_CONF_ELE_CTO_AS(conf, cto_as); + + SET_MESH_CONF_ELE_ACCEPT_PEERINGS(conf, accept_peerings); + SET_MESH_CONF_ELE_MCCA_SUP(conf, mcca_sup); + SET_MESH_CONF_ELE_MCCA_EN(conf, mcca_en); + SET_MESH_CONF_ELE_FORWARDING(conf, forwarding); + SET_MESH_CONF_ELE_MBCA_EN(conf, mbca_en); + SET_MESH_CONF_ELE_TBTT_ADJ(conf, tbtt_adj); + SET_MESH_CONF_ELE_PS_LEVEL(conf, ps_level); + + return rtw_set_ie(buf, WLAN_EID_MESH_CONFIG, 7, conf, buf_len); +} + +inline u8 *rtw_set_ie_mpm(u8 *buf, u32 *buf_len + , u8 proto_id, u16 llid, u16 *plid, u16 *reason, u8 *chosen_pmk) +{ + u8 data[24] = {0}; + u8 *pos = data; + + RTW_PUT_LE16(pos, proto_id); + pos += 2; + + RTW_PUT_LE16(pos, llid); + pos += 2; + + if (plid) { + RTW_PUT_LE16(pos, *plid); + pos += 2; + } + + if (reason) { + RTW_PUT_LE16(pos, *reason); + pos += 2; + } + + if (chosen_pmk) { + _rtw_memcpy(pos, chosen_pmk, 16); + pos += 16; + } + + return rtw_set_ie(buf, WLAN_EID_MPM, pos - data, data, buf_len); +} + +bool rtw_bss_is_forwarding(WLAN_BSSID_EX *bss) +{ + u8 *ie; + int ie_len; + bool ret = 0; + + ie = rtw_get_ie(BSS_EX_TLV_IES(bss), WLAN_EID_MESH_CONFIG, &ie_len, + BSS_EX_TLV_IES_LEN(bss)); + if (!ie || ie_len != 7) + goto exit; + + ret = GET_MESH_CONF_ELE_FORWARDING(ie + 2); + +exit: + return ret; +} + +bool rtw_bss_is_cto_mgate(WLAN_BSSID_EX *bss) +{ + u8 *ie; + int ie_len; + bool ret = 0; + + ie = rtw_get_ie(BSS_EX_TLV_IES(bss), WLAN_EID_MESH_CONFIG, &ie_len, + BSS_EX_TLV_IES_LEN(bss)); + if (!ie || ie_len != 7) + goto exit; + + ret = GET_MESH_CONF_ELE_CTO_MGATE(ie + 2); + +exit: + return ret; +} + +int _rtw_bss_is_same_mbss(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b, u8 **a_mconf_ie_r, u8 **b_mconf_ie_r) +{ + int ret = 0; + u8 *a_mconf_ie, *b_mconf_ie; + sint a_mconf_ie_len, b_mconf_ie_len; + + if (a->InfrastructureMode != Ndis802_11_mesh) + goto exit; + a_mconf_ie = rtw_get_ie(BSS_EX_TLV_IES(a), WLAN_EID_MESH_CONFIG, &a_mconf_ie_len, BSS_EX_TLV_IES_LEN(a)); + if (!a_mconf_ie || a_mconf_ie_len != 7) + goto exit; + if (a_mconf_ie_r) + *a_mconf_ie_r = a_mconf_ie; + + if (b->InfrastructureMode != Ndis802_11_mesh) + goto exit; + b_mconf_ie = rtw_get_ie(BSS_EX_TLV_IES(b), WLAN_EID_MESH_CONFIG, &b_mconf_ie_len, BSS_EX_TLV_IES_LEN(b)); + if (!b_mconf_ie || b_mconf_ie_len != 7) + goto exit; + if (b_mconf_ie_r) + *b_mconf_ie_r = b_mconf_ie; + + if (a->mesh_id.SsidLength != b->mesh_id.SsidLength + || _rtw_memcmp(a->mesh_id.Ssid, b->mesh_id.Ssid, a->mesh_id.SsidLength) == _FALSE) + goto exit; + + if (_rtw_memcmp(a_mconf_ie + 2, b_mconf_ie + 2, 5) == _FALSE) + goto exit; + + ret = 1; + +exit: + return ret; +} + +int rtw_bss_is_same_mbss(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b) +{ + return _rtw_bss_is_same_mbss(a, b, NULL, NULL); +} + +int rtw_bss_is_candidate_mesh_peer(_adapter *adapter, WLAN_BSSID_EX *target, u8 ch, u8 add_peer) +{ + int ret = 0; + WLAN_BSSID_EX *self = &adapter->mlmepriv.cur_network.network; + u8 *s_mconf_ie, *t_mconf_ie; + u8 auth_pid; + int i, j; + + if (ch && self->Configuration.DSConfig != target->Configuration.DSConfig) + goto exit; + + if (!_rtw_bss_is_same_mbss(self, target, &s_mconf_ie, &t_mconf_ie)) + goto exit; + + if (add_peer) { + /* Accept additional mesh peerings */ + if (GET_MESH_CONF_ELE_ACCEPT_PEERINGS(t_mconf_ie + 2) == 0) + goto exit; + } + + /* BSSBasicRateSet */ + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + if (target->SupportedRates[i] == 0) + break; + if (target->SupportedRates[i] & 0x80) { + u8 match = 0; + + if (!ch) { + /* off-channel, check target with our hardcode capability */ + if (target->Configuration.DSConfig > 14) + match = rtw_is_basic_rate_ofdm(target->SupportedRates[i]); + else + match = rtw_is_basic_rate_mix(target->SupportedRates[i]); + } else { + for (j = 0; j < NDIS_802_11_LENGTH_RATES_EX; j++) { + if (self->SupportedRates[j] == 0) + break; + if (self->SupportedRates[j] == target->SupportedRates[i]) { + match = 1; + break; + } + } + } + if (!match) + goto exit; + } + } + + /* BSSBasicMCSSet */ + + + auth_pid = GET_MESH_CONF_ELE_AUTH_PROTO_ID(s_mconf_ie + 2); + if (auth_pid && auth_pid <= 2) { + struct security_priv *sec = &adapter->securitypriv; + u8 *rsn_ie; + int rsn_ie_len; + int group_cipher = 0, pairwise_cipher = 0, gmcs = 0; + u8 mfp_opt = MFP_NO; + + /* 802.1X connected to AS ? */ + + /* RSN */ + rsn_ie = rtw_get_wpa2_ie(BSS_EX_TLV_IES(target), &rsn_ie_len, BSS_EX_TLV_IES_LEN(target)); + if (!rsn_ie || rsn_ie_len == 0) + goto exit; + if (rtw_parse_wpa2_ie(rsn_ie, rsn_ie_len + 2, &group_cipher, &pairwise_cipher, &gmcs, NULL, &mfp_opt, NULL) != _SUCCESS) + goto exit; + if ((sec->mfp_opt == MFP_REQUIRED && mfp_opt < MFP_OPTIONAL) + || (mfp_opt == MFP_REQUIRED && sec->mfp_opt < MFP_OPTIONAL)) + goto exit; + if (!(sec->wpa2_group_cipher & group_cipher)) + goto exit; + if (!(sec->wpa2_pairwise_cipher & pairwise_cipher)) + goto exit; + #ifdef CONFIG_IEEE80211W + if ((sec->mfp_opt >= MFP_OPTIONAL && mfp_opt >= MFP_OPTIONAL) + && security_type_bip_to_gmcs(sec->dot11wCipher) != gmcs) + goto exit; + #endif + } + + ret = 1; + +exit: + return ret; +} + +void rtw_mesh_bss_peering_status(WLAN_BSSID_EX *bss, u8 *nop, u8 *accept) +{ + u8 *ie; + int ie_len; + + if (nop) + *nop = 0; + if (accept) + *accept = 0; + + ie = rtw_get_ie(BSS_EX_TLV_IES(bss), WLAN_EID_MESH_CONFIG, &ie_len, + BSS_EX_TLV_IES_LEN(bss)); + if (!ie || ie_len != 7) + goto exit; + + if (nop) + *nop = GET_MESH_CONF_ELE_NUM_OF_PEERINGS(ie + 2); + if (accept) + *accept = GET_MESH_CONF_ELE_ACCEPT_PEERINGS(ie + 2); + +exit: + return; +} + +#if CONFIG_RTW_MESH_ACNODE_PREVENT +void rtw_mesh_update_scanned_acnode_status(_adapter *adapter, struct wlan_network *scanned) +{ + bool acnode; + u8 nop, accept; + + rtw_mesh_bss_peering_status(&scanned->network, &nop, &accept); + + acnode = !nop && accept; + + if (acnode && scanned->acnode_stime == 0) { + scanned->acnode_stime = rtw_get_current_time(); + if (scanned->acnode_stime == 0) + scanned->acnode_stime++; + } else if (!acnode) { + scanned->acnode_stime = 0; + scanned->acnode_notify_etime = 0; + } +} + +bool rtw_mesh_scanned_is_acnode_confirmed(_adapter *adapter, struct wlan_network *scanned) +{ + return scanned->acnode_stime + && rtw_get_passing_time_ms(scanned->acnode_stime) + > adapter->mesh_cfg.peer_sel_policy.acnode_conf_timeout_ms; +} + +static bool rtw_mesh_scanned_is_acnode_allow_notify(_adapter *adapter, struct wlan_network *scanned) +{ + return scanned->acnode_notify_etime + && rtw_time_after(scanned->acnode_notify_etime, rtw_get_current_time()); +} + +bool rtw_mesh_acnode_prevent_allow_sacrifice(_adapter *adapter) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct sta_priv *stapriv = &adapter->stapriv; + bool allow = 0; + + if (!mcfg->peer_sel_policy.acnode_prevent + || mcfg->max_peer_links <= 1 + || stapriv->asoc_list_cnt < mcfg->max_peer_links) + goto exit; + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + if (rtw_mesh_cto_mgate_required(adapter)) + goto exit; +#endif + + allow = 1; + +exit: + return allow; +} + +static bool rtw_mesh_acnode_candidate_exist(_adapter *adapter) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct sta_priv *stapriv = &adapter->stapriv; + struct mlme_priv *mlme = &adapter->mlmepriv; + _queue *queue = &(mlme->scanned_queue); + _list *head, *list; + _irqL irqL; + struct wlan_network *scanned = NULL; + struct sta_info *sta = NULL; + bool need = 0; + + _enter_critical_bh(&(mlme->scanned_queue.lock), &irqL); + + head = get_list_head(queue); + list = get_next(head); + while (!rtw_end_of_queue_search(head, list)) { + scanned = LIST_CONTAINOR(list, struct wlan_network, list); + list = get_next(list); + + if (rtw_get_passing_time_ms(scanned->last_scanned) < mcfg->peer_sel_policy.scanr_exp_ms + && rtw_mesh_scanned_is_acnode_confirmed(adapter, scanned) + && (!mcfg->rssi_threshold || mcfg->rssi_threshold <= scanned->network.Rssi) + #if CONFIG_RTW_MACADDR_ACL + && rtw_access_ctrl(adapter, scanned->network.MacAddress) == _TRUE + #endif + && rtw_bss_is_candidate_mesh_peer(adapter, &scanned->network, 1, 1) + #if CONFIG_RTW_MESH_PEER_BLACKLIST + && !rtw_mesh_peer_blacklist_search(adapter, scanned->network.MacAddress) + #endif + #if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + && rtw_mesh_cto_mgate_network_filter(adapter, scanned) + #endif + ) { + need = 1; + break; + } + } + + _exit_critical_bh(&(mlme->scanned_queue.lock), &irqL); + + return need; +} + +static int rtw_mesh_acnode_prevent_sacrifice_chk(_adapter *adapter, struct sta_info **sac, struct sta_info *com) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + int updated = 0; + + /* + * TODO: compare next_hop reference cnt of forwarding info + * don't sacrifice working next_hop or choose sta with least cnt + */ + + if (*sac == NULL) { + updated = 1; + goto exit; + } + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + if (mcfg->peer_sel_policy.cto_mgate_require + && !mcfg->dot11MeshGateAnnouncementProtocol + ) { + if (IS_CTO_MGATE_CONF_TIMEOUT(com->plink)) { + if (!IS_CTO_MGATE_CONF_TIMEOUT((*sac)->plink)) { + /* blacklist > not blacklist */ + updated = 1; + goto exit; + } + } else if (!IS_CTO_MGATE_CONF_DISABLED(com->plink)) { + if (IS_CTO_MGATE_CONF_DISABLED((*sac)->plink)) { + /* confirming > disabled */ + updated = 1; + goto exit; + } + } + } +#endif + +exit: + if (updated) + *sac = com; + + return updated; +} + +struct sta_info *_rtw_mesh_acnode_prevent_pick_sacrifice(_adapter *adapter) +{ + struct sta_priv *stapriv = &adapter->stapriv; + _list *head, *list; + struct sta_info *sta, *sacrifice = NULL; + u8 nop; + + head = &stapriv->asoc_list; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + sta = LIST_CONTAINOR(list, struct sta_info, asoc_list); + list = get_next(list); + + if (!sta->plink || !sta->plink->scanned) { + rtw_warn_on(1); + continue; + } + + rtw_mesh_bss_peering_status(&sta->plink->scanned->network, &nop, NULL); + if (nop < 2) + continue; + + rtw_mesh_acnode_prevent_sacrifice_chk(adapter, &sacrifice, sta); + } + + return sacrifice; +} + +struct sta_info *rtw_mesh_acnode_prevent_pick_sacrifice(_adapter *adapter) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct sta_info *sacrifice = NULL; + + enter_critical_bh(&stapriv->asoc_list_lock); + + sacrifice = _rtw_mesh_acnode_prevent_pick_sacrifice(adapter); + + exit_critical_bh(&stapriv->asoc_list_lock); + + return sacrifice; +} + +static void rtw_mesh_acnode_rsvd_chk(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + u8 acnode_rsvd = 0; + + if (rtw_mesh_acnode_prevent_allow_sacrifice(adapter) + && rtw_mesh_acnode_prevent_pick_sacrifice(adapter) + && rtw_mesh_acnode_candidate_exist(adapter)) + acnode_rsvd = 1; + + if (plink_ctl->acnode_rsvd != acnode_rsvd) { + plink_ctl->acnode_rsvd = acnode_rsvd; + RTW_INFO(FUNC_ADPT_FMT" acnode_rsvd = %d\n", FUNC_ADPT_ARG(adapter), plink_ctl->acnode_rsvd); + update_beacon(adapter, WLAN_EID_MESH_CONFIG, NULL, 1, 0); + } +} + +static void rtw_mesh_acnode_set_notify_etime(_adapter *adapter, u8 *rframe_whdr) +{ + if (adapter->mesh_info.plink_ctl.acnode_rsvd) { + struct wlan_network *scanned = rtw_find_network(&adapter->mlmepriv.scanned_queue, get_addr2_ptr(rframe_whdr)); + + if (rtw_mesh_scanned_is_acnode_confirmed(adapter, scanned)) { + scanned->acnode_notify_etime = rtw_get_current_time() + + rtw_ms_to_systime(adapter->mesh_cfg.peer_sel_policy.acnode_notify_timeout_ms); + if (scanned->acnode_notify_etime == 0) + scanned->acnode_notify_etime++; + } + } +} + +void dump_mesh_acnode_prevent_settings(void *sel, _adapter *adapter) +{ + struct mesh_peer_sel_policy *peer_sel_policy = &adapter->mesh_cfg.peer_sel_policy; + + RTW_PRINT_SEL(sel, "%-6s %-12s %-14s\n" + , "enable", "conf_timeout", "nofity_timeout"); + RTW_PRINT_SEL(sel, "%6u %12u %14u\n" + , peer_sel_policy->acnode_prevent + , peer_sel_policy->acnode_conf_timeout_ms + , peer_sel_policy->acnode_notify_timeout_ms); +} +#endif /* CONFIG_RTW_MESH_ACNODE_PREVENT */ + +#if CONFIG_RTW_MESH_PEER_BLACKLIST +int rtw_mesh_peer_blacklist_add(_adapter *adapter, const u8 *addr) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + return rtw_blacklist_add(&plink_ctl->peer_blacklist, addr + , mcfg->peer_sel_policy.peer_blacklist_timeout_ms); +} + +int rtw_mesh_peer_blacklist_del(_adapter *adapter, const u8 *addr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + return rtw_blacklist_del(&plink_ctl->peer_blacklist, addr); +} + +int rtw_mesh_peer_blacklist_search(_adapter *adapter, const u8 *addr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + return rtw_blacklist_search(&plink_ctl->peer_blacklist, addr); +} + +void rtw_mesh_peer_blacklist_flush(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + rtw_blacklist_flush(&plink_ctl->peer_blacklist); +} + +void dump_mesh_peer_blacklist(void *sel, _adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + dump_blacklist(sel, &plink_ctl->peer_blacklist, "blacklist"); +} + +void dump_mesh_peer_blacklist_settings(void *sel, _adapter *adapter) +{ + struct mesh_peer_sel_policy *peer_sel_policy = &adapter->mesh_cfg.peer_sel_policy; + + RTW_PRINT_SEL(sel, "%-12s %-17s\n" + , "conf_timeout", "blacklist_timeout"); + RTW_PRINT_SEL(sel, "%12u %17u\n" + , peer_sel_policy->peer_conf_timeout_ms + , peer_sel_policy->peer_blacklist_timeout_ms); +} +#endif /* CONFIG_RTW_MESH_PEER_BLACKLIST */ + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST +u8 rtw_mesh_cto_mgate_required(_adapter *adapter) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + + return mcfg->peer_sel_policy.cto_mgate_require + && !rtw_bss_is_cto_mgate(&(mlmeext->mlmext_info.network)); +} + +u8 rtw_mesh_cto_mgate_network_filter(_adapter *adapter, struct wlan_network *scanned) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + + return !rtw_mesh_cto_mgate_required(adapter) + || (rtw_bss_is_cto_mgate(&scanned->network) + && !rtw_mesh_cto_mgate_blacklist_search(adapter, scanned->network.MacAddress)); +} + +int rtw_mesh_cto_mgate_blacklist_add(_adapter *adapter, const u8 *addr) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + return rtw_blacklist_add(&plink_ctl->cto_mgate_blacklist, addr + , mcfg->peer_sel_policy.cto_mgate_blacklist_timeout_ms); +} + +int rtw_mesh_cto_mgate_blacklist_del(_adapter *adapter, const u8 *addr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + return rtw_blacklist_del(&plink_ctl->cto_mgate_blacklist, addr); +} + +int rtw_mesh_cto_mgate_blacklist_search(_adapter *adapter, const u8 *addr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + return rtw_blacklist_search(&plink_ctl->cto_mgate_blacklist, addr); +} + +void rtw_mesh_cto_mgate_blacklist_flush(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + rtw_blacklist_flush(&plink_ctl->cto_mgate_blacklist); +} + +void dump_mesh_cto_mgate_blacklist(void *sel, _adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + dump_blacklist(sel, &plink_ctl->cto_mgate_blacklist, "blacklist"); +} + +void dump_mesh_cto_mgate_blacklist_settings(void *sel, _adapter *adapter) +{ + struct mesh_peer_sel_policy *peer_sel_policy = &adapter->mesh_cfg.peer_sel_policy; + + RTW_PRINT_SEL(sel, "%-12s %-17s\n" + , "conf_timeout", "blacklist_timeout"); + RTW_PRINT_SEL(sel, "%12u %17u\n" + , peer_sel_policy->cto_mgate_conf_timeout_ms + , peer_sel_policy->cto_mgate_blacklist_timeout_ms); +} + +static void rtw_mesh_cto_mgate_blacklist_chk(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + _queue *blist = &plink_ctl->cto_mgate_blacklist; + _list *list, *head; + struct blacklist_ent *ent = NULL; + struct wlan_network *scanned = NULL; + + enter_critical_bh(&blist->lock); + head = &blist->queue; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + ent = LIST_CONTAINOR(list, struct blacklist_ent, list); + list = get_next(list); + + if (rtw_time_after(rtw_get_current_time(), ent->exp_time)) { + rtw_list_delete(&ent->list); + rtw_mfree(ent, sizeof(struct blacklist_ent)); + continue; + } + + scanned = rtw_find_network(&adapter->mlmepriv.scanned_queue, ent->addr); + if (!scanned) + continue; + + if (rtw_bss_is_forwarding(&scanned->network)) { + rtw_list_delete(&ent->list); + rtw_mfree(ent, sizeof(struct blacklist_ent)); + } + } + + exit_critical_bh(&blist->lock); +} +#endif /* CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST */ + +void rtw_chk_candidate_peer_notify(_adapter *adapter, struct wlan_network *scanned) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + struct mlme_priv *mlme = &adapter->mlmepriv; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + bool acnode = 0; + + if (IS_CH_WAITING(rfctl) && !IS_UNDER_CAC(rfctl)) + goto exit; + + if (plink_ctl->num >= RTW_MESH_MAX_PEER_CANDIDATES) + goto exit; + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + if (plink_ctl->acnode_rsvd) { + acnode = rtw_mesh_scanned_is_acnode_confirmed(adapter, scanned); + if (acnode && !rtw_mesh_scanned_is_acnode_allow_notify(adapter, scanned)) + goto exit; + } +#endif + + /* wpa_supplicant's auto peer will initiate peering when candidate peer is reported without max_peer_links consideration */ + if (plink_ctl->num >= mcfg->max_peer_links + acnode ? 1 : 0) + goto exit; + + if (rtw_get_passing_time_ms(scanned->last_scanned) >= mcfg->peer_sel_policy.scanr_exp_ms + || (mcfg->rssi_threshold && mcfg->rssi_threshold > scanned->network.Rssi) + || !rtw_bss_is_candidate_mesh_peer(adapter, &scanned->network, 1, 1) + #if CONFIG_RTW_MACADDR_ACL + || rtw_access_ctrl(adapter, scanned->network.MacAddress) == _FALSE + #endif + || rtw_mesh_plink_get(adapter, scanned->network.MacAddress) + #if CONFIG_RTW_MESH_PEER_BLACKLIST + || rtw_mesh_peer_blacklist_search(adapter, scanned->network.MacAddress) + #endif + #if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + || !rtw_mesh_cto_mgate_network_filter(adapter, scanned) + #endif + ) + goto exit; + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + if (acnode) { + scanned->acnode_notify_etime = 0; + RTW_INFO(FUNC_ADPT_FMT" acnode "MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(scanned->network.MacAddress)); + } +#endif + +#ifdef CONFIG_IOCTL_CFG80211 + rtw_cfg80211_notify_new_peer_candidate(adapter->rtw_wdev + , scanned->network.MacAddress + , BSS_EX_TLV_IES(&scanned->network) + , BSS_EX_TLV_IES_LEN(&scanned->network) + , scanned->network.Rssi + , GFP_ATOMIC + ); +#endif + +exit: + return; +} + +void rtw_mesh_peer_status_chk(_adapter *adapter) +{ + struct mlme_priv *mlme = &adapter->mlmepriv; + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *plink; + _list *head, *list; + struct sta_info *sta = NULL; + struct sta_priv *stapriv = &adapter->stapriv; + int stainfo_offset; +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + u8 cto_mgate, forwarding, mgate; +#endif + u8 flush; + s8 flush_list[NUM_STA]; + u8 flush_num = 0; + int i; + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + if (rtw_mesh_cto_mgate_required(adapter)) { + /* active scan on operating channel */ + issue_probereq_ex(adapter, &adapter->mlmepriv.cur_network.network.mesh_id, NULL, 0, 0, 0, 0); + } +#endif + + enter_critical_bh(&(plink_ctl->lock)); + + /* check established peers */ + enter_critical_bh(&stapriv->asoc_list_lock); + + head = &stapriv->asoc_list; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + sta = LIST_CONTAINOR(list, struct sta_info, asoc_list); + list = get_next(list); + + if (!sta->plink || !sta->plink->scanned) { + rtw_warn_on(1); + continue; + } + plink = sta->plink; + flush = 0; + + /* remove unsuitable peer */ + if (!rtw_bss_is_candidate_mesh_peer(adapter, &plink->scanned->network, 1, 0) + #if CONFIG_RTW_MACADDR_ACL + || rtw_access_ctrl(adapter, plink->addr) == _FALSE + #endif + ) { + flush = 1; + goto flush_add; + } + + #if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + cto_mgate = rtw_bss_is_cto_mgate(&(plink->scanned->network)); + forwarding = rtw_bss_is_forwarding(&(plink->scanned->network)); + mgate = rtw_mesh_gate_search(minfo->mesh_paths, sta->cmn.mac_addr); + + /* CTO_MGATE required, remove peer without CTO_MGATE */ + if (rtw_mesh_cto_mgate_required(adapter) && !cto_mgate) { + flush = 1; + goto flush_add; + } + + /* cto_mgate_conf status update */ + if (IS_CTO_MGATE_CONF_DISABLED(plink)) { + if (cto_mgate && !forwarding && !mgate) + SET_CTO_MGATE_CONF_END_TIME(plink, mcfg->peer_sel_policy.cto_mgate_conf_timeout_ms); + else + rtw_mesh_cto_mgate_blacklist_del(adapter, sta->cmn.mac_addr); + } else { + /* cto_mgate_conf ongoing */ + if (cto_mgate && !forwarding && !mgate) { + if (IS_CTO_MGATE_CONF_TIMEOUT(plink)) { + rtw_mesh_cto_mgate_blacklist_add(adapter, sta->cmn.mac_addr); + + /* CTO_MGATE required, remove peering can't achieve CTO_MGATE */ + if (rtw_mesh_cto_mgate_required(adapter)) { + flush = 1; + goto flush_add; + } + } + } else { + SET_CTO_MGATE_CONF_DISABLED(plink); + rtw_mesh_cto_mgate_blacklist_del(adapter, sta->cmn.mac_addr); + } + } + #endif /* CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST */ + +flush_add: + if (flush) { + rtw_list_delete(&sta->asoc_list); + stapriv->asoc_list_cnt--; +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (sta->tbtx_enable) + stapriv->tbtx_asoc_list_cnt--; +#endif + STA_SET_MESH_PLINK(sta, NULL); + + stainfo_offset = rtw_stainfo_offset(stapriv, sta); + if (stainfo_offset_valid(stainfo_offset)) + flush_list[flush_num++] = stainfo_offset; + else + rtw_warn_on(1); + } + } + + exit_critical_bh(&stapriv->asoc_list_lock); + + /* check non-established peers */ + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + plink = &plink_ctl->ent[i]; + if (plink->valid != _TRUE || plink->plink_state == RTW_MESH_PLINK_ESTAB) + continue; + + /* remove unsuitable peer */ + if (!rtw_bss_is_candidate_mesh_peer(adapter, &plink->scanned->network, 1, 1) + #if CONFIG_RTW_MACADDR_ACL + || rtw_access_ctrl(adapter, plink->addr) == _FALSE + #endif + ) { + _rtw_mesh_expire_peer_ent(adapter, plink); + continue; + } + + #if CONFIG_RTW_MESH_PEER_BLACKLIST + /* peer confirm check timeout, add to black list */ + if (IS_PEER_CONF_TIMEOUT(plink)) { + rtw_mesh_peer_blacklist_add(adapter, plink->addr); + _rtw_mesh_expire_peer_ent(adapter, plink); + } + #endif + } + + exit_critical_bh(&(plink_ctl->lock)); + + if (flush_num) { + u8 sta_addr[ETH_ALEN]; + u8 updated = _FALSE; + + for (i = 0; i < flush_num; i++) { + sta = rtw_get_stainfo_by_offset(stapriv, flush_list[i]); + _rtw_memcpy(sta_addr, sta->cmn.mac_addr, ETH_ALEN); + + updated |= ap_free_sta(adapter, sta, _TRUE, WLAN_REASON_DEAUTH_LEAVING, _FALSE); + rtw_mesh_expire_peer(adapter, sta_addr); + } + + associated_clients_update(adapter, updated, STA_INFO_UPDATE_ALL); + } + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + /* loop cto_mgate_blacklist to remove ent according to scan_r */ + rtw_mesh_cto_mgate_blacklist_chk(adapter); +#endif + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + rtw_mesh_acnode_rsvd_chk(adapter); +#endif + + return; +} + +#if CONFIG_RTW_MESH_OFFCH_CAND +static u8 rtw_mesh_offch_cto_mgate_required(_adapter *adapter) +{ +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct mlme_priv *mlme = &adapter->mlmepriv; + _queue *queue = &(mlme->scanned_queue); + _list *head, *pos; + struct wlan_network *scanned = NULL; + u8 ret = 0; + + if (!rtw_mesh_cto_mgate_required(adapter)) + goto exit; + + enter_critical_bh(&(mlme->scanned_queue.lock)); + + head = get_list_head(queue); + pos = get_next(head); + while (!rtw_end_of_queue_search(head, pos)) { + scanned = LIST_CONTAINOR(pos, struct wlan_network, list); + + if (rtw_get_passing_time_ms(scanned->last_scanned) < mcfg->peer_sel_policy.scanr_exp_ms + && (!mcfg->rssi_threshold || mcfg->rssi_threshold <= scanned->network.Rssi) + #if CONFIG_RTW_MACADDR_ACL + && rtw_access_ctrl(adapter, scanned->network.MacAddress) == _TRUE + #endif + && rtw_bss_is_candidate_mesh_peer(adapter, &scanned->network, 1, 1) + && rtw_bss_is_cto_mgate(&scanned->network) + #if CONFIG_RTW_MESH_PEER_BLACKLIST + && !rtw_mesh_peer_blacklist_search(adapter, scanned->network.MacAddress) + #endif + && !rtw_mesh_cto_mgate_blacklist_search(adapter, scanned->network.MacAddress) + ) + break; + + pos = get_next(pos); + } + + if (rtw_end_of_queue_search(head, pos)) + ret = 1; + + exit_critical_bh(&(mlme->scanned_queue.lock)); + +exit: + return ret; +#else + return 0; +#endif /* CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST */ +} + +u8 rtw_mesh_offch_candidate_accepted(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + u8 ret = 0; + + if (!adapter->mesh_cfg.peer_sel_policy.offch_cand) + goto exit; + + ret = MLME_IS_MESH(adapter) && MLME_IS_ASOC(adapter) + && (!plink_ctl->num || rtw_mesh_offch_cto_mgate_required(adapter)) + ; + +#ifdef CONFIG_CONCURRENT_MODE + if (ret) { + struct mi_state mstate_no_self; + + rtw_mi_status_no_self(adapter, &mstate_no_self); + if (MSTATE_STA_LD_NUM(&mstate_no_self)) + ret = 0; + } +#endif + +exit: + return ret; +} + +/* + * this function is called under off channel candidate is required + * the channel with maximum candidate count is selected +*/ +u8 rtw_mesh_select_operating_ch(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct mlme_priv *mlme = &adapter->mlmepriv; + _queue *queue = &(mlme->scanned_queue); + _list *head, *pos; + _irqL irqL; + struct wlan_network *scanned = NULL; + int i; + /* statistics for candidate accept peering */ + u8 cand_ap_cnt[MAX_CHANNEL_NUM] = {0}; + u8 max_cand_ap_ch = 0; + u8 max_cand_ap_cnt = 0; + /* statistics for candidate including not accept peering */ + u8 cand_cnt[MAX_CHANNEL_NUM] = {0}; + u8 max_cand_ch = 0; + u8 max_cand_cnt = 0; + + _enter_critical_bh(&(mlme->scanned_queue.lock), &irqL); + + head = get_list_head(queue); + pos = get_next(head); + while (!rtw_end_of_queue_search(head, pos)) { + scanned = LIST_CONTAINOR(pos, struct wlan_network, list); + pos = get_next(pos); + + if (rtw_get_passing_time_ms(scanned->last_scanned) < mcfg->peer_sel_policy.scanr_exp_ms + && (!mcfg->rssi_threshold || mcfg->rssi_threshold <= scanned->network.Rssi) + #if CONFIG_RTW_MACADDR_ACL + && rtw_access_ctrl(adapter, scanned->network.MacAddress) == _TRUE + #endif + && rtw_bss_is_candidate_mesh_peer(adapter, &scanned->network, 0, 0) + #if CONFIG_RTW_MESH_PEER_BLACKLIST + && !rtw_mesh_peer_blacklist_search(adapter, scanned->network.MacAddress) + #endif + #if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + && rtw_mesh_cto_mgate_network_filter(adapter, scanned) + #endif + ) { + int ch_set_idx = rtw_chset_search_ch(rfctl->channel_set, scanned->network.Configuration.DSConfig); + + if (ch_set_idx >= 0 + && !(rfctl->channel_set[ch_set_idx].flags & RTW_CHF_NO_IR) + && !CH_IS_NON_OCP(&rfctl->channel_set[ch_set_idx]) + ) { + u8 nop, accept; + + rtw_mesh_bss_peering_status(&scanned->network, &nop, &accept); + cand_cnt[ch_set_idx]++; + if (max_cand_cnt < cand_cnt[ch_set_idx]) { + max_cand_cnt = cand_cnt[ch_set_idx]; + max_cand_ch = rfctl->channel_set[ch_set_idx].ChannelNum; + } + if (accept) { + cand_ap_cnt[ch_set_idx]++; + if (max_cand_ap_cnt < cand_ap_cnt[ch_set_idx]) { + max_cand_ap_cnt = cand_ap_cnt[ch_set_idx]; + max_cand_ap_ch = rfctl->channel_set[ch_set_idx].ChannelNum; + } + } + } + } + } + + _exit_critical_bh(&(mlme->scanned_queue.lock), &irqL); + + return max_cand_ap_ch ? max_cand_ap_ch : max_cand_ch; +} + +void dump_mesh_offch_cand_settings(void *sel, _adapter *adapter) +{ + struct mesh_peer_sel_policy *peer_sel_policy = &adapter->mesh_cfg.peer_sel_policy; + + RTW_PRINT_SEL(sel, "%-6s %-11s\n" + , "enable", "find_int_ms"); + RTW_PRINT_SEL(sel, "%6u %11u\n" + , peer_sel_policy->offch_cand, peer_sel_policy->offch_find_int_ms); +} +#endif /* CONFIG_RTW_MESH_OFFCH_CAND */ + +void dump_mesh_peer_sel_policy(void *sel, _adapter *adapter) +{ + struct mesh_peer_sel_policy *peer_sel_policy = &adapter->mesh_cfg.peer_sel_policy; + + RTW_PRINT_SEL(sel, "%-12s\n", "scanr_exp_ms"); + RTW_PRINT_SEL(sel, "%12u\n", peer_sel_policy->scanr_exp_ms); +} + +void dump_mesh_networks(void *sel, _adapter *adapter) +{ +#if CONFIG_RTW_MESH_ACNODE_PREVENT +#define NSTATE_TITLE_FMT_ACN " %-5s" +#define NSTATE_VALUE_FMT_ACN " %5d" +#define NSTATE_TITLE_ARG_ACN , "acn" +#define NSTATE_VALUE_ARG_ACN , (acn_ms < 99999 ? acn_ms : 99999) +#else +#define NSTATE_TITLE_FMT_ACN "" +#define NSTATE_VALUE_FMT_ACN "" +#define NSTATE_TITLE_ARG_ACN +#define NSTATE_VALUE_ARG_ACN +#endif + + struct mlme_priv *mlme = &(adapter->mlmepriv); + _queue *queue = &(mlme->scanned_queue); + struct wlan_network *network; + _list *list, *head; + u8 same_mbss; + u8 candidate; + struct mesh_plink_ent *plink; + u8 blocked; + u8 established; + s32 age_ms; +#if CONFIG_RTW_MESH_ACNODE_PREVENT + s32 acn_ms; +#endif + u8 *mesh_conf_ie; + sint mesh_conf_ie_len; + u8 auth_pid; + u8 *rsn_ie; + int rsn_ie_len; + int gcs, pcs, gmcs; + u8 mfp_opt; + struct wlan_network **mesh_networks; + u8 mesh_network_cnt = 0; + int i; + + mesh_networks = rtw_zvmalloc(mlme->max_bss_cnt * sizeof(struct wlan_network *)); + if (!mesh_networks) + return; + + enter_critical_bh(&queue->lock); + head = get_list_head(queue); + list = get_next(head); + + while (rtw_end_of_queue_search(head, list) == _FALSE) { + network = LIST_CONTAINOR(list, struct wlan_network, list); + list = get_next(list); + + if (network->network.InfrastructureMode != Ndis802_11_mesh) + continue; + + mesh_conf_ie = rtw_get_ie(BSS_EX_TLV_IES(&network->network), WLAN_EID_MESH_CONFIG + , &mesh_conf_ie_len, BSS_EX_TLV_IES_LEN(&network->network)); + if (!mesh_conf_ie || mesh_conf_ie_len != 7) + continue; + + mesh_networks[mesh_network_cnt++] = network; + } + + exit_critical_bh(&queue->lock); + + RTW_PRINT_SEL(sel, " %-17s %-3s %-4s %-5s %-32s %-10s" + " %-3s %-3s %-4s" + " %-3s %-3s %-3s" + NSTATE_TITLE_FMT_ACN + "\n" + , "bssid", "ch", "rssi", "age", "mesh_id", "P M C S A " + , "pcs", "gcs", "gmcs" + , "nop", "fwd", "cto" + NSTATE_TITLE_ARG_ACN + ); + + if (MLME_IS_MESH(adapter) && MLME_IS_ASOC(adapter)) { + network = &mlme->cur_network; + mesh_conf_ie = rtw_get_ie(BSS_EX_TLV_IES(&network->network), WLAN_EID_MESH_CONFIG + , &mesh_conf_ie_len, BSS_EX_TLV_IES_LEN(&network->network)); + if (mesh_conf_ie && mesh_conf_ie_len == 7) { + gcs = pcs = gmcs = 0; + mfp_opt = MFP_NO; + auth_pid = GET_MESH_CONF_ELE_AUTH_PROTO_ID(mesh_conf_ie + 2); + if (auth_pid && auth_pid <= 2) { + rsn_ie = rtw_get_wpa2_ie(BSS_EX_TLV_IES(&network->network) + , &rsn_ie_len, BSS_EX_TLV_IES_LEN(&network->network)); + if (rsn_ie && rsn_ie_len) + rtw_parse_wpa2_ie(rsn_ie, rsn_ie_len + 2, &gcs, &pcs, &gmcs, NULL, &mfp_opt, NULL); + } + RTW_PRINT_SEL(sel, "* "MAC_FMT" %3d %-32s %2x%2x%2x%2x%2x" + " %03x %03x %c%03x" + " %c%2u %3u %c%c " + "\n" + , MAC_ARG(network->network.MacAddress) + , network->network.Configuration.DSConfig + , network->network.mesh_id.Ssid + , GET_MESH_CONF_ELE_PATH_SEL_PROTO_ID(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_PATH_SEL_METRIC_ID(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_CONGEST_CTRL_MODE_ID(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_SYNC_METHOD_ID(mesh_conf_ie + 2) + , auth_pid + , pcs, gcs, mfp_opt == MFP_REQUIRED ? 'R' : (mfp_opt == MFP_OPTIONAL ? 'C' : ' ') + , mfp_opt >= MFP_OPTIONAL ? gmcs : 0 + , GET_MESH_CONF_ELE_ACCEPT_PEERINGS(mesh_conf_ie + 2) ? '+' : ' ' + , GET_MESH_CONF_ELE_NUM_OF_PEERINGS(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_FORWARDING(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_CTO_MGATE(mesh_conf_ie + 2) ? 'G' : ' ' + , GET_MESH_CONF_ELE_CTO_AS(mesh_conf_ie + 2) ? 'A' : ' ' + ); + } + } + + for (i = 0; i < mesh_network_cnt; i++) { + network = mesh_networks[i]; + + if (network->network.InfrastructureMode != Ndis802_11_mesh) + continue; + + mesh_conf_ie = rtw_get_ie(BSS_EX_TLV_IES(&network->network), WLAN_EID_MESH_CONFIG + , &mesh_conf_ie_len, BSS_EX_TLV_IES_LEN(&network->network)); + if (!mesh_conf_ie || mesh_conf_ie_len != 7) + continue; + + gcs = pcs = gmcs = 0; + mfp_opt = MFP_NO; + auth_pid = GET_MESH_CONF_ELE_AUTH_PROTO_ID(mesh_conf_ie + 2); + if (auth_pid && auth_pid <= 2) { + rsn_ie = rtw_get_wpa2_ie(BSS_EX_TLV_IES(&network->network), &rsn_ie_len, BSS_EX_TLV_IES_LEN(&network->network)); + if (rsn_ie && rsn_ie_len) + rtw_parse_wpa2_ie(rsn_ie, rsn_ie_len + 2, &gcs, &pcs, &gmcs, NULL, &mfp_opt, NULL); + } + age_ms = rtw_get_passing_time_ms(network->last_scanned); + #if CONFIG_RTW_MESH_ACNODE_PREVENT + if (network->acnode_stime == 0) + acn_ms = 0; + else + acn_ms = rtw_get_passing_time_ms(network->acnode_stime); + #endif + same_mbss = 0; + candidate = 0; + plink = NULL; + blocked = 0; + established = 0; + + if (MLME_IS_MESH(adapter) && MLME_IS_ASOC(adapter)) { + plink = rtw_mesh_plink_get(adapter, network->network.MacAddress); + if (plink && plink->plink_state == RTW_MESH_PLINK_ESTAB) + established = 1; + else if (plink && plink->plink_state == RTW_MESH_PLINK_BLOCKED) + blocked = 1; + else if (plink) + ; + else if (rtw_bss_is_candidate_mesh_peer(adapter, &network->network, 0, 1)) + candidate = 1; + else if (rtw_bss_is_same_mbss(&mlme->cur_network.network, &network->network)) + same_mbss = 1; + } + + RTW_PRINT_SEL(sel, "%c "MAC_FMT" %3d %4ld %5d %-32s %2x%2x%2x%2x%2x" + " %03x %03x %c%03x" + " %c%2u %3u %c%c " + NSTATE_VALUE_FMT_ACN + "\n" + , established ? 'E' : (blocked ? 'B' : (plink ? 'N' : (candidate ? 'C' : (same_mbss ? 'S' : ' ')))) + , MAC_ARG(network->network.MacAddress) + , network->network.Configuration.DSConfig + , network->network.Rssi + , age_ms < 99999 ? age_ms : 99999 + , network->network.mesh_id.Ssid + , GET_MESH_CONF_ELE_PATH_SEL_PROTO_ID(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_PATH_SEL_METRIC_ID(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_CONGEST_CTRL_MODE_ID(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_SYNC_METHOD_ID(mesh_conf_ie + 2) + , auth_pid + , pcs, gcs, mfp_opt == MFP_REQUIRED ? 'R' : (mfp_opt == MFP_OPTIONAL ? 'C' : ' ') + , mfp_opt >= MFP_OPTIONAL ? gmcs : 0 + , GET_MESH_CONF_ELE_ACCEPT_PEERINGS(mesh_conf_ie + 2) ? '+' : ' ' + , GET_MESH_CONF_ELE_NUM_OF_PEERINGS(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_FORWARDING(mesh_conf_ie + 2) + , GET_MESH_CONF_ELE_CTO_MGATE(mesh_conf_ie + 2) ? 'G' : ' ' + , GET_MESH_CONF_ELE_CTO_AS(mesh_conf_ie + 2) ? 'A' : ' ' + NSTATE_VALUE_ARG_ACN + ); + } + + rtw_vmfree(mesh_networks, mlme->max_bss_cnt * sizeof(struct wlan_network *)); +} + +void rtw_mesh_adjust_chbw(u8 req_ch, u8 *req_bw, u8 *req_offset) +{ + if (req_ch >= 5 && req_ch <= 9) { + /* prevent secondary channel offset mismatch */ + if (*req_bw > CHANNEL_WIDTH_20) { + *req_bw = CHANNEL_WIDTH_20; + *req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + } +} + +void rtw_mesh_sae_check_frames(_adapter *adapter, const u8 *buf, u32 len, u8 tx, u16 alg, u16 seq, u16 status) +{ +#if CONFIG_RTW_MESH_PEER_BLACKLIST + if (tx && seq == 1) + rtw_mesh_plink_set_peer_conf_timeout(adapter, GetAddr1Ptr(buf)); +#endif +} + +#if CONFIG_RTW_MPM_TX_IES_SYNC_BSS +#ifdef CONFIG_RTW_MESH_AEK +static int rtw_mpm_ampe_dec(_adapter *adapter, struct mesh_plink_ent *plink + , u8 *fhead, size_t flen, u8* fbody, u8 *mic_ie, u8 *ampe_buf) +{ + int ret = _FAIL, verify_ret; + const u8 *aad[] = {adapter_mac_addr(adapter), plink->addr, fbody}; + const size_t aad_len[] = {ETH_ALEN, ETH_ALEN, mic_ie - fbody}; + u8 *iv_crypt; + size_t iv_crypt_len = flen - (mic_ie + 2 - fhead); + + iv_crypt = rtw_malloc(iv_crypt_len); + if (!iv_crypt) + goto exit; + + _rtw_memcpy(iv_crypt, mic_ie + 2, iv_crypt_len); + + verify_ret = rtw_aes_siv_decrypt(plink->aek, 32, iv_crypt, iv_crypt_len + , 3, aad, aad_len, ampe_buf); + + rtw_mfree(iv_crypt, iv_crypt_len); + + if (verify_ret) { + RTW_WARN("verify error, aek_valid=%u\n", plink->aek_valid); + goto exit; + } else if (*ampe_buf != WLAN_EID_AMPE) { + RTW_WARN("plaintext is not AMPE IE\n"); + goto exit; + } else if ( 16 /* AES_BLOCK_SIZE*/ + 2 + *(ampe_buf + 1) > iv_crypt_len) { + RTW_WARN("plaintext AMPE IE length is not valid\n"); + goto exit; + } + + ret = _SUCCESS; + +exit: + return ret; +} + +static int rtw_mpm_ampe_enc(_adapter *adapter, struct mesh_plink_ent *plink + , u8* fbody, u8 *mic_ie, u8 *ampe_buf, bool inverse) +{ + int ret = _FAIL, protect_ret; + const u8 *aad[3]; + const size_t aad_len[3] = {ETH_ALEN, ETH_ALEN, mic_ie - fbody}; + u8 *ampe_ie; + size_t ampe_ie_len = *(ampe_buf + 1) + 2; /* including id & len */ + + if (inverse) { + aad[0] = plink->addr; + aad[1] = adapter_mac_addr(adapter); + } else { + aad[0] = adapter_mac_addr(adapter); + aad[1] = plink->addr; + } + aad[2] = fbody; + + ampe_ie = rtw_malloc(ampe_ie_len); + if (!ampe_ie) + goto exit; + + _rtw_memcpy(ampe_ie, ampe_buf, ampe_ie_len); + + protect_ret = rtw_aes_siv_encrypt(plink->aek, 32, ampe_ie, ampe_ie_len + , 3, aad, aad_len, mic_ie + 2); + + rtw_mfree(ampe_ie, ampe_ie_len); + + if (protect_ret) { + RTW_WARN("protect error, aek_valid=%u\n", plink->aek_valid); + goto exit; + } + + ret = _SUCCESS; + +exit: + return ret; +} +#endif /* CONFIG_RTW_MESH_AEK */ + +static int rtw_mpm_tx_ies_sync_bss(_adapter *adapter, struct mesh_plink_ent *plink + , u8 *fhead, size_t flen, u8* fbody, u8 tlv_ies_offset, u8 *mpm_ie, u8 *mic_ie + , u8 **nbuf, size_t *nlen) +{ + int ret = _FAIL; + struct mlme_priv *mlme = &(adapter->mlmepriv); + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *mlmeinfo = &(mlmeext->mlmext_info); + WLAN_BSSID_EX *network = &(mlmeinfo->network); + uint left; + u8 *pos; + + uint mpm_ielen = *(mpm_ie + 1); + u8 *fpos; + u8 *new_buf = NULL; + size_t new_len = 0; + + u8 *new_fhead; + size_t new_flen; + u8 *new_fbody; + u8 *new_mic_ie; + +#ifdef CONFIG_RTW_MESH_AEK + u8 *ampe_buf = NULL; + size_t ampe_buf_len = 0; + + /* decode */ + if (mic_ie) { + ampe_buf_len = flen - (mic_ie + 2 + 16 /* AES_BLOCK_SIZE */ - fhead); + ampe_buf = rtw_malloc(ampe_buf_len); + if (!ampe_buf) + goto exit; + + if (rtw_mpm_ampe_dec(adapter, plink, fhead, flen, fbody, mic_ie, ampe_buf) != _SUCCESS) + goto exit; + + if (*(ampe_buf + 1) >= 68) { + _rtw_memcpy(plink->sel_pcs, ampe_buf + 2, 4); + _rtw_memcpy(plink->l_nonce, ampe_buf + 6, 32); + _rtw_memcpy(plink->p_nonce, ampe_buf + 38, 32); + } + } +#endif + + /* count for new frame length */ + new_len = sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset; + left = BSS_EX_TLV_IES_LEN(network); + pos = BSS_EX_TLV_IES(network); + while (left >= 2) { + u8 id, elen; + + id = *pos++; + elen = *pos++; + left -= 2; + + if (elen > left) + break; + + switch (id) { + case WLAN_EID_SSID: + case WLAN_EID_DS_PARAMS: + case WLAN_EID_TIM: + break; + default: + new_len += 2 + elen; + } + + left -= elen; + pos += elen; + } + new_len += mpm_ielen + 2; + if (mic_ie) + new_len += 16 /* AES_BLOCK_SIZE*/ + 2 + ampe_buf_len; + + /* alloc new frame */ + new_buf = rtw_malloc(new_len); + if (!new_buf) { + rtw_warn_on(1); + goto exit; + } + + /* build new frame */ + _rtw_memcpy(new_buf, fhead, sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset); + new_fhead = new_buf; + new_flen = new_len; + new_fbody = new_fhead + sizeof(struct rtw_ieee80211_hdr_3addr); + + fpos = new_fbody + tlv_ies_offset; + left = BSS_EX_TLV_IES_LEN(network); + pos = BSS_EX_TLV_IES(network); + while (left >= 2) { + u8 id, elen; + + id = *pos++; + elen = *pos++; + left -= 2; + + if (elen > left) + break; + + switch (id) { + case WLAN_EID_SSID: + case WLAN_EID_DS_PARAMS: + case WLAN_EID_TIM: + break; + default: + fpos = rtw_set_ie(fpos, id, elen, pos, NULL); + if (id == WLAN_EID_MESH_CONFIG) + fpos = rtw_set_ie(fpos, WLAN_EID_MPM, mpm_ielen, mpm_ie + 2, NULL); + } + + left -= elen; + pos += elen; + } + if (mic_ie) { + new_mic_ie = fpos; + *fpos++ = WLAN_EID_MIC; + *fpos++ = 16 /* AES_BLOCK_SIZE */; + } + +#ifdef CONFIG_RTW_MESH_AEK + /* encode */ + if (mic_ie) { + int enc_ret = rtw_mpm_ampe_enc(adapter, plink, new_fbody, new_mic_ie, ampe_buf, 0); + if (enc_ret != _SUCCESS) + goto exit; + } +#endif + + *nlen = new_len; + *nbuf = new_buf; + + ret = _SUCCESS; + +exit: + if (ret != _SUCCESS && new_buf) + rtw_mfree(new_buf, new_len); + +#ifdef CONFIG_RTW_MESH_AEK + if (ampe_buf) + rtw_mfree(ampe_buf, ampe_buf_len); +#endif + + return ret; +} +#endif /* CONFIG_RTW_MPM_TX_IES_SYNC_BSS */ + +struct mpm_frame_info { + u8 *aid; + u16 aid_v; + u8 *pid; + u16 pid_v; + u8 *llid; + u16 llid_v; + u8 *plid; + u16 plid_v; + u8 *reason; + u16 reason_v; + u8 *chosen_pmk; +}; + +/* +* pid:00000 llid:00000 chosen_pmk:0x00000000000000000000000000000000 +* aid:00000 pid:00000 llid:00000 plid:00000 chosen_pmk:0x00000000000000000000000000000000 +* pid:00000 llid:00000 plid:00000 reason:00000 chosen_pmk:0x00000000000000000000000000000000 +*/ +#define MPM_LOG_BUF_LEN 92 /* this length is limited for legal combination */ +static void rtw_mpm_info_msg(struct mpm_frame_info *mpm_info, u8 *mpm_log_buf) +{ + int cnt = 0; + + if (mpm_info->aid) { + cnt += snprintf(mpm_log_buf + cnt, MPM_LOG_BUF_LEN - cnt - 1, "aid:%u ", mpm_info->aid_v); + if (cnt >= MPM_LOG_BUF_LEN - 1) + goto exit; + } + if (mpm_info->pid) { + cnt += snprintf(mpm_log_buf + cnt, MPM_LOG_BUF_LEN - cnt - 1, "pid:%u ", mpm_info->pid_v); + if (cnt >= MPM_LOG_BUF_LEN - 1) + goto exit; + } + if (mpm_info->llid) { + cnt += snprintf(mpm_log_buf + cnt, MPM_LOG_BUF_LEN - cnt - 1, "llid:%u ", mpm_info->llid_v); + if (cnt >= MPM_LOG_BUF_LEN - 1) + goto exit; + } + if (mpm_info->plid) { + cnt += snprintf(mpm_log_buf + cnt, MPM_LOG_BUF_LEN - cnt - 1, "plid:%u ", mpm_info->plid_v); + if (cnt >= MPM_LOG_BUF_LEN - 1) + goto exit; + } + if (mpm_info->reason) { + cnt += snprintf(mpm_log_buf + cnt, MPM_LOG_BUF_LEN - cnt - 1, "reason:%u ", mpm_info->reason_v); + if (cnt >= MPM_LOG_BUF_LEN - 1) + goto exit; + } + if (mpm_info->chosen_pmk) { + cnt += snprintf(mpm_log_buf + cnt, MPM_LOG_BUF_LEN - cnt - 1, "chosen_pmk:0x"KEY_FMT, KEY_ARG(mpm_info->chosen_pmk)); + if (cnt >= MPM_LOG_BUF_LEN - 1) + goto exit; + } + +exit: + return; +} + +static int rtw_mpm_check_frames(_adapter *adapter, u8 action, const u8 **buf, size_t *len, u8 tx) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *plink = NULL; + u8 *nbuf = NULL; + size_t nlen = 0; + u8 *fhead = (u8 *)*buf; + size_t flen = *len; + u8 *peer_addr = tx ? GetAddr1Ptr(fhead) : get_addr2_ptr(fhead); + u8 *frame_body = fhead + sizeof(struct rtw_ieee80211_hdr_3addr); + struct mpm_frame_info mpm_info; + u8 tlv_ies_offset; + u8 *mpm_ie = NULL; + uint mpm_ielen = 0; + u8 *mic_ie = NULL; + uint mic_ielen = 0; + int ret = 0; + u8 mpm_log_buf[MPM_LOG_BUF_LEN] = {0}; + + if (action == RTW_ACT_SELF_PROTECTED_MESH_OPEN) + tlv_ies_offset = 4; + else if (action == RTW_ACT_SELF_PROTECTED_MESH_CONF) + tlv_ies_offset = 6; + else if (action == RTW_ACT_SELF_PROTECTED_MESH_CLOSE) + tlv_ies_offset = 2; + else { + rtw_warn_on(1); + goto exit; + } + + plink = rtw_mesh_plink_get(adapter, peer_addr); + if (!plink && (tx == _TRUE || action == RTW_ACT_SELF_PROTECTED_MESH_CONF)) { + /* warning message if no plink when: 1.TX all MPM or 2.RX CONF */ + RTW_WARN("RTW_%s:%s without plink of "MAC_FMT"\n" + , (tx == _TRUE) ? "Tx" : "Rx", action_self_protected_str(action), MAC_ARG(peer_addr)); + goto exit; + } + + _rtw_memset(&mpm_info, 0, sizeof(struct mpm_frame_info)); + + if (action == RTW_ACT_SELF_PROTECTED_MESH_CONF) { + mpm_info.aid = (u8 *)frame_body + 4; + mpm_info.aid_v = RTW_GET_LE16(mpm_info.aid); + } + + mpm_ie = rtw_get_ie(fhead + sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset + , WLAN_EID_MPM, &mpm_ielen + , flen - sizeof(struct rtw_ieee80211_hdr_3addr) - tlv_ies_offset); + if (!mpm_ie || mpm_ielen < 2 + 2) + goto exit; + + mpm_info.pid = mpm_ie + 2; + mpm_info.pid_v = RTW_GET_LE16(mpm_info.pid); + mpm_info.llid = mpm_info.pid + 2; + mpm_info.llid_v = RTW_GET_LE16(mpm_info.llid); + + switch (action) { + case RTW_ACT_SELF_PROTECTED_MESH_OPEN: + /* pid:2, llid:2, (chosen_pmk:16) */ + if (mpm_info.pid_v == 0 && mpm_ielen == 4) + ; + else if (mpm_info.pid_v == 1 && mpm_ielen == 20) + mpm_info.chosen_pmk = mpm_info.llid + 2; + else + goto exit; + break; + case RTW_ACT_SELF_PROTECTED_MESH_CONF: + /* pid:2, llid:2, plid:2, (chosen_pmk:16) */ + mpm_info.plid = mpm_info.llid + 2; + mpm_info.plid_v = RTW_GET_LE16(mpm_info.plid); + if (mpm_info.pid_v == 0 && mpm_ielen == 6) + ; + else if (mpm_info.pid_v == 1 && mpm_ielen == 22) + mpm_info.chosen_pmk = mpm_info.plid + 2; + else + goto exit; + break; + case RTW_ACT_SELF_PROTECTED_MESH_CLOSE: + /* pid:2, llid:2, (plid:2), reason:2, (chosen_pmk:16) */ + if (mpm_info.pid_v == 0 && mpm_ielen == 6) { + /* MPM, without plid */ + mpm_info.reason = mpm_info.llid + 2; + mpm_info.reason_v = RTW_GET_LE16(mpm_info.reason); + } else if (mpm_info.pid_v == 0 && mpm_ielen == 8) { + /* MPM, with plid */ + mpm_info.plid = mpm_info.llid + 2; + mpm_info.plid_v = RTW_GET_LE16(mpm_info.plid); + mpm_info.reason = mpm_info.plid + 2; + mpm_info.reason_v = RTW_GET_LE16(mpm_info.reason); + } else if (mpm_info.pid_v == 1 && mpm_ielen == 22) { + /* AMPE, without plid */ + mpm_info.reason = mpm_info.llid + 2; + mpm_info.reason_v = RTW_GET_LE16(mpm_info.reason); + mpm_info.chosen_pmk = mpm_info.reason + 2; + } else if (mpm_info.pid_v == 1 && mpm_ielen == 24) { + /* AMPE, with plid */ + mpm_info.plid = mpm_info.llid + 2; + mpm_info.plid_v = RTW_GET_LE16(mpm_info.plid); + mpm_info.reason = mpm_info.plid + 2; + mpm_info.reason_v = RTW_GET_LE16(mpm_info.reason); + mpm_info.chosen_pmk = mpm_info.reason + 2; + } else + goto exit; + break; + }; + + if (mpm_info.pid_v == 1) { + mic_ie = rtw_get_ie(fhead + sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset + , WLAN_EID_MIC, &mic_ielen + , flen - sizeof(struct rtw_ieee80211_hdr_3addr) - tlv_ies_offset); + if (!mic_ie || mic_ielen != 16 /* AES_BLOCK_SIZE */) + goto exit; + } + +#if CONFIG_RTW_MPM_TX_IES_SYNC_BSS + if ((action == RTW_ACT_SELF_PROTECTED_MESH_OPEN || action == RTW_ACT_SELF_PROTECTED_MESH_CONF) + && tx == _TRUE + ) { +#define DBG_RTW_MPM_TX_IES_SYNC_BSS 0 + + if (mpm_info.pid_v == 1 && (!plink || !MESH_PLINK_AEK_VALID(plink))) { + RTW_WARN("AEK not ready, IEs can't sync with BSS\n"); + goto bypass_sync_bss; + } + + if (DBG_RTW_MPM_TX_IES_SYNC_BSS) { + RTW_INFO(FUNC_ADPT_FMT" before:\n", FUNC_ADPT_ARG(adapter)); + dump_ies(RTW_DBGDUMP + , fhead + sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset + , flen - sizeof(struct rtw_ieee80211_hdr_3addr) - tlv_ies_offset); + } + + rtw_mpm_tx_ies_sync_bss(adapter, plink + , fhead, flen, frame_body, tlv_ies_offset, mpm_ie, mic_ie + , &nbuf, &nlen); + if (!nbuf) + goto exit; + + /* update pointer & len for new frame */ + fhead = nbuf; + flen = nlen; + frame_body = fhead + sizeof(struct rtw_ieee80211_hdr_3addr); + if (mpm_info.pid_v == 1) { + mic_ie = rtw_get_ie(fhead + sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset + , WLAN_EID_MIC, &mic_ielen + , flen - sizeof(struct rtw_ieee80211_hdr_3addr) - tlv_ies_offset); + } + + if (DBG_RTW_MPM_TX_IES_SYNC_BSS) { + RTW_INFO(FUNC_ADPT_FMT" after:\n", FUNC_ADPT_ARG(adapter)); + dump_ies(RTW_DBGDUMP + , fhead + sizeof(struct rtw_ieee80211_hdr_3addr) + tlv_ies_offset + , flen - sizeof(struct rtw_ieee80211_hdr_3addr) - tlv_ies_offset); + } + } +bypass_sync_bss: +#endif /* CONFIG_RTW_MPM_TX_IES_SYNC_BSS */ + + if (!plink) + goto mpm_log; + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + if (action == RTW_ACT_SELF_PROTECTED_MESH_OPEN) { + if (tx) + rtw_mesh_plink_set_peer_conf_timeout(adapter, peer_addr); + + } else +#endif +#if CONFIG_RTW_MESH_ACNODE_PREVENT + if (action == RTW_ACT_SELF_PROTECTED_MESH_CLOSE) { + if (tx && mpm_info.reason && mpm_info.reason_v == WLAN_REASON_MESH_MAX_PEERS) { + if (rtw_mesh_scanned_is_acnode_confirmed(adapter, plink->scanned) + && rtw_mesh_acnode_prevent_allow_sacrifice(adapter) + ) { + struct sta_info *sac = rtw_mesh_acnode_prevent_pick_sacrifice(adapter); + + if (sac) { + struct sta_priv *stapriv = &adapter->stapriv; + _irqL irqL; + u8 sta_addr[ETH_ALEN]; + u8 updated = _FALSE; + + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + if (!rtw_is_list_empty(&sac->asoc_list)) { + rtw_list_delete(&sac->asoc_list); + stapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (sac->tbtx_enable) + stapriv->tbtx_asoc_list_cnt--; + #endif + STA_SET_MESH_PLINK(sac, NULL); + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + RTW_INFO(FUNC_ADPT_FMT" sacrifice "MAC_FMT" for acnode\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sac->cmn.mac_addr)); + + _rtw_memcpy(sta_addr, sac->cmn.mac_addr, ETH_ALEN); + updated = ap_free_sta(adapter, sac, 0, 0, 1); + rtw_mesh_expire_peer(stapriv->padapter, sta_addr); + + associated_clients_update(adapter, updated, STA_INFO_UPDATE_ALL); + } + } + } + } else +#endif + if (action == RTW_ACT_SELF_PROTECTED_MESH_CONF) { + _irqL irqL; + u8 *ies = NULL; + u16 ies_len = 0; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + + plink = _rtw_mesh_plink_get(adapter, peer_addr); + if (!plink) + goto release_plink_ctl; + + if (tx == _FALSE) { + ies = plink->rx_conf_ies; + ies_len = plink->rx_conf_ies_len; + plink->rx_conf_ies = NULL; + plink->rx_conf_ies_len = 0; + + plink->llid = mpm_info.plid_v; + plink->plid = mpm_info.llid_v; + plink->peer_aid = mpm_info.aid_v; + if (mpm_info.pid_v == 1) + _rtw_memcpy(plink->chosen_pmk, mpm_info.chosen_pmk, 16); + } + #ifdef CONFIG_RTW_MESH_DRIVER_AID + else { + ies = plink->tx_conf_ies; + ies_len = plink->tx_conf_ies_len; + plink->tx_conf_ies = NULL; + plink->tx_conf_ies_len = 0; + } + #endif + + if (ies && ies_len) + rtw_mfree(ies, ies_len); + + #ifndef CONFIG_RTW_MESH_DRIVER_AID + if (tx == _TRUE) + goto release_plink_ctl; /* no need to copy tx conf ies */ + #endif + + /* copy mesh confirm IEs */ + if (mpm_info.pid_v == 1) /* not include MIC & encrypted AMPE */ + ies_len = (mic_ie - fhead) - sizeof(struct rtw_ieee80211_hdr_3addr) - 2; + else + ies_len = flen - sizeof(struct rtw_ieee80211_hdr_3addr) - 2; + + ies = rtw_zmalloc(ies_len); + if (ies) { + _rtw_memcpy(ies, fhead + sizeof(struct rtw_ieee80211_hdr_3addr) + 2, ies_len); + if (tx == _FALSE) { + plink->rx_conf_ies = ies; + plink->rx_conf_ies_len = ies_len; + } + #ifdef CONFIG_RTW_MESH_DRIVER_AID + else { + plink->tx_conf_ies = ies; + plink->tx_conf_ies_len = ies_len; + } + #endif + } + +release_plink_ctl: + _exit_critical_bh(&(plink_ctl->lock), &irqL); + } + +mpm_log: + rtw_mpm_info_msg(&mpm_info, mpm_log_buf); + RTW_INFO("RTW_%s:%s %s\n" + , (tx == _TRUE) ? "Tx" : "Rx" + , action_self_protected_str(action) + , mpm_log_buf + ); + + ret = 1; + +exit: + if (nbuf) { + if (ret == 1) { + *buf = nbuf; + *len = nlen; + } else + rtw_mfree(nbuf, nlen); + } + + return ret; +} + +static int rtw_mesh_check_frames(_adapter *adapter, const u8 **buf, size_t *len, u8 tx) +{ + int is_mesh_frame = -1; + const u8 *frame_body; + u8 category, action; + + frame_body = *buf + sizeof(struct rtw_ieee80211_hdr_3addr); + category = frame_body[0]; + + if (category == RTW_WLAN_CATEGORY_SELF_PROTECTED) { + action = frame_body[1]; + switch (action) { + case RTW_ACT_SELF_PROTECTED_MESH_OPEN: + case RTW_ACT_SELF_PROTECTED_MESH_CONF: + case RTW_ACT_SELF_PROTECTED_MESH_CLOSE: + rtw_mpm_check_frames(adapter, action, buf, len, tx); + is_mesh_frame = action; + break; + case RTW_ACT_SELF_PROTECTED_MESH_GK_INFORM: + case RTW_ACT_SELF_PROTECTED_MESH_GK_ACK: + RTW_INFO("RTW_%s:%s\n", (tx == _TRUE) ? "Tx" : "Rx", action_self_protected_str(action)); + is_mesh_frame = action; + break; + default: + break; + }; + } + + return is_mesh_frame; +} + +int rtw_mesh_check_frames_tx(_adapter *adapter, const u8 **buf, size_t *len) +{ + return rtw_mesh_check_frames(adapter, buf, len, _TRUE); +} + +int rtw_mesh_check_frames_rx(_adapter *adapter, const u8 *buf, size_t len) +{ + return rtw_mesh_check_frames(adapter, &buf, &len, _FALSE); +} + +int rtw_mesh_on_auth(_adapter *adapter, union recv_frame *rframe) +{ + u8 *whdr = rframe->u.hdr.rx_data; + +#if CONFIG_RTW_MACADDR_ACL + if (rtw_access_ctrl(adapter, get_addr2_ptr(whdr)) == _FALSE) + return _SUCCESS; +#endif + + if (!rtw_mesh_plink_get(adapter, get_addr2_ptr(whdr))) { + #if CONFIG_RTW_MESH_ACNODE_PREVENT + rtw_mesh_acnode_set_notify_etime(adapter, whdr); + #endif + + if (adapter_to_rfctl(adapter)->offch_state == OFFCHS_NONE) + issue_probereq(adapter, &adapter->mlmepriv.cur_network.network.mesh_id, get_addr2_ptr(whdr)); + + /* only peer being added (checked by notify conditions) is allowed */ + return _SUCCESS; + } + + rtw_cfg80211_rx_mframe(adapter, rframe, NULL); + return _SUCCESS; +} + +unsigned int on_action_self_protected(_adapter *adapter, union recv_frame *rframe) +{ + unsigned int ret = _FAIL; + struct sta_info *sta = NULL; + u8 *pframe = rframe->u.hdr.rx_data; + uint frame_len = rframe->u.hdr.len; + u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 category; + u8 action; + + /* check RA matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(adapter), GetAddr1Ptr(pframe), ETH_ALEN)) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_SELF_PROTECTED) + goto exit; + + action = frame_body[1]; + switch (action) { + case RTW_ACT_SELF_PROTECTED_MESH_OPEN: + case RTW_ACT_SELF_PROTECTED_MESH_CONF: + case RTW_ACT_SELF_PROTECTED_MESH_CLOSE: + case RTW_ACT_SELF_PROTECTED_MESH_GK_INFORM: + case RTW_ACT_SELF_PROTECTED_MESH_GK_ACK: + if (!(MLME_IS_MESH(adapter) && MLME_IS_ASOC(adapter))) + goto exit; +#ifdef CONFIG_IOCTL_CFG80211 + #if CONFIG_RTW_MACADDR_ACL + if (rtw_access_ctrl(adapter, get_addr2_ptr(pframe)) == _FALSE) + goto exit; + #endif + #if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + if (rtw_mesh_cto_mgate_required(adapter) + /* only peer being added (checked by notify conditions) is allowed */ + && !rtw_mesh_plink_get(adapter, get_addr2_ptr(pframe))) + goto exit; + #endif + rtw_cfg80211_rx_action(adapter, rframe, NULL); + ret = _SUCCESS; +#endif /* CONFIG_IOCTL_CFG80211 */ + break; + default: + break; + } + +exit: + return ret; +} + +const u8 ae_to_mesh_ctrl_len[] = { + 6, + 12, /* MESH_FLAGS_AE_A4 */ + 18, /* MESH_FLAGS_AE_A5_A6 */ + 0, +}; + +unsigned int on_action_mesh(_adapter *adapter, union recv_frame *rframe) +{ + unsigned int ret = _FAIL; + struct sta_info *sta = NULL; + struct sta_priv *stapriv = &adapter->stapriv; + u8 *pframe = rframe->u.hdr.rx_data; + uint frame_len = rframe->u.hdr.len; + u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 category; + u8 action; + + if (!MLME_IS_MESH(adapter)) + goto exit; + + /* check stainfo exist? */ + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_MESH) + goto exit; + + action = frame_body[1]; + switch (action) { + case RTW_ACT_MESH_HWMP_PATH_SELECTION: + rtw_mesh_rx_path_sel_frame(adapter, rframe); + ret = _SUCCESS; + break; + default: + break; + } + +exit: + return ret; +} + +bool rtw_mesh_update_bss_peering_status(_adapter *adapter, WLAN_BSSID_EX *bss) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + u8 num_of_peerings = stapriv->asoc_list_cnt; + bool accept_peerings = stapriv->asoc_list_cnt < mcfg->max_peer_links; + u8 *ie; + int ie_len; + bool updated = 0; + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + accept_peerings |= plink_ctl->acnode_rsvd; +#endif + + ie = rtw_get_ie(BSS_EX_TLV_IES(bss), WLAN_EID_MESH_CONFIG, &ie_len, BSS_EX_TLV_IES_LEN(bss)); + if (!ie || ie_len != 7) { + rtw_warn_on(1); + goto exit; + } + + if (GET_MESH_CONF_ELE_NUM_OF_PEERINGS(ie + 2) != num_of_peerings) { + SET_MESH_CONF_ELE_NUM_OF_PEERINGS(ie + 2, num_of_peerings); + updated = 1; + } + + if (GET_MESH_CONF_ELE_ACCEPT_PEERINGS(ie + 2) != accept_peerings) { + SET_MESH_CONF_ELE_ACCEPT_PEERINGS(ie + 2, accept_peerings); + updated = 1; + } + +exit: + return updated; +} + +bool rtw_mesh_update_bss_formation_info(_adapter *adapter, WLAN_BSSID_EX *bss) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + u8 cto_mgate = (minfo->num_gates || mcfg->dot11MeshGateAnnouncementProtocol); + u8 cto_as = 0; + u8 *ie; + int ie_len; + bool updated = 0; + + ie = rtw_get_ie(BSS_EX_TLV_IES(bss), WLAN_EID_MESH_CONFIG, &ie_len, + BSS_EX_TLV_IES_LEN(bss)); + if (!ie || ie_len != 7) { + rtw_warn_on(1); + goto exit; + } + + if (GET_MESH_CONF_ELE_CTO_MGATE(ie + 2) != cto_mgate) { + SET_MESH_CONF_ELE_CTO_MGATE(ie + 2, cto_mgate); + updated = 1; + } + + if (GET_MESH_CONF_ELE_CTO_AS(ie + 2) != cto_as) { + SET_MESH_CONF_ELE_CTO_AS(ie + 2, cto_as); + updated = 1; + } + +exit: + return updated; +} + +bool rtw_mesh_update_bss_forwarding_state(_adapter *adapter, WLAN_BSSID_EX *bss) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + u8 forward = mcfg->dot11MeshForwarding; + u8 *ie; + int ie_len; + bool updated = 0; + + ie = rtw_get_ie(BSS_EX_TLV_IES(bss), WLAN_EID_MESH_CONFIG, &ie_len, + BSS_EX_TLV_IES_LEN(bss)); + if (!ie || ie_len != 7) { + rtw_warn_on(1); + goto exit; + } + + if (GET_MESH_CONF_ELE_FORWARDING(ie + 2) != forward) { + SET_MESH_CONF_ELE_FORWARDING(ie + 2, forward); + updated = 1; + } + +exit: + return updated; +} + +struct mesh_plink_ent *_rtw_mesh_plink_get(_adapter *adapter, const u8 *hwaddr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + int i; + + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + if (plink_ctl->ent[i].valid == _TRUE + && _rtw_memcmp(plink_ctl->ent[i].addr, hwaddr, ETH_ALEN) == _TRUE + ) { + ent = &plink_ctl->ent[i]; + break; + } + } + + return ent; +} + +struct mesh_plink_ent *rtw_mesh_plink_get(_adapter *adapter, const u8 *hwaddr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + ent = _rtw_mesh_plink_get(adapter, hwaddr); + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return ent; +} + +struct mesh_plink_ent *rtw_mesh_plink_get_no_estab_by_idx(_adapter *adapter, u8 idx) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + int i, j = 0; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + if (plink_ctl->ent[i].valid == _TRUE + && plink_ctl->ent[i].plink_state != RTW_MESH_PLINK_ESTAB + ) { + if (j == idx) { + ent = &plink_ctl->ent[i]; + break; + } + j++; + } + } + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return ent; +} + +int _rtw_mesh_plink_add(_adapter *adapter, const u8 *hwaddr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + u8 exist = _FALSE; + int i; + + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + if (plink_ctl->ent[i].valid == _TRUE + && _rtw_memcmp(plink_ctl->ent[i].addr, hwaddr, ETH_ALEN) == _TRUE + ) { + ent = &plink_ctl->ent[i]; + exist = _TRUE; + break; + } + + if (ent == NULL && plink_ctl->ent[i].valid == _FALSE) + ent = &plink_ctl->ent[i]; + } + + if (exist == _FALSE && ent) { + _rtw_memcpy(ent->addr, hwaddr, ETH_ALEN); + ent->valid = _TRUE; + #ifdef CONFIG_RTW_MESH_AEK + ent->aek_valid = 0; + #endif + ent->llid = 0; + ent->plid = 0; + _rtw_memset(ent->chosen_pmk, 0, 16); + #ifdef CONFIG_RTW_MESH_AEK + _rtw_memset(ent->sel_pcs, 0, 4); + _rtw_memset(ent->l_nonce, 0, 32); + _rtw_memset(ent->p_nonce, 0, 32); + #endif + ent->plink_state = RTW_MESH_PLINK_LISTEN; + #ifndef CONFIG_RTW_MESH_DRIVER_AID + ent->aid = 0; + #endif + ent->peer_aid = 0; + SET_PEER_CONF_DISABLED(ent); + SET_CTO_MGATE_CONF_DISABLED(ent); + plink_ctl->num++; + } + + return exist == _TRUE ? RTW_ALREADY : (ent ? _SUCCESS : _FAIL); +} + +int rtw_mesh_plink_add(_adapter *adapter, const u8 *hwaddr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + _irqL irqL; + int ret; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + ret = _rtw_mesh_plink_add(adapter, hwaddr); + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return ret; +} + +int rtw_mesh_plink_set_state(_adapter *adapter, const u8 *hwaddr, u8 state) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + ent = _rtw_mesh_plink_get(adapter, hwaddr); + if (ent) + ent->plink_state = state; + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return ent ? _SUCCESS : _FAIL; +} + +#ifdef CONFIG_RTW_MESH_AEK +int rtw_mesh_plink_set_aek(_adapter *adapter, const u8 *hwaddr, const u8 *aek) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + ent = _rtw_mesh_plink_get(adapter, hwaddr); + if (ent) { + _rtw_memcpy(ent->aek, aek, 32); + ent->aek_valid = 1; + } + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return ent ? _SUCCESS : _FAIL; +} +#endif + +#if CONFIG_RTW_MESH_PEER_BLACKLIST +int rtw_mesh_plink_set_peer_conf_timeout(_adapter *adapter, const u8 *hwaddr) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + ent = _rtw_mesh_plink_get(adapter, hwaddr); + if (ent) { + if (IS_PEER_CONF_DISABLED(ent)) + SET_PEER_CONF_END_TIME(ent, mcfg->peer_sel_policy.peer_conf_timeout_ms); + } + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return ent ? _SUCCESS : _FAIL; +} +#endif + +void _rtw_mesh_plink_del_ent(_adapter *adapter, struct mesh_plink_ent *ent) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + + ent->valid = _FALSE; + #ifdef CONFIG_RTW_MESH_DRIVER_AID + if (ent->tx_conf_ies && ent->tx_conf_ies_len) + rtw_mfree(ent->tx_conf_ies, ent->tx_conf_ies_len); + ent->tx_conf_ies = NULL; + ent->tx_conf_ies_len = 0; + #endif + if (ent->rx_conf_ies && ent->rx_conf_ies_len) + rtw_mfree(ent->rx_conf_ies, ent->rx_conf_ies_len); + ent->rx_conf_ies = NULL; + ent->rx_conf_ies_len = 0; + if (ent->scanned) + ent->scanned = NULL; + plink_ctl->num--; +} + +int rtw_mesh_plink_del(_adapter *adapter, const u8 *hwaddr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent = NULL; + u8 exist = _FALSE; + int i; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + if (plink_ctl->ent[i].valid == _TRUE + && _rtw_memcmp(plink_ctl->ent[i].addr, hwaddr, ETH_ALEN) == _TRUE + ) { + ent = &plink_ctl->ent[i]; + exist = _TRUE; + break; + } + } + + if (exist == _TRUE) + _rtw_mesh_plink_del_ent(adapter, ent); + + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + return exist == _TRUE ? _SUCCESS : RTW_ALREADY; +} + +void rtw_mesh_plink_ctl_init(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + int i; + + _rtw_spinlock_init(&plink_ctl->lock); + plink_ctl->num = 0; + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) + plink_ctl->ent[i].valid = _FALSE; + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + _rtw_init_queue(&plink_ctl->peer_blacklist); +#endif +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + _rtw_init_queue(&plink_ctl->cto_mgate_blacklist); +#endif +} + +void rtw_mesh_plink_ctl_deinit(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent; + int i; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + ent = &plink_ctl->ent[i]; + #ifdef CONFIG_RTW_MESH_DRIVER_AID + if (ent->tx_conf_ies && ent->tx_conf_ies_len) + rtw_mfree(ent->tx_conf_ies, ent->tx_conf_ies_len); + #endif + if (ent->rx_conf_ies && ent->rx_conf_ies_len) + rtw_mfree(ent->rx_conf_ies, ent->rx_conf_ies_len); + } + _exit_critical_bh(&(plink_ctl->lock), &irqL); + + _rtw_spinlock_free(&plink_ctl->lock); + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + rtw_mesh_peer_blacklist_flush(adapter); + _rtw_deinit_queue(&plink_ctl->peer_blacklist); +#endif +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + rtw_mesh_cto_mgate_blacklist_flush(adapter); + _rtw_deinit_queue(&plink_ctl->cto_mgate_blacklist); +#endif +} + +void dump_mesh_plink_ctl(void *sel, _adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *ent; + int i; + + RTW_PRINT_SEL(sel, "num:%u\n", plink_ctl->num); + #if CONFIG_RTW_MESH_ACNODE_PREVENT + RTW_PRINT_SEL(sel, "acnode_rsvd:%u\n", plink_ctl->acnode_rsvd); + #endif + + for (i = 0; i < RTW_MESH_MAX_PEER_CANDIDATES; i++) { + ent = &plink_ctl->ent[i]; + if (!ent->valid) + continue; + + RTW_PRINT_SEL(sel, "\n"); + RTW_PRINT_SEL(sel, "peer:"MAC_FMT"\n", MAC_ARG(ent->addr)); + RTW_PRINT_SEL(sel, "plink_state:%s\n", rtw_mesh_plink_str(ent->plink_state)); + + #ifdef CONFIG_RTW_MESH_AEK + if (ent->aek_valid) + RTW_PRINT_SEL(sel, "aek:"KEY_FMT KEY_FMT"\n", KEY_ARG(ent->aek), KEY_ARG(ent->aek + 16)); + #endif + + RTW_PRINT_SEL(sel, "llid:%u, plid:%u\n", ent->llid, ent->plid); + #ifndef CONFIG_RTW_MESH_DRIVER_AID + RTW_PRINT_SEL(sel, "aid:%u\n", ent->aid); + #endif + RTW_PRINT_SEL(sel, "peer_aid:%u\n", ent->peer_aid); + + RTW_PRINT_SEL(sel, "chosen_pmk:"KEY_FMT"\n", KEY_ARG(ent->chosen_pmk)); + + #ifdef CONFIG_RTW_MESH_AEK + RTW_PRINT_SEL(sel, "sel_pcs:%02x%02x%02x%02x\n" + , ent->sel_pcs[0], ent->sel_pcs[1], ent->sel_pcs[2], ent->sel_pcs[3]); + RTW_PRINT_SEL(sel, "l_nonce:"KEY_FMT KEY_FMT"\n", KEY_ARG(ent->l_nonce), KEY_ARG(ent->l_nonce + 16)); + RTW_PRINT_SEL(sel, "p_nonce:"KEY_FMT KEY_FMT"\n", KEY_ARG(ent->p_nonce), KEY_ARG(ent->p_nonce + 16)); + #endif + + #ifdef CONFIG_RTW_MESH_DRIVER_AID + RTW_PRINT_SEL(sel, "tx_conf_ies:%p, len:%u\n", ent->tx_conf_ies, ent->tx_conf_ies_len); + #endif + RTW_PRINT_SEL(sel, "rx_conf_ies:%p, len:%u\n", ent->rx_conf_ies, ent->rx_conf_ies_len); + RTW_PRINT_SEL(sel, "scanned:%p\n", ent->scanned); + + #if CONFIG_RTW_MESH_PEER_BLACKLIST + if (!IS_PEER_CONF_DISABLED(ent)) { + if (!IS_PEER_CONF_TIMEOUT(ent)) + RTW_PRINT_SEL(sel, "peer_conf:%d\n", rtw_systime_to_ms(ent->peer_conf_end_time - rtw_get_current_time())); + else + RTW_PRINT_SEL(sel, "peer_conf:TIMEOUT\n"); + } + #endif + + #if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + if (!IS_CTO_MGATE_CONF_DISABLED(ent)) { + if (!IS_CTO_MGATE_CONF_TIMEOUT(ent)) + RTW_PRINT_SEL(sel, "cto_mgate_conf:%d\n", rtw_systime_to_ms(ent->cto_mgate_conf_end_time - rtw_get_current_time())); + else + RTW_PRINT_SEL(sel, "cto_mgate_conf:TIMEOUT\n"); + } + #endif + } +} + +/* this function is called with plink_ctl being locked */ +static int rtw_mesh_peer_establish(_adapter *adapter, struct mesh_plink_ent *plink, struct sta_info *sta) +{ +#ifndef DBG_RTW_MESH_PEER_ESTABLISH +#define DBG_RTW_MESH_PEER_ESTABLISH 0 +#endif + + struct sta_priv *stapriv = &adapter->stapriv; + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + u8 *tlv_ies; + u16 tlv_ieslen; + struct rtw_ieee802_11_elems elems; + _irqL irqL; + int i; + u16 status = 0; + int ret = _FAIL; +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + u8 sta_tbtx_enable = _FALSE; +#endif + + if (!plink->rx_conf_ies || !plink->rx_conf_ies_len) { + RTW_INFO(FUNC_ADPT_FMT" no rx confirm from sta "MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + goto exit; + } + + if (plink->rx_conf_ies_len < 4) { + RTW_INFO(FUNC_ADPT_FMT" confirm from sta "MAC_FMT" too short\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + goto exit; + } + +#ifdef CONFIG_RTW_MESH_DRIVER_AID + if (!plink->tx_conf_ies || !plink->tx_conf_ies_len) { + RTW_INFO(FUNC_ADPT_FMT" no tx confirm to sta "MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + goto exit; + } + + if (plink->tx_conf_ies_len < 4) { + RTW_INFO(FUNC_ADPT_FMT" confirm to sta "MAC_FMT" too short\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + goto exit; + } +#endif + + tlv_ies = plink->rx_conf_ies + 4; + tlv_ieslen = plink->rx_conf_ies_len - 4; + + if (DBG_RTW_MESH_PEER_ESTABLISH) + dump_ies(RTW_DBGDUMP, tlv_ies, tlv_ieslen); + + if (rtw_ieee802_11_parse_elems(tlv_ies, tlv_ieslen, &elems, 1) == ParseFailed) { + RTW_INFO(FUNC_ADPT_FMT" sta "MAC_FMT" sent invalid confirm\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + goto exit; + } + + SET_PEER_CONF_DISABLED(plink); + if (rtw_bss_is_cto_mgate(&plink->scanned->network) + && !rtw_bss_is_forwarding(&plink->scanned->network)) + SET_CTO_MGATE_CONF_END_TIME(plink, mcfg->peer_sel_policy.cto_mgate_conf_timeout_ms); + else + SET_CTO_MGATE_CONF_DISABLED(plink); + + sta->state &= (~WIFI_FW_AUTH_SUCCESS); + sta->state |= WIFI_FW_ASSOC_STATE; + + rtw_ap_parse_sta_capability(adapter, sta, plink->rx_conf_ies); + + status = rtw_ap_parse_sta_supported_rates(adapter, sta, tlv_ies, tlv_ieslen); + if (status != _STATS_SUCCESSFUL_) + goto exit; + + status = rtw_ap_parse_sta_security_ie(adapter, sta, &elems); + if (status != _STATS_SUCCESSFUL_) { + RTW_INFO(FUNC_ADPT_FMT" security check fail, status=%u\n", FUNC_ADPT_ARG(adapter), status); + goto exit; + } + + rtw_ap_parse_sta_wmm_ie(adapter, sta, tlv_ies, tlv_ieslen); +#ifdef CONFIG_RTS_FULL_BW + /*check vendor IE*/ + rtw_parse_sta_vendor_ie_8812(adapter, sta, tlv_ies, tlv_ieslen); +#endif/*CONFIG_RTS_FULL_BW*/ + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (elems.tbtx_cap && elems.tbtx_cap_len != 0) { + if(rtw_is_tbtx_capabilty(elems.tbtx_cap, elems.tbtx_cap_len)) { + sta_tbtx_enable = _TRUE; + } + } +#endif + + rtw_ap_parse_sta_ht_ie(adapter, sta, &elems); + rtw_ap_parse_sta_vht_ie(adapter, sta, &elems); + + /* AID */ +#ifdef CONFIG_RTW_MESH_DRIVER_AID + sta->cmn.aid = RTW_GET_LE16(plink->tx_conf_ies + 2); +#else + sta->cmn.aid = plink->aid; +#endif + stapriv->sta_aid[sta->cmn.aid - 1] = sta; + RTW_INFO(FUNC_ADPT_FMT" sta "MAC_FMT" aid:%u\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr), sta->cmn.aid); + + sta->state &= (~WIFI_FW_ASSOC_STATE); + sta->state |= WIFI_FW_ASSOC_SUCCESS; + + sta->local_mps = RTW_MESH_PS_ACTIVE; + + rtw_ewma_err_rate_init(&sta->metrics.err_rate); + rtw_ewma_err_rate_add(&sta->metrics.err_rate, 1); + /* init data_rate to 1M */ + sta->metrics.data_rate = 10; + sta->alive = _TRUE; + + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&sta->asoc_list)) { + STA_SET_MESH_PLINK(sta, plink); + /* TBD: up layer timeout mechanism */ + /* sta->expire_to = mcfg->plink_timeout / 2; */ + rtw_list_insert_tail(&sta->asoc_list, &stapriv->asoc_list); + stapriv->asoc_list_cnt++; +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (sta_tbtx_enable) { + sta->tbtx_enable = _TRUE; + stapriv->tbtx_asoc_list_cnt++; + } +#endif + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + + bss_cap_update_on_sta_join(adapter, sta); + sta_info_update(adapter, sta); + report_add_sta_event(adapter, sta->cmn.mac_addr); + + ret = _SUCCESS; + +exit: + return ret; +} + +int rtw_mesh_set_plink_state(_adapter *adapter, const u8 *mac, u8 plink_state) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *plink = NULL; + _irqL irqL2; + struct sta_priv *stapriv = &adapter->stapriv; + struct sta_info *sta = NULL; + _irqL irqL; + struct sta_info *del_sta = NULL; + int ret = _SUCCESS; + + _enter_critical_bh(&(plink_ctl->lock), &irqL2); + + plink = _rtw_mesh_plink_get(adapter, mac); + if (!plink) { + ret = _FAIL; + goto release_plink_ctl; + } + + plink->plink_state = plink_state; + + #if CONFIG_RTW_MESH_ACNODE_PREVENT + if (plink_state == RTW_MESH_PLINK_OPN_SNT) { + if (rtw_mesh_scanned_is_acnode_confirmed(adapter, plink->scanned) + && rtw_mesh_acnode_prevent_allow_sacrifice(adapter) + ) { + struct sta_info *sac = rtw_mesh_acnode_prevent_pick_sacrifice(adapter); + + if (sac) { + del_sta = sac; + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + if (!rtw_is_list_empty(&del_sta->asoc_list)) { + rtw_list_delete(&del_sta->asoc_list); + stapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (del_sta->tbtx_enable) + stapriv->tbtx_asoc_list_cnt--; + #endif + STA_SET_MESH_PLINK(del_sta, NULL); + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + RTW_INFO(FUNC_ADPT_FMT" sacrifice "MAC_FMT" for acnode\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(del_sta->cmn.mac_addr)); + } + } + } else + #endif + if (plink_state == RTW_MESH_PLINK_OPN_RCVD + || plink_state == RTW_MESH_PLINK_CNF_RCVD + || plink_state == RTW_MESH_PLINK_ESTAB + ) { + sta = rtw_get_stainfo(stapriv, mac); + if (!sta) { + sta = rtw_alloc_stainfo(stapriv, mac); + if (!sta) + goto release_plink_ctl; + } + + if (plink_state == RTW_MESH_PLINK_ESTAB) { + if (rtw_mesh_peer_establish(adapter, plink, sta) != _SUCCESS) { + del_sta = sta; + ret = _FAIL; + goto release_plink_ctl; + } + } + } + else if (plink_state == RTW_MESH_PLINK_HOLDING) { + del_sta = rtw_get_stainfo(stapriv, mac); + if (!del_sta) + goto release_plink_ctl; + + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + if (!rtw_is_list_empty(&del_sta->asoc_list)) { + rtw_list_delete(&del_sta->asoc_list); + stapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (del_sta->tbtx_enable) + stapriv->tbtx_asoc_list_cnt--; + #endif + STA_SET_MESH_PLINK(del_sta, NULL); + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + } + +release_plink_ctl: + _exit_critical_bh(&(plink_ctl->lock), &irqL2); + + if (del_sta) { + u8 sta_addr[ETH_ALEN]; + u8 updated = _FALSE; + + _rtw_memcpy(sta_addr, del_sta->cmn.mac_addr, ETH_ALEN); + updated = ap_free_sta(adapter, del_sta, 0, 0, 1); + rtw_mesh_expire_peer(stapriv->padapter, sta_addr); + + associated_clients_update(adapter, updated, STA_INFO_UPDATE_ALL); + } + + return ret; +} + +struct mesh_set_plink_cmd_parm { + const u8 *mac; + u8 plink_state; +}; + +u8 rtw_mesh_set_plink_state_cmd_hdl(_adapter *adapter, u8 *parmbuf) +{ + struct mesh_set_plink_cmd_parm *parm = (struct mesh_set_plink_cmd_parm *)parmbuf; + + if (rtw_mesh_set_plink_state(adapter, parm->mac, parm->plink_state) == _SUCCESS) + return H2C_SUCCESS; + + return H2C_CMD_FAIL; +} + +u8 rtw_mesh_set_plink_state_cmd(_adapter *adapter, const u8 *mac, u8 plink_state) +{ + struct cmd_obj *cmdobj; + struct mesh_set_plink_cmd_parm *parm; + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + /* prepare cmd parameter */ + parm = rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + parm->mac = mac; + parm->plink_state = plink_state; + + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree(parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_MESH_PLINK_STATE); + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + if (res == _SUCCESS) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&cmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&cmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + +exit: + return res; +} + +void rtw_mesh_expire_peer_notify(_adapter *adapter, const u8 *peer_addr) +{ + u8 null_ssid[2] = {0, 0}; + +#ifdef CONFIG_IOCTL_CFG80211 + rtw_cfg80211_notify_new_peer_candidate(adapter->rtw_wdev + , peer_addr + , null_ssid + , 2 + , 0 + , GFP_ATOMIC + ); +#endif + + return; +} + +static u8 *rtw_mesh_construct_peer_mesh_close(_adapter *adapter, struct mesh_plink_ent *plink, u16 reason, u32 *len) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + u8 *frame = NULL, *pos; + u32 flen; + struct rtw_ieee80211_hdr *whdr; + + if (minfo->mesh_auth_id && !MESH_PLINK_AEK_VALID(plink)) + goto exit; + + flen = sizeof(struct rtw_ieee80211_hdr_3addr) + + 2 /* category, action */ + + 2 + minfo->mesh_id_len /* mesh id */ + + 2 + 8 + (minfo->mesh_auth_id ? 16 : 0) /* mpm */ + + (minfo->mesh_auth_id ? 2 + 16 /* AES_BLOCK_SIZE */ : 0) /* mic */ + + (minfo->mesh_auth_id ? 70 : 0) /* ampe */ + ; + + pos = frame = rtw_zmalloc(flen); + if (!frame) + goto exit; + + whdr = (struct rtw_ieee80211_hdr *)frame; + _rtw_memcpy(whdr->addr1, adapter_mac_addr(adapter), ETH_ALEN); + _rtw_memcpy(whdr->addr2, plink->addr, ETH_ALEN); + _rtw_memcpy(whdr->addr3, adapter_mac_addr(adapter), ETH_ALEN); + + set_frame_sub_type(frame, WIFI_ACTION); + + pos += sizeof(struct rtw_ieee80211_hdr_3addr); + *(pos++) = RTW_WLAN_CATEGORY_SELF_PROTECTED; + *(pos++) = RTW_ACT_SELF_PROTECTED_MESH_CLOSE; + + pos = rtw_set_ie_mesh_id(pos, NULL, minfo->mesh_id, minfo->mesh_id_len); + + pos = rtw_set_ie_mpm(pos, NULL + , minfo->mesh_auth_id ? 1 : 0 + , plink->plid + , &plink->llid + , &reason + , minfo->mesh_auth_id ? plink->chosen_pmk : NULL); + +#ifdef CONFIG_RTW_MESH_AEK + if (minfo->mesh_auth_id) { + u8 ampe_buf[70]; + int enc_ret; + + *pos = WLAN_EID_MIC; + *(pos + 1) = 16 /* AES_BLOCK_SIZE */; + + ampe_buf[0] = WLAN_EID_AMPE; + ampe_buf[1] = 68; + _rtw_memcpy(ampe_buf + 2, plink->sel_pcs, 4); + _rtw_memcpy(ampe_buf + 6, plink->p_nonce, 32); + _rtw_memcpy(ampe_buf + 38, plink->l_nonce, 32); + + enc_ret = rtw_mpm_ampe_enc(adapter, plink + , frame + sizeof(struct rtw_ieee80211_hdr_3addr) + , pos, ampe_buf, 1); + if (enc_ret != _SUCCESS) { + rtw_mfree(frame, flen); + frame = NULL; + goto exit; + } + } +#endif + + *len = flen; + +exit: + return frame; +} + +void _rtw_mesh_expire_peer_ent(_adapter *adapter, struct mesh_plink_ent *plink) +{ +#if defined(CONFIG_RTW_MESH_STA_DEL_DISASOC) + _rtw_mesh_plink_del_ent(adapter, plink); + rtw_cfg80211_indicate_sta_disassoc(adapter, plink->addr, 0); +#else + u8 *frame = NULL; + u32 flen; + + if (plink->plink_state == RTW_MESH_PLINK_ESTAB) + frame = rtw_mesh_construct_peer_mesh_close(adapter, plink, WLAN_REASON_MESH_CLOSE, &flen); + + if (frame) { + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct wireless_dev *wdev = adapter->rtw_wdev; + s32 freq = rtw_ch2freq(mlmeext->cur_channel); + + #if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) || defined(COMPAT_KERNEL_RELEASE) + rtw_cfg80211_rx_mgmt(wdev, freq, 0, frame, flen, GFP_ATOMIC); + #else + cfg80211_rx_action(adapter->pnetdev, freq, frame, flen, GFP_ATOMIC); + #endif + + rtw_mfree(frame, flen); + } else { + rtw_mesh_expire_peer_notify(adapter, plink->addr); + RTW_INFO(FUNC_ADPT_FMT" set "MAC_FMT" plink unknown\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(plink->addr)); + plink->plink_state = RTW_MESH_PLINK_UNKNOWN; + } +#endif +} + +void rtw_mesh_expire_peer(_adapter *adapter, const u8 *peer_addr) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct mesh_plink_pool *plink_ctl = &minfo->plink_ctl; + struct mesh_plink_ent *plink; + _irqL irqL; + + _enter_critical_bh(&(plink_ctl->lock), &irqL); + + plink = _rtw_mesh_plink_get(adapter, peer_addr); + if (!plink) + goto exit; + + _rtw_mesh_expire_peer_ent(adapter, plink); + +exit: + _exit_critical_bh(&(plink_ctl->lock), &irqL); +} + +u8 rtw_mesh_ps_annc(_adapter *adapter, u8 ps) +{ + _irqL irqL; + _list *head, *list; + struct sta_info *sta; + struct sta_priv *stapriv = &adapter->stapriv; + u8 sta_alive_num = 0, i; + char sta_alive_list[NUM_STA]; + u8 annc_cnt = 0; + + if (rtw_linked_check(adapter) == _FALSE) + goto exit; + + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + + head = &stapriv->asoc_list; + list = get_next(head); + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + int stainfo_offset; + + sta = LIST_CONTAINOR(list, struct sta_info, asoc_list); + list = get_next(list); + + stainfo_offset = rtw_stainfo_offset(stapriv, sta); + if (stainfo_offset_valid(stainfo_offset)) + sta_alive_list[sta_alive_num++] = stainfo_offset; + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + + for (i = 0; i < sta_alive_num; i++) { + sta = rtw_get_stainfo_by_offset(stapriv, sta_alive_list[i]); + if (!sta) + continue; + + issue_qos_nulldata(adapter, sta->cmn.mac_addr, 7, ps, 3, 500); + annc_cnt++; + } + +exit: + return annc_cnt; +} + +static void mpath_tx_tasklet_hdl(void *priv) +{ + _adapter *adapter = (_adapter *)priv; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct xmit_frame *xframe; + _list *list, *head; + _list tmp; + u32 tmp_len; + s32 res; + + _rtw_init_listhead(&tmp); + + while (1) { + tmp_len = 0; + enter_critical_bh(&minfo->mpath_tx_queue.lock); + if (minfo->mpath_tx_queue_len) { + rtw_list_splice_init(&minfo->mpath_tx_queue.queue, &tmp); + tmp_len = minfo->mpath_tx_queue_len; + minfo->mpath_tx_queue_len = 0; + } + exit_critical_bh(&minfo->mpath_tx_queue.lock); + + if (!tmp_len) + break; + + head = &tmp; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + xframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + rtw_list_delete(&xframe->list); + res = rtw_xmit_posthandle(adapter, xframe, xframe->pkt); + if (res < 0) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s rtw_xmit fail\n", __FUNCTION__); + #endif + adapter->xmitpriv.tx_drop++; + } + } + } +} + +static void rtw_mpath_tx_queue_flush(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct xmit_frame *xframe; + _list *list, *head; + _list tmp; + + _rtw_init_listhead(&tmp); + + enter_critical_bh(&minfo->mpath_tx_queue.lock); + rtw_list_splice_init(&minfo->mpath_tx_queue.queue, &tmp); + minfo->mpath_tx_queue_len = 0; + exit_critical_bh(&minfo->mpath_tx_queue.lock); + + head = &tmp; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + xframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + rtw_list_delete(&xframe->list); + rtw_free_xmitframe(&adapter->xmitpriv, xframe); + } +} + +#ifdef PLATFORM_LINUX /* 3.10 ~ 4.13 checked */ +#if defined(CONFIG_SLUB) +#include +#elif defined(CONFIG_SLAB) +#include +#endif +typedef struct kmem_cache rtw_mcache; +#endif + +rtw_mcache *rtw_mcache_create(const char *name, size_t size) +{ +#ifdef PLATFORM_LINUX /* 3.10 ~ 4.13 checked */ + return kmem_cache_create(name, size, 0, 0, NULL); +#else + #error "TBD\n"; +#endif +} + +void rtw_mcache_destroy(rtw_mcache *s) +{ +#ifdef PLATFORM_LINUX /* 3.10 ~ 4.13 checked */ + kmem_cache_destroy(s); +#else + #error "TBD\n"; +#endif +} + +void *_rtw_mcache_alloc(rtw_mcache *cachep) +{ +#ifdef PLATFORM_LINUX /* 3.10 ~ 4.13 checked */ + return kmem_cache_alloc(cachep, GFP_ATOMIC); +#else + #error "TBD\n"; +#endif +} + +void _rtw_mcache_free(rtw_mcache *cachep, void *objp) +{ +#ifdef PLATFORM_LINUX /* 3.10 ~ 4.13 checked */ + kmem_cache_free(cachep, objp); +#else + #error "TBD\n"; +#endif +} + +#ifdef DBG_MEM_ALLOC +inline void *dbg_rtw_mcache_alloc(rtw_mcache *cachep, const enum mstat_f flags, const char *func, const int line) +{ + void *p; + u32 sz = cachep->size; + + if (match_mstat_sniff_rules(flags, sz)) + RTW_INFO("DBG_MEM_ALLOC %s:%d %s(%u)\n", func, line, __func__, sz); + + p = _rtw_mcache_alloc(cachep); + + rtw_mstat_update( + flags + , p ? MSTAT_ALLOC_SUCCESS : MSTAT_ALLOC_FAIL + , sz + ); + + return p; +} + +inline void dbg_rtw_mcache_free(rtw_mcache *cachep, void *pbuf, const enum mstat_f flags, const char *func, const int line) +{ + u32 sz = cachep->size; + + if (match_mstat_sniff_rules(flags, sz)) + RTW_INFO("DBG_MEM_ALLOC %s:%d %s(%u)\n", func, line, __func__, sz); + + _rtw_mcache_free(cachep, pbuf); + + rtw_mstat_update( + flags + , MSTAT_FREE + , sz + ); +} + +#define rtw_mcache_alloc(cachep) dbg_rtw_mcache_alloc(cachep, MSTAT_TYPE_PHY, __FUNCTION__, __LINE__) +#define rtw_mcache_free(cachep, objp) dbg_rtw_mcache_free(cachep, objp, MSTAT_TYPE_PHY, __FUNCTION__, __LINE__) +#else +#define rtw_mcache_alloc(cachep) _rtw_mcache_alloc(cachep) +#define rtw_mcache_free(cachep, objp) _rtw_mcache_free(cachep, objp) +#endif /* DBG_MEM_ALLOC */ + +/* Mesh Received Cache */ +#define RTW_MRC_BUCKETS 256 /* must be a power of 2 */ +#define RTW_MRC_QUEUE_MAX_LEN 4 +#define RTW_MRC_TIMEOUT_MS (3 * 1000) + +/** + * struct rtw_mrc_entry - entry in the Mesh Received Cache + * + * @seqnum: mesh sequence number of the frame + * @exp_time: expiration time of the entry + * @msa: mesh source address of the frame + * @list: hashtable list pointer + * + * The Mesh Received Cache keeps track of the latest received frames that + * have been received by a mesh interface and discards received frames + * that are found in the cache. + */ +struct rtw_mrc_entry { + rtw_hlist_node list; + systime exp_time; + u32 seqnum; + u8 msa[ETH_ALEN]; +}; + +struct rtw_mrc { + rtw_hlist_head bucket[RTW_MRC_BUCKETS]; + u32 idx_mask; + rtw_mcache *cache; +}; + +static int rtw_mrc_init(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + char cache_name[IFNAMSIZ + 8 + 1]; + int i; + + minfo->mrc = rtw_malloc(sizeof(struct rtw_mrc)); + if (!minfo->mrc) + return -ENOMEM; + minfo->mrc->idx_mask = RTW_MRC_BUCKETS - 1; + for (i = 0; i < RTW_MRC_BUCKETS; i++) + rtw_hlist_head_init(&minfo->mrc->bucket[i]); + + sprintf(cache_name, "rtw_mrc_%s", ADPT_ARG(adapter)); + minfo->mrc->cache = rtw_mcache_create(cache_name, sizeof(struct rtw_mrc_entry)); + + return 0; +} + +static void rtw_mrc_free(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mrc *mrc = minfo->mrc; + struct rtw_mrc_entry *p; + rtw_hlist_node *np, *n; + int i; + + if (!mrc) + return; + + for (i = 0; i < RTW_MRC_BUCKETS; i++) { + rtw_hlist_for_each_entry_safe(p, np, n, &mrc->bucket[i], list) { + rtw_hlist_del(&p->list); + rtw_mcache_free(mrc->cache, p); + } + } + + rtw_mcache_destroy(mrc->cache); + + rtw_mfree(mrc, sizeof(struct rtw_mrc)); + minfo->mrc = NULL; +} + +/** + * rtw_mrc_check - Check frame in mesh received cache and add if absent. + * + * @adapter: interface + * @msa: mesh source address + * @seq: mesh seq number + * + * Returns: 0 if the frame is not in the cache, nonzero otherwise. + * + * Checks using the mesh source address and the mesh sequence number if we have + * received this frame lately. If the frame is not in the cache, it is added to + * it. + */ +static int rtw_mrc_check(_adapter *adapter, const u8 *msa, u32 seq) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mrc *mrc = minfo->mrc; + int entries = 0; + u8 idx; + struct rtw_mrc_entry *p; + rtw_hlist_node *np, *n; + u8 timeout; + + if (!mrc) + return -1; + + idx = seq & mrc->idx_mask; + rtw_hlist_for_each_entry_safe(p, np, n, &mrc->bucket[idx], list) { + ++entries; + timeout = rtw_time_after(rtw_get_current_time(), p->exp_time); + if (timeout || entries == RTW_MRC_QUEUE_MAX_LEN) { + if (!timeout) + minfo->mshstats.mrc_del_qlen++; + + rtw_hlist_del(&p->list); + rtw_mcache_free(mrc->cache, p); + --entries; + } else if ((seq == p->seqnum) && _rtw_memcmp(msa, p->msa, ETH_ALEN) == _TRUE) + return -1; + } + + p = rtw_mcache_alloc(mrc->cache); + if (!p) + return 0; + + p->seqnum = seq; + p->exp_time = rtw_get_current_time() + rtw_ms_to_systime(RTW_MRC_TIMEOUT_MS); + _rtw_memcpy(p->msa, msa, ETH_ALEN); + rtw_hlist_add_head(&p->list, &mrc->bucket[idx]); + return 0; +} + +static int rtw_mesh_decache(_adapter *adapter, const u8 *msa, u32 seq) +{ + return rtw_mrc_check(adapter, msa, seq); +} + +#ifndef RTW_MESH_SCAN_RESULT_EXP_MS +#define RTW_MESH_SCAN_RESULT_EXP_MS (10 * 1000) +#endif + +#ifndef RTW_MESH_ACNODE_PREVENT +#define RTW_MESH_ACNODE_PREVENT 0 +#endif +#ifndef RTW_MESH_ACNODE_CONF_TIMEOUT_MS +#define RTW_MESH_ACNODE_CONF_TIMEOUT_MS (20 * 1000) +#endif +#ifndef RTW_MESH_ACNODE_NOTIFY_TIMEOUT_MS +#define RTW_MESH_ACNODE_NOTIFY_TIMEOUT_MS (2 * 1000) +#endif + +#ifndef RTW_MESH_OFFCH_CAND +#define RTW_MESH_OFFCH_CAND 1 +#endif +#ifndef RTW_MESH_OFFCH_CAND_FIND_INT_MS +#define RTW_MESH_OFFCH_CAND_FIND_INT_MS (10 * 1000) +#endif + +#ifndef RTW_MESH_PEER_CONF_TIMEOUT_MS +#define RTW_MESH_PEER_CONF_TIMEOUT_MS (20 * 1000) +#endif +#ifndef RTW_MESH_PEER_BLACKLIST_TIMEOUT_MS +#define RTW_MESH_PEER_BLACKLIST_TIMEOUT_MS (20 * 1000) +#endif + +#ifndef RTW_MESH_CTO_MGATE_REQUIRE +#define RTW_MESH_CTO_MGATE_REQUIRE 0 +#endif +#ifndef RTW_MESH_CTO_MGATE_CONF_TIMEOUT_MS +#define RTW_MESH_CTO_MGATE_CONF_TIMEOUT_MS (20 * 1000) +#endif +#ifndef RTW_MESH_CTO_MGATE_BLACKLIST_TIMEOUT_MS +#define RTW_MESH_CTO_MGATE_BLACKLIST_TIMEOUT_MS (20 * 1000) +#endif + +void rtw_mesh_cfg_init_peer_sel_policy(struct rtw_mesh_cfg *mcfg) +{ + struct mesh_peer_sel_policy *sel_policy = &mcfg->peer_sel_policy; + + sel_policy->scanr_exp_ms = RTW_MESH_SCAN_RESULT_EXP_MS; + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + sel_policy->acnode_prevent = RTW_MESH_ACNODE_PREVENT; + sel_policy->acnode_conf_timeout_ms = RTW_MESH_ACNODE_CONF_TIMEOUT_MS; + sel_policy->acnode_notify_timeout_ms = RTW_MESH_ACNODE_NOTIFY_TIMEOUT_MS; +#endif + +#if CONFIG_RTW_MESH_OFFCH_CAND + sel_policy->offch_cand = RTW_MESH_OFFCH_CAND; + sel_policy->offch_find_int_ms = RTW_MESH_OFFCH_CAND_FIND_INT_MS; +#endif + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + sel_policy->peer_conf_timeout_ms = RTW_MESH_PEER_CONF_TIMEOUT_MS; + sel_policy->peer_blacklist_timeout_ms = RTW_MESH_PEER_BLACKLIST_TIMEOUT_MS; +#endif + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + sel_policy->cto_mgate_require = RTW_MESH_CTO_MGATE_REQUIRE; + sel_policy->cto_mgate_conf_timeout_ms = RTW_MESH_CTO_MGATE_CONF_TIMEOUT_MS; + sel_policy->cto_mgate_blacklist_timeout_ms = RTW_MESH_CTO_MGATE_BLACKLIST_TIMEOUT_MS; +#endif +} + +void rtw_mesh_cfg_init(_adapter *adapter) +{ + struct registry_priv *regsty = adapter_to_regsty(adapter); + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + + mcfg->max_peer_links = RTW_MESH_MAX_PEER_LINKS; + mcfg->plink_timeout = RTW_MESH_PEER_LINK_TIMEOUT; + + mcfg->dot11MeshTTL = RTW_MESH_TTL; + mcfg->element_ttl = RTW_MESH_DEFAULT_ELEMENT_TTL; + mcfg->dot11MeshHWMPmaxPREQretries = RTW_MESH_MAX_PREQ_RETRIES; + mcfg->path_refresh_time = RTW_MESH_PATH_REFRESH_TIME; + mcfg->min_discovery_timeout = RTW_MESH_MIN_DISCOVERY_TIMEOUT; + mcfg->dot11MeshHWMPactivePathTimeout = RTW_MESH_PATH_TIMEOUT; + mcfg->dot11MeshHWMPpreqMinInterval = RTW_MESH_PREQ_MIN_INT; + mcfg->dot11MeshHWMPperrMinInterval = RTW_MESH_PERR_MIN_INT; + mcfg->dot11MeshHWMPnetDiameterTraversalTime = RTW_MESH_DIAM_TRAVERSAL_TIME; + mcfg->dot11MeshHWMPRootMode = RTW_IEEE80211_ROOTMODE_NO_ROOT; + mcfg->dot11MeshHWMPRannInterval = RTW_MESH_RANN_INTERVAL; + mcfg->dot11MeshGateAnnouncementProtocol = _FALSE; + mcfg->dot11MeshForwarding = _TRUE; + mcfg->rssi_threshold = 0; + mcfg->dot11MeshHWMPactivePathToRootTimeout = RTW_MESH_PATH_TO_ROOT_TIMEOUT; + mcfg->dot11MeshHWMProotInterval = RTW_MESH_ROOT_INTERVAL; + mcfg->dot11MeshHWMPconfirmationInterval = RTW_MESH_ROOT_CONFIRMATION_INTERVAL; + mcfg->path_gate_timeout_factor = 3; + rtw_mesh_cfg_init_peer_sel_policy(mcfg); +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + mcfg->sane_metric_delta = RTW_MESH_SANE_METRIC_DELTA; + mcfg->max_root_add_chk_cnt = RTW_MESH_MAX_ROOT_ADD_CHK_CNT; +#endif + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + mcfg->b2u_flags_msrc = regsty->msrc_b2u_flags; + mcfg->b2u_flags_mfwd = regsty->mfwd_b2u_flags; +#endif +} + +void rtw_mesh_cfg_init_max_peer_links(_adapter *adapter, u8 stack_conf) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + + mcfg->max_peer_links = RTW_MESH_MAX_PEER_LINKS; + + if (mcfg->max_peer_links > stack_conf) + mcfg->max_peer_links = stack_conf; +} + +void rtw_mesh_cfg_init_plink_timeout(_adapter *adapter, u32 stack_conf) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + + mcfg->plink_timeout = stack_conf; +} + +void rtw_mesh_init_mesh_info(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + + _rtw_memset(minfo, 0, sizeof(struct rtw_mesh_info)); + + rtw_mesh_plink_ctl_init(adapter); + + minfo->last_preq = rtw_get_current_time(); + /* minfo->last_sn_update = rtw_get_current_time(); */ + minfo->next_perr = rtw_get_current_time(); + + ATOMIC_SET(&minfo->mpaths, 0); + rtw_mesh_pathtbl_init(adapter); + + _rtw_init_queue(&minfo->mpath_tx_queue); + tasklet_init(&minfo->mpath_tx_tasklet + , (void(*)(unsigned long))mpath_tx_tasklet_hdl + , (unsigned long)adapter); + + rtw_mrc_init(adapter); + + _rtw_init_listhead(&minfo->preq_queue.list); + _rtw_spinlock_init(&minfo->mesh_preq_queue_lock); + + rtw_init_timer(&adapter->mesh_path_timer, adapter, rtw_ieee80211_mesh_path_timer, adapter); + rtw_init_timer(&adapter->mesh_path_root_timer, adapter, rtw_ieee80211_mesh_path_root_timer, adapter); + rtw_init_timer(&adapter->mesh_atlm_param_req_timer, adapter, rtw_mesh_atlm_param_req_timer, adapter); + _init_workitem(&adapter->mesh_work, rtw_mesh_work_hdl, NULL); +} + +void rtw_mesh_deinit_mesh_info(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + + tasklet_kill(&minfo->mpath_tx_tasklet); + rtw_mpath_tx_queue_flush(adapter); + _rtw_deinit_queue(&adapter->mesh_info.mpath_tx_queue); + + rtw_mrc_free(adapter); + + rtw_mesh_pathtbl_unregister(adapter); + + rtw_mesh_plink_ctl_deinit(adapter); + + _cancel_workitem_sync(&adapter->mesh_work); + _cancel_timer_ex(&adapter->mesh_path_timer); + _cancel_timer_ex(&adapter->mesh_path_root_timer); + _cancel_timer_ex(&adapter->mesh_atlm_param_req_timer); +} + +/** + * rtw_mesh_nexthop_resolve - lookup next hop; conditionally start path discovery + * + * @skb: 802.11 frame to be sent + * @sdata: network subif the frame will be sent through + * + * Lookup next hop for given skb and start path discovery if no + * forwarding information is found. + * + * Returns: 0 if the next hop was found and -ENOENT if the frame was queued. + * skb is freeed here if no mpath could be allocated. + */ +int rtw_mesh_nexthop_resolve(_adapter *adapter, + struct xmit_frame *xframe) +{ + struct pkt_attrib *attrib = &xframe->attrib; + struct rtw_mesh_path *mpath; + struct xmit_frame *xframe_to_free = NULL; + u8 *target_addr = attrib->mda; + int err = 0; + int ret = _SUCCESS; + + rtw_rcu_read_lock(); + err = rtw_mesh_nexthop_lookup(adapter, target_addr, attrib->msa, attrib->ra); + if (!err) + goto endlookup; + + /* no nexthop found, start resolving */ + mpath = rtw_mesh_path_lookup(adapter, target_addr); + if (!mpath) { + mpath = rtw_mesh_path_add(adapter, target_addr); + if (IS_ERR(mpath)) { + xframe->pkt = NULL; /* free pkt outside */ + rtw_mesh_path_discard_frame(adapter, xframe); + err = PTR_ERR(mpath); + ret = _FAIL; + goto endlookup; + } + } + + if (!(mpath->flags & RTW_MESH_PATH_RESOLVING)) + rtw_mesh_queue_preq(mpath, RTW_PREQ_Q_F_START); + + enter_critical_bh(&mpath->frame_queue.lock); + + if (mpath->frame_queue_len >= RTW_MESH_FRAME_QUEUE_LEN) { + xframe_to_free = LIST_CONTAINOR(get_next(get_list_head(&mpath->frame_queue)), struct xmit_frame, list); + rtw_list_delete(&(xframe_to_free->list)); + mpath->frame_queue_len--; + } + + rtw_list_insert_tail(&xframe->list, get_list_head(&mpath->frame_queue)); + mpath->frame_queue_len++; + + exit_critical_bh(&mpath->frame_queue.lock); + + ret = RTW_RA_RESOLVING; + if (xframe_to_free) + rtw_mesh_path_discard_frame(adapter, xframe_to_free); + +endlookup: + rtw_rcu_read_unlock(); + return ret; +} + +/** + * rtw_mesh_nexthop_lookup - put the appropriate next hop on a mesh frame. Calling + * this function is considered "using" the associated mpath, so preempt a path + * refresh if this mpath expires soon. + * + * @skb: 802.11 frame to be sent + * @sdata: network subif the frame will be sent through + * + * Returns: 0 if the next hop was found. Nonzero otherwise. + */ +int rtw_mesh_nexthop_lookup(_adapter *adapter, + const u8 *mda, const u8 *msa, u8 *ra) +{ + struct rtw_mesh_path *mpath; + struct sta_info *next_hop; + const u8 *target_addr = mda; + int err = -ENOENT; + struct registry_priv *registry_par = &adapter->registrypriv; + u8 peer_alive_based_preq = registry_par->peer_alive_based_preq; + BOOLEAN nexthop_alive = _TRUE; + + rtw_rcu_read_lock(); + mpath = rtw_mesh_path_lookup(adapter, target_addr); + + if (!mpath || !(mpath->flags & RTW_MESH_PATH_ACTIVE)) + goto endlookup; + + next_hop = rtw_rcu_dereference(mpath->next_hop); + if (next_hop) { + _rtw_memcpy(ra, next_hop->cmn.mac_addr, ETH_ALEN); + err = 0; + } + + if (peer_alive_based_preq && next_hop) + nexthop_alive = next_hop->alive; + + if (_rtw_memcmp(adapter_mac_addr(adapter), msa, ETH_ALEN) == _TRUE && + !(mpath->flags & RTW_MESH_PATH_RESOLVING) && + !(mpath->flags & RTW_MESH_PATH_FIXED)) { + u8 flags = RTW_PREQ_Q_F_START | RTW_PREQ_Q_F_REFRESH; + + if (peer_alive_based_preq && nexthop_alive == _FALSE) { + flags |= RTW_PREQ_Q_F_BCAST_PREQ; + rtw_mesh_queue_preq(mpath, flags); + } else if (rtw_time_after(rtw_get_current_time(), + mpath->exp_time - + rtw_ms_to_systime(adapter->mesh_cfg.path_refresh_time))) { + rtw_mesh_queue_preq(mpath, flags); + } + /* Avoid keeping trying unicast PREQ toward root, + when next_hop leaves */ + } else if (peer_alive_based_preq && + _rtw_memcmp(adapter_mac_addr(adapter), msa, ETH_ALEN) == _TRUE && + (mpath->flags & RTW_MESH_PATH_RESOLVING) && + !(mpath->flags & RTW_MESH_PATH_FIXED) && + !(mpath->flags & RTW_MESH_PATH_BCAST_PREQ) && + mpath->is_root && nexthop_alive == _FALSE) { + enter_critical_bh(&mpath->state_lock); + mpath->flags |= RTW_MESH_PATH_BCAST_PREQ; + exit_critical_bh(&mpath->state_lock); + } + +endlookup: + rtw_rcu_read_unlock(); + return err; +} + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC +static bool rtw_mesh_data_bmc_to_uc(_adapter *adapter + , const u8 *da, const u8 *sa, const u8 *mda, const u8 *msa + , u8 ae_need, const u8 *ori_ta, u8 mfwd_ttl + , u16 os_qid, _list *b2u_list, u8 *b2u_num, u32 *b2u_mseq) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct xmit_priv *xmitpriv = &adapter->xmitpriv; + _irqL irqL; + _list *head, *list; + struct sta_info *sta; + char b2u_sta_id[NUM_STA]; + u8 b2u_sta_num = 0; + bool bmc_need = _FALSE; + int i; + + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + head = &stapriv->asoc_list; + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + int stainfo_offset; + + sta = LIST_CONTAINOR(list, struct sta_info, asoc_list); + list = get_next(list); + + stainfo_offset = rtw_stainfo_offset(stapriv, sta); + if (stainfo_offset_valid(stainfo_offset)) + b2u_sta_id[b2u_sta_num++] = stainfo_offset; + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + + if (!b2u_sta_num) + goto exit; + + for (i = 0; i < b2u_sta_num; i++) { + struct xmit_frame *b2uframe; + struct pkt_attrib *attrib; + + sta = rtw_get_stainfo_by_offset(stapriv, b2u_sta_id[i]); + if (!(sta->state & WIFI_ASOC_STATE) + || _rtw_memcmp(sta->cmn.mac_addr, msa, ETH_ALEN) == _TRUE + || (ori_ta && _rtw_memcmp(sta->cmn.mac_addr, ori_ta, ETH_ALEN) == _TRUE) + || is_broadcast_mac_addr(sta->cmn.mac_addr) + || is_zero_mac_addr(sta->cmn.mac_addr)) + continue; + + b2uframe = rtw_alloc_xmitframe(xmitpriv, os_qid); + if (!b2uframe) { + bmc_need = _TRUE; + break; + } + + if ((*b2u_num)++ == 0 && !ori_ta) { + *b2u_mseq = (cpu_to_le32(adapter->mesh_info.mesh_seqnum)); + adapter->mesh_info.mesh_seqnum++; + } + + attrib = &b2uframe->attrib; + + attrib->mb2u = 1; + attrib->mseq = *b2u_mseq; + attrib->mfwd_ttl = ori_ta ? mfwd_ttl : 0; + _rtw_memcpy(attrib->ra, sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(attrib->ta, adapter_mac_addr(adapter), ETH_ALEN); + _rtw_memcpy(attrib->mda, mda, ETH_ALEN); + _rtw_memcpy(attrib->msa, msa, ETH_ALEN); + _rtw_memcpy(attrib->dst, da, ETH_ALEN); + _rtw_memcpy(attrib->src, sa, ETH_ALEN); + attrib->mesh_frame_mode = ae_need ? MESH_UCAST_PX_DATA : MESH_UCAST_DATA; + + rtw_list_insert_tail(&b2uframe->list, b2u_list); + } + +exit: + return bmc_need; +} + +void dump_mesh_b2u_flags(void *sel, _adapter *adapter) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + + RTW_PRINT_SEL(sel, "%4s %4s\n", "msrc", "mfwd"); + RTW_PRINT_SEL(sel, "0x%02x 0x%02x\n", mcfg->b2u_flags_msrc, mcfg->b2u_flags_mfwd); +} +#endif /* CONFIG_RTW_MESH_DATA_BMC_TO_UC */ + +int rtw_mesh_addr_resolve(_adapter *adapter, u16 os_qid, struct xmit_frame *xframe, _pkt *pkt, _list *b2u_list) +{ + struct pkt_file pktfile; + struct ethhdr etherhdr; + struct pkt_attrib *attrib; + struct rtw_mesh_path *mpath = NULL, *mppath = NULL; + u8 is_da_mcast; + u8 ae_need; +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + bool bmc_need = _TRUE; + u8 b2u_num = 0; + u32 b2u_mseq = 0; +#endif + int res = _SUCCESS; + + _rtw_open_pktfile(pkt, &pktfile); + if (_rtw_pktfile_read(&pktfile, (u8 *)ðerhdr, ETH_HLEN) != ETH_HLEN) { + res = _FAIL; + goto exit; + } + + xframe->pkt = pkt; +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + _rtw_init_listhead(b2u_list); +#endif + + is_da_mcast = IS_MCAST(etherhdr.h_dest); + if (!is_da_mcast) { + struct sta_info *next_hop; + bool mpp_lookup = 1; + + mpath = rtw_mesh_path_lookup(adapter, etherhdr.h_dest); + if (mpath) { + mpp_lookup = 0; + next_hop = rtw_rcu_dereference(mpath->next_hop); + if (!next_hop + || !(mpath->flags & (RTW_MESH_PATH_ACTIVE | RTW_MESH_PATH_RESOLVING)) + ) { + /* mpath is not valid, search mppath */ + mpp_lookup = 1; + } + } + + if (mpp_lookup) { + mppath = rtw_mpp_path_lookup(adapter, etherhdr.h_dest); + if (mppath) + mppath->exp_time = rtw_get_current_time(); + } + + if (mppath && mpath) + rtw_mesh_path_del(adapter, mpath->dst); + + ae_need = _rtw_memcmp(adapter_mac_addr(adapter), etherhdr.h_source, ETH_ALEN) == _FALSE + || (mppath && _rtw_memcmp(mppath->mpp, etherhdr.h_dest, ETH_ALEN) == _FALSE); + } else { + ae_need = _rtw_memcmp(adapter_mac_addr(adapter), etherhdr.h_source, ETH_ALEN) == _FALSE; + + #if CONFIG_RTW_MESH_DATA_BMC_TO_UC + if (rtw_msrc_b2u_policy_chk(adapter->mesh_cfg.b2u_flags_msrc, etherhdr.h_dest)) { + bmc_need = rtw_mesh_data_bmc_to_uc(adapter + , etherhdr.h_dest, etherhdr.h_source + , etherhdr.h_dest, adapter_mac_addr(adapter), ae_need, NULL, 0 + , os_qid, b2u_list, &b2u_num, &b2u_mseq); + if (bmc_need == _FALSE) { + res = RTW_BMC_NO_NEED; + goto exit; + } + } + #endif + } + + attrib = &xframe->attrib; + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + if (b2u_num) { + attrib->mb2u = 1; + attrib->mseq = b2u_mseq; + } else + attrib->mb2u = 0; +#endif + + attrib->mfwd_ttl = 0; + _rtw_memcpy(attrib->dst, etherhdr.h_dest, ETH_ALEN); + _rtw_memcpy(attrib->src, etherhdr.h_source, ETH_ALEN); + _rtw_memcpy(attrib->ta, adapter_mac_addr(adapter), ETH_ALEN); + + if (is_da_mcast) { + attrib->mesh_frame_mode = ae_need ? MESH_BMCAST_PX_DATA : MESH_BMCAST_DATA; + _rtw_memcpy(attrib->ra, attrib->dst, ETH_ALEN); + _rtw_memcpy(attrib->msa, adapter_mac_addr(adapter), ETH_ALEN); + } else { + attrib->mesh_frame_mode = ae_need ? MESH_UCAST_PX_DATA : MESH_UCAST_DATA; + _rtw_memcpy(attrib->mda, (mppath && ae_need) ? mppath->mpp : attrib->dst, ETH_ALEN); + _rtw_memcpy(attrib->msa, adapter_mac_addr(adapter), ETH_ALEN); + /* RA needs to be resolved */ + res = rtw_mesh_nexthop_resolve(adapter, xframe); + } + +exit: + return res; +} + +s8 rtw_mesh_tx_set_whdr_mctrl_len(u8 mesh_frame_mode, struct pkt_attrib *attrib) +{ + u8 ret = 0; + switch (mesh_frame_mode) { + case MESH_UCAST_DATA: + attrib->hdrlen = WLAN_HDR_A4_QOS_LEN; + /* mesh flag + mesh TTL + Mesh SN. no ext addr. */ + attrib->meshctrl_len = 6; + break; + case MESH_BMCAST_DATA: + attrib->hdrlen = WLAN_HDR_A3_QOS_LEN; + /* mesh flag + mesh TTL + Mesh SN. no ext addr. */ + attrib->meshctrl_len = 6; + break; + case MESH_UCAST_PX_DATA: + attrib->hdrlen = WLAN_HDR_A4_QOS_LEN; + /* mesh flag + mesh TTL + Mesh SN + extaddr1 + extaddr2. */ + attrib->meshctrl_len = 18; + break; + case MESH_BMCAST_PX_DATA: + attrib->hdrlen = WLAN_HDR_A3_QOS_LEN; + /* mesh flag + mesh TTL + Mesh SN + extaddr1 */ + attrib->meshctrl_len = 12; + break; + default: + RTW_WARN("Invalid mesh frame mode:%u\n", mesh_frame_mode); + ret = -1; + break; + } + + return ret; +} + +void rtw_mesh_tx_build_mctrl(_adapter *adapter, struct pkt_attrib *attrib, u8 *buf) +{ + struct rtw_ieee80211s_hdr *mctrl = (struct rtw_ieee80211s_hdr *)buf; + + _rtw_memset(mctrl, 0, XATTRIB_GET_MCTRL_LEN(attrib)); + + if (attrib->mfwd_ttl + #if CONFIG_RTW_MESH_DATA_BMC_TO_UC + || attrib->mb2u + #endif + ) { + #if CONFIG_RTW_MESH_DATA_BMC_TO_UC + if (!attrib->mfwd_ttl) + mctrl->ttl = adapter->mesh_cfg.dot11MeshTTL; + else + #endif + mctrl->ttl = attrib->mfwd_ttl; + + mctrl->seqnum = (cpu_to_le32(attrib->mseq)); + } else { + mctrl->ttl = adapter->mesh_cfg.dot11MeshTTL; + mctrl->seqnum = (cpu_to_le32(adapter->mesh_info.mesh_seqnum)); + adapter->mesh_info.mesh_seqnum++; + } + + switch (attrib->mesh_frame_mode){ + case MESH_UCAST_DATA: + case MESH_BMCAST_DATA: + break; + case MESH_UCAST_PX_DATA: + mctrl->flags |= MESH_FLAGS_AE_A5_A6; + _rtw_memcpy(mctrl->eaddr1, attrib->dst, ETH_ALEN); + _rtw_memcpy(mctrl->eaddr2, attrib->src, ETH_ALEN); + break; + case MESH_BMCAST_PX_DATA: + mctrl->flags |= MESH_FLAGS_AE_A4; + _rtw_memcpy(mctrl->eaddr1, attrib->src, ETH_ALEN); + break; + case MESH_MHOP_UCAST_ACT: + /* TBD */ + break; + case MESH_MHOP_BMCAST_ACT: + /* TBD */ + break; + default: + break; + } +} + +u8 rtw_mesh_tx_build_whdr(_adapter *adapter, struct pkt_attrib *attrib + , u16 *fctrl, struct rtw_ieee80211_hdr *whdr) +{ + switch (attrib->mesh_frame_mode) { + case MESH_UCAST_DATA: /* 1, 1, RA, TA, mDA(=DA), mSA(=SA) */ + case MESH_UCAST_PX_DATA: /* 1, 1, RA, TA, mDA, mSA, [DA, SA] */ + SetToDs(fctrl); + SetFrDs(fctrl); + _rtw_memcpy(whdr->addr1, attrib->ra, ETH_ALEN); + _rtw_memcpy(whdr->addr2, attrib->ta, ETH_ALEN); + _rtw_memcpy(whdr->addr3, attrib->mda, ETH_ALEN); + _rtw_memcpy(whdr->addr4, attrib->msa, ETH_ALEN); + break; + case MESH_BMCAST_DATA: /* 0, 1, RA(DA), TA, mSA(SA) */ + case MESH_BMCAST_PX_DATA: /* 0, 1, RA(DA), TA, mSA, [SA] */ + SetFrDs(fctrl); + _rtw_memcpy(whdr->addr1, attrib->ra, ETH_ALEN); + _rtw_memcpy(whdr->addr2, attrib->ta, ETH_ALEN); + _rtw_memcpy(whdr->addr3, attrib->msa, ETH_ALEN); + break; + case MESH_MHOP_UCAST_ACT: + /* TBD */ + RTW_INFO("MESH_MHOP_UCAST_ACT\n"); + break; + case MESH_MHOP_BMCAST_ACT: + /* TBD */ + RTW_INFO("MESH_MHOP_BMCAST_ACT\n"); + break; + default: + RTW_WARN("Invalid mesh frame mode\n"); + break; + } + + return 0; +} + +int rtw_mesh_rx_data_validate_hdr(_adapter *adapter, union recv_frame *rframe, struct sta_info **sta) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + u8 *whdr = get_recvframe_data(rframe); + u8 is_ra_bmc = 0; + u8 a4_shift = 0; + u8 ps; + u8 *qc; + u8 mps_mode = RTW_MESH_PS_UNKNOWN; + sint ret = _FAIL; + + if (!(MLME_STATE(adapter) & WIFI_ASOC_STATE)) + goto exit; + + if (!rattrib->qos) + goto exit; + + switch (rattrib->to_fr_ds) { + case 2: + if (!IS_MCAST(GetAddr1Ptr(whdr))) + goto exit; + *sta = rtw_get_stainfo(stapriv, get_addr2_ptr(whdr)); + if (*sta == NULL) { + ret = _SUCCESS; /* return _SUCCESS to drop at sta checking */ + goto exit; + } + _rtw_memcpy(rattrib->ra, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->ta, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->mda, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->msa, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->dst, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->src, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking mesh ctrl field */ + _rtw_memcpy(rattrib->bssid, get_addr2_ptr(whdr), ETH_ALEN); + is_ra_bmc = 1; + break; + case 3: + if (IS_MCAST(GetAddr1Ptr(whdr))) + goto exit; + *sta = rtw_get_stainfo(stapriv, get_addr2_ptr(whdr)); + if (*sta == NULL) { + ret = _SUCCESS; /* return _SUCCESS to drop at sta checking */ + goto exit; + } + _rtw_memcpy(rattrib->ra, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->ta, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->mda, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->msa, GetAddr4Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->dst, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking mesh ctrl field */ + _rtw_memcpy(rattrib->src, GetAddr4Ptr(whdr), ETH_ALEN); /* may change after checking mesh ctrl field */ + _rtw_memcpy(rattrib->bssid, get_addr2_ptr(whdr), ETH_ALEN); + a4_shift = ETH_ALEN; + break; + default: + goto exit; + } + + qc = whdr + WLAN_HDR_A3_LEN + a4_shift; + ps = GetPwrMgt(whdr); + mps_mode = ps ? (is_ra_bmc || (get_mps_lv(qc)) ? RTW_MESH_PS_DSLEEP : RTW_MESH_PS_LSLEEP) : RTW_MESH_PS_ACTIVE; + + if (ps) { + if (!((*sta)->state & WIFI_SLEEP_STATE)) + stop_sta_xmit(adapter, *sta); + } else { + if ((*sta)->state & WIFI_SLEEP_STATE) + wakeup_sta_to_xmit(adapter, *sta); + } + + if (is_ra_bmc) + (*sta)->nonpeer_mps = mps_mode; + else { + (*sta)->peer_mps = mps_mode; + if (mps_mode != RTW_MESH_PS_ACTIVE && (*sta)->nonpeer_mps == RTW_MESH_PS_ACTIVE) + (*sta)->nonpeer_mps = RTW_MESH_PS_DSLEEP; + } + + if (get_frame_sub_type(whdr) & BIT(6)) { + /* No data, will not indicate to upper layer, temporily count it here */ + count_rx_stats(adapter, rframe, *sta); + ret = RTW_RX_HANDLED; + goto exit; + } + + rattrib->mesh_ctrl_present = get_mctrl_present(qc) ? 1 : 0; + if (!rattrib->mesh_ctrl_present) + goto exit; + + ret = _SUCCESS; + +exit: + return ret; +} + +int rtw_mesh_rx_data_validate_mctrl(_adapter *adapter, union recv_frame *rframe + , const struct rtw_ieee80211s_hdr *mctrl, const u8 *mda, const u8 *msa + , u8 *mctrl_len + , const u8 **da, const u8 **sa) +{ + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + u8 mlen; + u8 ae; + int ret = _SUCCESS; + + ae = mctrl->flags & MESH_FLAGS_AE; + mlen = ae_to_mesh_ctrl_len[ae]; + switch (rattrib->to_fr_ds) { + case 2: + *da = mda; + if (ae == MESH_FLAGS_AE_A4) + *sa = mctrl->eaddr1; + else if (ae == 0) + *sa = msa; + else + ret = _FAIL; + break; + case 3: + if (ae == MESH_FLAGS_AE_A5_A6) { + *da = mctrl->eaddr1; + *sa = mctrl->eaddr2; + } else if (ae == 0) { + *da = mda; + *sa = msa; + } else + ret = _FAIL; + break; + default: + ret = _FAIL; + } + + if (ret == _FAIL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" invalid tfDS:%u AE:%u combination ra="MAC_FMT" ta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), rattrib->to_fr_ds, ae, MAC_ARG(rattrib->ra), MAC_ARG(rattrib->ta)); + #endif + *mctrl_len = 0; + } else + *mctrl_len = mlen; + + return ret; +} + +inline int rtw_mesh_rx_validate_mctrl_non_amsdu(_adapter *adapter, union recv_frame *rframe) +{ + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + const u8 *da, *sa; + int ret; + + ret = rtw_mesh_rx_data_validate_mctrl(adapter, rframe + , (struct rtw_ieee80211s_hdr *)(get_recvframe_data(rframe) + rattrib->hdrlen + rattrib->iv_len) + , rattrib->mda, rattrib->msa + , &rattrib->mesh_ctrl_len + , &da, &sa); + + if (ret == _SUCCESS) { + _rtw_memcpy(rattrib->dst, da, ETH_ALEN); + _rtw_memcpy(rattrib->src, sa, ETH_ALEN); + } + + return ret; +} + +/** + * rtw_mesh_rx_nexthop_resolve - lookup next hop; conditionally start path discovery + * + * @skb: 802.11 frame to be sent + * @sdata: network subif the frame will be sent through + * + * Lookup next hop for given skb and start path discovery if no + * forwarding information is found. + * + * Returns: 0 if the next hop was found and -ENOENT if the frame was queued. + * skb is freeed here if no mpath could be allocated. + */ +static int rtw_mesh_rx_nexthop_resolve(_adapter *adapter, + const u8 *mda, const u8 *msa, u8 *ra) +{ + struct rtw_mesh_path *mpath; + struct xmit_frame *xframe_to_free = NULL; + int err = 0; + int ret = _SUCCESS; + + rtw_rcu_read_lock(); + err = rtw_mesh_nexthop_lookup(adapter, mda, msa, ra); + if (!err) + goto endlookup; + + /* no nexthop found, start resolving */ + mpath = rtw_mesh_path_lookup(adapter, mda); + if (!mpath) { + mpath = rtw_mesh_path_add(adapter, mda); + if (IS_ERR(mpath)) { + err = PTR_ERR(mpath); + ret = _FAIL; + goto endlookup; + } + } + + if (!(mpath->flags & RTW_MESH_PATH_RESOLVING)) + rtw_mesh_queue_preq(mpath, RTW_PREQ_Q_F_START); + + ret = _FAIL; + +endlookup: + rtw_rcu_read_unlock(); + return ret; +} + +#define RTW_MESH_DECACHE_BMC 1 +#define RTW_MESH_DECACHE_UC 0 + +#define RTW_MESH_FORWARD_MDA_SELF_COND 0 +#define DBG_RTW_MESH_FORWARD_MDA_SELF_COND 0 +int rtw_mesh_rx_msdu_act_check(union recv_frame *rframe + , const u8 *mda, const u8 *msa + , const u8 *da, const u8 *sa + , struct rtw_ieee80211s_hdr *mctrl + , u8 *msdu, enum rtw_rx_llc_hdl llc_hdl + , struct xmit_frame **fwd_frame, _list *b2u_list) +{ + _adapter *adapter = rframe->u.hdr.adapter; + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + struct rtw_mesh_path *mppath; + u8 is_mda_bmc = IS_MCAST(mda); + u8 is_mda_self = !is_mda_bmc && _rtw_memcmp(mda, adapter_mac_addr(adapter), ETH_ALEN); + u16 os_qid; + struct xmit_frame *xframe; + struct pkt_attrib *xattrib; + u8 fwd_ra[ETH_ALEN] = {0}; + u8 fwd_mpp[ETH_ALEN] = {0}; /* forward to other gate */ + u32 fwd_mseq; + int act = 0; + u8 ae_need; +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + bool bmc_need = _TRUE; + u8 b2u_num = 0; +#endif + + /* fwd info lifetime update */ + #if 0 + if (!is_mda_self) + mDA(A3) fwinfo.lifetime + mSA(A4) fwinfo.lifetime + Precursor-to-mDA(A2) fwinfo.lifetime + #endif + + /* update/create pxoxy info for SA, mSA */ + if ((mctrl->flags & MESH_FLAGS_AE) + && sa != msa && _rtw_memcmp(sa, msa, ETH_ALEN) == _FALSE + ) { + const u8 *proxied_addr = sa; + const u8 *mpp_addr = msa; + + rtw_rcu_read_lock(); + mppath = rtw_mpp_path_lookup(adapter, proxied_addr); + if (!mppath) + rtw_mpp_path_add(adapter, proxied_addr, mpp_addr); + else { + enter_critical_bh(&mppath->state_lock); + if (_rtw_memcmp(mppath->mpp, mpp_addr, ETH_ALEN) == _FALSE) + _rtw_memcpy(mppath->mpp, mpp_addr, ETH_ALEN); + mppath->exp_time = rtw_get_current_time(); + exit_critical_bh(&mppath->state_lock); + } + rtw_rcu_read_unlock(); + } + + /* mSA is self, need no further process */ + if (_rtw_memcmp(msa, adapter_mac_addr(adapter), ETH_ALEN) == _TRUE) + goto exit; + + fwd_mseq = le32_to_cpu(mctrl->seqnum); + + /* check duplicate MSDU from mSA */ + if (((RTW_MESH_DECACHE_BMC && is_mda_bmc) + || (RTW_MESH_DECACHE_UC && !is_mda_bmc)) + && rtw_mesh_decache(adapter, msa, fwd_mseq) + ) { + minfo->mshstats.dropped_frames_duplicate++; + goto exit; + } + + if (is_mda_bmc) { + /* mDA is bmc addr */ + act |= RTW_RX_MSDU_ACT_INDICATE; + if (!mcfg->dot11MeshForwarding) + goto exit; + goto fwd_chk; + + } else if (!is_mda_self) { + /* mDA is unicast but not self */ + if (!mcfg->dot11MeshForwarding) { + rtw_mesh_path_error_tx(adapter + , adapter->mesh_cfg.element_ttl + , mda, 0 + , WLAN_REASON_MESH_PATH_NOFORWARD + , rattrib->ta + ); + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" mDA("MAC_FMT") not self, !dot11MeshForwarding\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(mda)); + #endif + goto exit; + } + + if (rtw_mesh_rx_nexthop_resolve(adapter, mda, msa, fwd_ra) != _SUCCESS) { + /* mDA is unknown */ + rtw_mesh_path_error_tx(adapter + , adapter->mesh_cfg.element_ttl + , mda, 0 + , WLAN_REASON_MESH_PATH_NOFORWARD + , rattrib->ta + ); + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" mDA("MAC_FMT") unknown\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(mda)); + #endif + minfo->mshstats.dropped_frames_no_route++; + goto exit; + + } else { + /* mDA is known in fwd info */ + #if 0 + if (TA is not in precursors) + goto exit; + #endif + goto fwd_chk; + } + + } else { + /* mDA is self */ + #if RTW_MESH_FORWARD_MDA_SELF_COND + if (da == mda + || _rtw_memcmp(da, adapter_mac_addr(adapter), ETH_ALEN) + ) { + /* DA is self, indicate */ + act |= RTW_RX_MSDU_ACT_INDICATE; + goto exit; + } + + if (rtw_get_iface_by_macddr(adapter, da)) { + /* DA is buddy, indicate */ + act |= RTW_RX_MSDU_ACT_INDICATE; + #if DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO(FUNC_ADPT_FMT" DA("MAC_FMT") is buddy("ADPT_FMT")\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da), ADPT_ARG(rtw_get_iface_by_macddr(adapter, da))); + #endif + goto exit; + } + + /* DA is not self or buddy */ + if (rtw_mesh_nexthop_lookup(adapter, da, msa, fwd_ra) == 0) { + /* DA is known in fwd info */ + if (!mcfg->dot11MeshForwarding) { + /* path error to? */ + #if defined(DBG_RX_DROP_FRAME) || DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" DA("MAC_FMT") not self, !dot11MeshForwarding\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da)); + #endif + goto exit; + } + mda = da; + #if DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO(FUNC_ADPT_FMT" fwd to DA("MAC_FMT"), fwd_RA("MAC_FMT")\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da), MAC_ARG(fwd_ra)); + #endif + goto fwd_chk; + } + + rtw_rcu_read_lock(); + mppath = rtw_mpp_path_lookup(adapter, da); + if (mppath) { + if (_rtw_memcmp(mppath->mpp, adapter_mac_addr(adapter), ETH_ALEN) == _FALSE) { + /* DA is proxied by others */ + if (!mcfg->dot11MeshForwarding) { + /* path error to? */ + #if defined(DBG_RX_DROP_FRAME) || DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" DA("MAC_FMT") is proxied by ("MAC_FMT"), !dot11MeshForwarding\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da), MAC_ARG(mppath->mpp)); + #endif + rtw_rcu_read_unlock(); + goto exit; + } + _rtw_memcpy(fwd_mpp, mppath->mpp, ETH_ALEN); + mda = fwd_mpp; + msa = adapter_mac_addr(adapter); + rtw_rcu_read_unlock(); + + /* resolve RA */ + if (rtw_mesh_nexthop_lookup(adapter, mda, msa, fwd_ra) != 0) { + minfo->mshstats.dropped_frames_no_route++; + #if defined(DBG_RX_DROP_FRAME) || DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" DA("MAC_FMT") is proxied by ("MAC_FMT"), RA resolve fail\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da), MAC_ARG(mppath->mpp)); + #endif + goto exit; + } + #if DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO(FUNC_ADPT_FMT" DA("MAC_FMT") is proxied by ("MAC_FMT"), fwd_RA("MAC_FMT")\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da), MAC_ARG(mppath->mpp), MAC_ARG(fwd_ra)); + #endif + goto fwd_chk; /* forward to other gate */ + } else { + #if DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO(FUNC_ADPT_FMT" DA("MAC_FMT") is proxied by self\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da)); + #endif + } + } + rtw_rcu_read_unlock(); + + if (!mppath) { + #if DBG_RTW_MESH_FORWARD_MDA_SELF_COND + RTW_INFO(FUNC_ADPT_FMT" DA("MAC_FMT") unknown\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da)); + #endif + /* DA is unknown */ + #if 0 /* TODO: flags with AE bit */ + rtw_mesh_path_error_tx(adapter + , adapter->mesh_cfg.element_ttl + , mda, adapter->mesh_info.last_sn_update + , WLAN_REASON_MESH_PATH_NOPROXY + , msa + ); + #endif + } + + /* + * indicate to DS for both cases: + * 1.) DA is proxied by self + * 2.) DA is unknown + */ + #endif /* RTW_MESH_FORWARD_MDA_SELF_COND */ + act |= RTW_RX_MSDU_ACT_INDICATE; + goto exit; + } + +fwd_chk: + + if (adapter->stapriv.asoc_list_cnt <= 1) + goto exit; + + if (mctrl->ttl == 1) { + minfo->mshstats.dropped_frames_ttl++; + if (!act) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" ttl reaches 0, not forwarding\n" + , FUNC_ADPT_ARG(adapter)); + #endif + } + goto exit; + } + + os_qid = rtw_os_recv_select_queue(msdu, llc_hdl); + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + _rtw_init_listhead(b2u_list); +#endif + + ae_need = _rtw_memcmp(da , mda, ETH_ALEN) == _FALSE + || _rtw_memcmp(sa , msa, ETH_ALEN) == _FALSE; + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + if (is_mda_bmc + && rtw_mfwd_b2u_policy_chk(mcfg->b2u_flags_mfwd, mda, rattrib->to_fr_ds == 3) + ) { + bmc_need = rtw_mesh_data_bmc_to_uc(adapter + , da, sa, mda, msa, ae_need, rframe->u.hdr.psta->cmn.mac_addr, mctrl->ttl - 1 + , os_qid, b2u_list, &b2u_num, &fwd_mseq); + } + + if (bmc_need == _TRUE) +#endif + { + xframe = rtw_alloc_xmitframe(&adapter->xmitpriv, os_qid); + if (!xframe) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME "FUNC_ADPT_FMT" rtw_alloc_xmitframe fail\n" + , FUNC_ADPT_ARG(adapter)); + #endif + goto exit; + } + + xattrib = &xframe->attrib; + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + if (b2u_num) + xattrib->mb2u = 1; + else + xattrib->mb2u = 0; +#endif + xattrib->mfwd_ttl = mctrl->ttl - 1; + xattrib->mseq = fwd_mseq; + _rtw_memcpy(xattrib->dst, da, ETH_ALEN); + _rtw_memcpy(xattrib->src, sa, ETH_ALEN); + _rtw_memcpy(xattrib->mda, mda, ETH_ALEN); + _rtw_memcpy(xattrib->msa, msa, ETH_ALEN); + _rtw_memcpy(xattrib->ta, adapter_mac_addr(adapter), ETH_ALEN); + + if (is_mda_bmc) { + xattrib->mesh_frame_mode = ae_need ? MESH_BMCAST_PX_DATA : MESH_BMCAST_DATA; + _rtw_memcpy(xattrib->ra, mda, ETH_ALEN); + } else { + xattrib->mesh_frame_mode = ae_need ? MESH_UCAST_PX_DATA : MESH_UCAST_DATA; + _rtw_memcpy(xattrib->ra, fwd_ra, ETH_ALEN); + } + + *fwd_frame = xframe; + } + + act |= RTW_RX_MSDU_ACT_FORWARD; + if (is_mda_bmc) + minfo->mshstats.fwded_mcast++; + else + minfo->mshstats.fwded_unicast++; + minfo->mshstats.fwded_frames++; + +exit: + return act; +} + +void dump_mesh_stats(void *sel, _adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mesh_stats *stats = &minfo->mshstats; + + RTW_PRINT_SEL(sel, "fwd_bmc:%u\n", stats->fwded_mcast); + RTW_PRINT_SEL(sel, "fwd_uc:%u\n", stats->fwded_unicast); + + RTW_PRINT_SEL(sel, "drop_ttl:%u\n", stats->dropped_frames_ttl); + RTW_PRINT_SEL(sel, "drop_no_route:%u\n", stats->dropped_frames_no_route); + RTW_PRINT_SEL(sel, "drop_congestion:%u\n", stats->dropped_frames_congestion); + RTW_PRINT_SEL(sel, "drop_dup:%u\n", stats->dropped_frames_duplicate); + + RTW_PRINT_SEL(sel, "mrc_del_qlen:%u\n", stats->mrc_del_qlen); +} +#endif /* CONFIG_RTW_MESH */ + diff --git a/core/mesh/rtw_mesh.h b/core/mesh/rtw_mesh.h new file mode 100644 index 0000000..5410ba4 --- /dev/null +++ b/core/mesh/rtw_mesh.h @@ -0,0 +1,537 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifndef __RTW_MESH_H_ +#define __RTW_MESH_H_ + +#ifndef CONFIG_AP_MODE + #error "CONFIG_RTW_MESH can't be enabled when CONFIG_AP_MODE is not defined\n" +#endif + +#define RTW_MESH_TTL 31 +#define RTW_MESH_PERR_MIN_INT 100 +#define RTW_MESH_DEFAULT_ELEMENT_TTL 31 +#define RTW_MESH_RANN_INTERVAL 5000 +#define RTW_MESH_PATH_TO_ROOT_TIMEOUT 6000 +#define RTW_MESH_DIAM_TRAVERSAL_TIME 50 +#define RTW_MESH_PATH_TIMEOUT 5000 +#define RTW_MESH_PREQ_MIN_INT 10 +#define RTW_MESH_MAX_PREQ_RETRIES 4 +#define RTW_MESH_MIN_DISCOVERY_TIMEOUT (2 * RTW_MESH_DIAM_TRAVERSAL_TIME) +#define RTW_MESH_ROOT_CONFIRMATION_INTERVAL 2000 +#define RTW_MESH_PATH_REFRESH_TIME 1000 +#define RTW_MESH_ROOT_INTERVAL 5000 + +#define RTW_MESH_SANE_METRIC_DELTA 100 +#define RTW_MESH_MAX_ROOT_ADD_CHK_CNT 2 + +#define RTW_MESH_PLINK_UNKNOWN 0 +#define RTW_MESH_PLINK_LISTEN 1 +#define RTW_MESH_PLINK_OPN_SNT 2 +#define RTW_MESH_PLINK_OPN_RCVD 3 +#define RTW_MESH_PLINK_CNF_RCVD 4 +#define RTW_MESH_PLINK_ESTAB 5 +#define RTW_MESH_PLINK_HOLDING 6 +#define RTW_MESH_PLINK_BLOCKED 7 + +extern const char *_rtw_mesh_plink_str[]; +#define rtw_mesh_plink_str(s) ((s <= RTW_MESH_PLINK_BLOCKED) ? _rtw_mesh_plink_str[s] : _rtw_mesh_plink_str[RTW_MESH_PLINK_UNKNOWN]) + +#define RTW_MESH_PS_UNKNOWN 0 +#define RTW_MESH_PS_ACTIVE 1 +#define RTW_MESH_PS_LSLEEP 2 +#define RTW_MESH_PS_DSLEEP 3 + +extern const char *_rtw_mesh_ps_str[]; +#define rtw_mesh_ps_str(mps) ((mps <= RTW_MESH_PS_DSLEEP) ? _rtw_mesh_ps_str[mps] : _rtw_mesh_ps_str[RTW_MESH_PS_UNKNOWN]) + +#define GET_MESH_CONF_ELE_PATH_SEL_PROTO_ID(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 0, 0, 8) +#define GET_MESH_CONF_ELE_PATH_SEL_METRIC_ID(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 1, 0, 8) +#define GET_MESH_CONF_ELE_CONGEST_CTRL_MODE_ID(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 2, 0, 8) +#define GET_MESH_CONF_ELE_SYNC_METHOD_ID(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 3, 0, 8) +#define GET_MESH_CONF_ELE_AUTH_PROTO_ID(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 4, 0, 8) + +#define GET_MESH_CONF_ELE_MESH_FORMATION(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 5, 0, 8) +#define GET_MESH_CONF_ELE_CTO_MGATE(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 5, 0, 1) +#define GET_MESH_CONF_ELE_NUM_OF_PEERINGS(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 5, 1, 6) +#define GET_MESH_CONF_ELE_CTO_AS(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 5, 7, 1) + +#define GET_MESH_CONF_ELE_MESH_CAP(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 0, 8) +#define GET_MESH_CONF_ELE_ACCEPT_PEERINGS(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 0, 1) +#define GET_MESH_CONF_ELE_MCCA_SUP(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 1, 1) +#define GET_MESH_CONF_ELE_MCCA_EN(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 2, 1) +#define GET_MESH_CONF_ELE_FORWARDING(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 3, 1) +#define GET_MESH_CONF_ELE_MBCA_EN(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 4, 1) +#define GET_MESH_CONF_ELE_TBTT_ADJ(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 5, 1) +#define GET_MESH_CONF_ELE_PS_LEVEL(_iec) LE_BITS_TO_1BYTE(((u8 *)(_iec)) + 6, 6, 1) + +#define SET_MESH_CONF_ELE_PATH_SEL_PROTO_ID(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 0, 0, 8, _val) +#define SET_MESH_CONF_ELE_PATH_SEL_METRIC_ID(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 1, 0, 8, _val) +#define SET_MESH_CONF_ELE_CONGEST_CTRL_MODE_ID(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 2, 0, 8, _val) +#define SET_MESH_CONF_ELE_SYNC_METHOD_ID(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 3, 0, 8, _val) +#define SET_MESH_CONF_ELE_AUTH_PROTO_ID(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 4, 0, 8, _val) + +#define SET_MESH_CONF_ELE_CTO_MGATE(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 5, 0, 1, _val) +#define SET_MESH_CONF_ELE_NUM_OF_PEERINGS(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 5, 1, 6, _val) +#define SET_MESH_CONF_ELE_CTO_AS(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 5, 7, 1, _val) + +#define SET_MESH_CONF_ELE_ACCEPT_PEERINGS(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 0, 1, _val) +#define SET_MESH_CONF_ELE_MCCA_SUP(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 1, 1, _val) +#define SET_MESH_CONF_ELE_MCCA_EN(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 2, 1, _val) +#define SET_MESH_CONF_ELE_FORWARDING(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 3, 1, _val) +#define SET_MESH_CONF_ELE_MBCA_EN(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 4, 1, _val) +#define SET_MESH_CONF_ELE_TBTT_ADJ(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 5, 1, _val) +#define SET_MESH_CONF_ELE_PS_LEVEL(_iec, _val) SET_BITS_TO_LE_1BYTE(((u8 *)(_iec)) + 6, 6, 1, _val) + +/* Mesh flags */ +#define MESH_FLAGS_AE 0x3 /* mask */ +#define MESH_FLAGS_AE_A4 0x1 +#define MESH_FLAGS_AE_A5_A6 0x2 + +/* Max number of paths */ +#define RTW_MESH_MAX_PATHS 1024 + +#define RTW_PREQ_Q_F_START 0x1 +#define RTW_PREQ_Q_F_REFRESH 0x2 +#define RTW_PREQ_Q_F_CHK 0x4 +#define RTW_PREQ_Q_F_PEER_AKA 0x8 +#define RTW_PREQ_Q_F_BCAST_PREQ 0x10 /* force path_dicover using broadcast */ +struct rtw_mesh_preq_queue { + _list list; + u8 dst[ETH_ALEN]; + u8 flags; +}; + +extern const u8 ae_to_mesh_ctrl_len[]; + +enum mesh_frame_type { + MESH_UCAST_DATA = 0x0, + MESH_BMCAST_DATA = 0x1, + MESH_UCAST_PX_DATA = 0x2, + MESH_BMCAST_PX_DATA = 0x3, + MESH_MHOP_UCAST_ACT = 0x4, + MESH_MHOP_BMCAST_ACT = 0x5, +}; + +enum mpath_sel_frame_type { + MPATH_PREQ = 0, + MPATH_PREP, + MPATH_PERR, + MPATH_RANN +}; + +/** + * enum rtw_mesh_deferred_task_flags - mesh deferred tasks + * + * + * + * @RTW_MESH_WORK_HOUSEKEEPING: run the periodic mesh housekeeping tasks + * @RTW_MESH_WORK_ROOT: the mesh root station needs to send a frame + * @RTW_MESH_WORK_DRIFT_ADJUST: time to compensate for clock drift relative to other + * mesh nodes + * @RTW_MESH_WORK_MBSS_CHANGED: rebuild beacon and notify driver of BSS changes + */ +enum rtw_mesh_deferred_task_flags { + RTW_MESH_WORK_HOUSEKEEPING, + RTW_MESH_WORK_ROOT, + RTW_MESH_WORK_DRIFT_ADJUST, + RTW_MESH_WORK_MBSS_CHANGED, +}; + +#define RTW_MESH_MAX_PEER_CANDIDATES 15 /* aid consideration */ +#define RTW_MESH_MAX_PEER_LINKS 8 +#define RTW_MESH_PEER_LINK_TIMEOUT 20 + +#define RTW_MESH_PEER_CONF_DISABLED 0 /* special time value means no confirmation ongoing */ +#if CONFIG_RTW_MESH_PEER_BLACKLIST +#define IS_PEER_CONF_DISABLED(plink) ((plink)->peer_conf_end_time == RTW_MESH_PEER_CONF_DISABLED) +#define IS_PEER_CONF_TIMEOUT(plink)(!IS_PEER_CONF_DISABLED(plink) && rtw_time_after(rtw_get_current_time(), (plink)->peer_conf_end_time)) +#define SET_PEER_CONF_DISABLED(plink) (plink)->peer_conf_end_time = RTW_MESH_PEER_CONF_DISABLED +#define SET_PEER_CONF_END_TIME(plink, timeout_ms) \ + do { \ + (plink)->peer_conf_end_time = rtw_get_current_time() + rtw_ms_to_systime(timeout_ms); \ + if ((plink)->peer_conf_end_time == RTW_MESH_PEER_CONF_DISABLED) \ + (plink)->peer_conf_end_time++; \ + } while (0) +#else +#define IS_PEER_CONF_DISABLED(plink) 1 +#define IS_PEER_CONF_TIMEOUT(plink) 0 +#define SET_PEER_CONF_DISABLED(plink) do {} while (0) +#define SET_PEER_CONF_END_TIME(plink, timeout_ms) do {} while (0) +#endif /* CONFIG_RTW_MESH_PEER_BLACKLIST */ + +#define RTW_MESH_CTO_MGATE_CONF_DISABLED 0 /* special time value means no confirmation ongoing */ +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST +#define IS_CTO_MGATE_CONF_DISABLED(plink) ((plink)->cto_mgate_conf_end_time == RTW_MESH_CTO_MGATE_CONF_DISABLED) +#define IS_CTO_MGATE_CONF_TIMEOUT(plink)(!IS_CTO_MGATE_CONF_DISABLED(plink) && rtw_time_after(rtw_get_current_time(), (plink)->cto_mgate_conf_end_time)) +#define SET_CTO_MGATE_CONF_DISABLED(plink) (plink)->cto_mgate_conf_end_time = RTW_MESH_CTO_MGATE_CONF_DISABLED +#define SET_CTO_MGATE_CONF_END_TIME(plink, timeout_ms) \ + do { \ + (plink)->cto_mgate_conf_end_time = rtw_get_current_time() + rtw_ms_to_systime(timeout_ms); \ + if ((plink)->cto_mgate_conf_end_time == RTW_MESH_CTO_MGATE_CONF_DISABLED) \ + (plink)->cto_mgate_conf_end_time++; \ + } while (0) +#else +#define IS_CTO_MGATE_CONF_DISABLED(plink) 1 +#define IS_CTO_MGATE_CONF_TIMEOUT(plink) 0 +#define SET_CTO_MGATE_CONF_DISABLED(plink) do {} while (0) +#define SET_CTO_MGATE_CONF_END_TIME(plink, timeout_ms) do {} while (0) +#endif /* CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST */ + +struct mesh_plink_ent { + u8 valid; + u8 addr[ETH_ALEN]; + u8 plink_state; + +#ifdef CONFIG_RTW_MESH_AEK + u8 aek_valid; + u8 aek[32]; +#endif + + u16 llid; + u16 plid; +#ifndef CONFIG_RTW_MESH_DRIVER_AID + u16 aid; /* aid assigned from upper layer */ +#endif + u16 peer_aid; /* aid assigned from peer */ + + u8 chosen_pmk[16]; + +#ifdef CONFIG_RTW_MESH_AEK + u8 sel_pcs[4]; + u8 l_nonce[32]; + u8 p_nonce[32]; +#endif + +#ifdef CONFIG_RTW_MESH_DRIVER_AID + u8 *tx_conf_ies; + u16 tx_conf_ies_len; +#endif + u8 *rx_conf_ies; + u16 rx_conf_ies_len; + + struct wlan_network *scanned; + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + systime peer_conf_end_time; +#endif +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + systime cto_mgate_conf_end_time; +#endif +}; + +#ifdef CONFIG_RTW_MESH_AEK +#define MESH_PLINK_AEK_VALID(ent) ent->aek_valid +#else +#define MESH_PLINK_AEK_VALID(ent) 0 +#endif + +struct mesh_plink_pool { + _lock lock; + u8 num; /* current ent being used */ + struct mesh_plink_ent ent[RTW_MESH_MAX_PEER_CANDIDATES]; + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + u8 acnode_rsvd; +#endif + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + _queue peer_blacklist; +#endif +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + _queue cto_mgate_blacklist; +#endif +}; + +struct mesh_peer_sel_policy { + u32 scanr_exp_ms; + +#if CONFIG_RTW_MESH_ACNODE_PREVENT + u8 acnode_prevent; + u32 acnode_conf_timeout_ms; + u32 acnode_notify_timeout_ms; +#endif + +#if CONFIG_RTW_MESH_OFFCH_CAND + u8 offch_cand; + u32 offch_find_int_ms; /* 0 means no offch find triggerred by driver self*/ +#endif + +#if CONFIG_RTW_MESH_PEER_BLACKLIST + u32 peer_conf_timeout_ms; + u32 peer_blacklist_timeout_ms; +#endif + +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST + u8 cto_mgate_require; + u32 cto_mgate_conf_timeout_ms; + u32 cto_mgate_blacklist_timeout_ms; +#endif +}; + +/* b2u flags */ +#define RTW_MESH_B2U_ALL BIT0 +#define RTW_MESH_B2U_GA_UCAST BIT1 /* Group addressed unicast frame, forward only */ +#define RTW_MESH_B2U_BCAST BIT2 +#define RTW_MESH_B2U_IP_MCAST BIT3 + +#define rtw_msrc_b2u_policy_chk(flags, mda) ( \ + (flags & RTW_MESH_B2U_ALL) \ + || ((flags & RTW_MESH_B2U_BCAST) && is_broadcast_mac_addr(mda)) \ + || ((flags & RTW_MESH_B2U_IP_MCAST) && (IP_MCAST_MAC(mda) || ICMPV6_MCAST_MAC(mda))) \ + ) + +#define rtw_mfwd_b2u_policy_chk(flags, mda, ucst) ( \ + (flags & RTW_MESH_B2U_ALL) \ + || ((flags & RTW_MESH_B2U_GA_UCAST) && ucst) \ + || ((flags & RTW_MESH_B2U_BCAST) && is_broadcast_mac_addr(mda)) \ + || ((flags & RTW_MESH_B2U_IP_MCAST) && (IP_MCAST_MAC(mda) || ICMPV6_MCAST_MAC(mda))) \ + ) + +/** + * @sane_metric_delta: Controlling if trigger additional path check mechanism + * @max_root_add_chk_cnt: The retry cnt to send additional root confirmation + * PREQ through old(last) path + */ +struct rtw_mesh_cfg { + u8 max_peer_links; /* peering limit */ + u32 plink_timeout; /* seconds */ + + u8 dot11MeshTTL; + u8 element_ttl; + u32 path_refresh_time; + u16 dot11MeshHWMPpreqMinInterval; + u16 dot11MeshHWMPnetDiameterTraversalTime; + u32 dot11MeshHWMPactivePathTimeout; + u8 dot11MeshHWMPmaxPREQretries; + u16 min_discovery_timeout; + u16 dot11MeshHWMPconfirmationInterval; + u16 dot11MeshHWMPperrMinInterval; + u8 dot11MeshHWMPRootMode; + BOOLEAN dot11MeshForwarding; + s32 rssi_threshold; /* in dBm, 0: no specified */ + u16 dot11MeshHWMPRannInterval; + BOOLEAN dot11MeshGateAnnouncementProtocol; + u32 dot11MeshHWMPactivePathToRootTimeout; + u16 dot11MeshHWMProotInterval; + u8 path_gate_timeout_factor; +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + u16 sane_metric_delta; + u8 max_root_add_chk_cnt; +#endif + + struct mesh_peer_sel_policy peer_sel_policy; + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC + u8 b2u_flags_msrc; + u8 b2u_flags_mfwd; +#endif +}; + +struct rtw_mesh_stats { + u32 fwded_mcast; /* Mesh forwarded multicast frames */ + u32 fwded_unicast; /* Mesh forwarded unicast frames */ + u32 fwded_frames; /* Mesh total forwarded frames */ + u32 dropped_frames_ttl; /* Not transmitted since mesh_ttl == 0*/ + u32 dropped_frames_no_route; /* Not transmitted, no route found */ + u32 dropped_frames_congestion;/* Not forwarded due to congestion */ + u32 dropped_frames_duplicate; + + u32 mrc_del_qlen; /* MRC entry deleted cause by queue length limit */ +}; + +struct rtw_mrc; + +struct rtw_mesh_info { + u8 mesh_id[NDIS_802_11_LENGTH_SSID]; + size_t mesh_id_len; + /* Active Path Selection Protocol Identifier */ + u8 mesh_pp_id; + /* Active Path Selection Metric Identifier */ + u8 mesh_pm_id; + /* Congestion Control Mode Identifier */ + u8 mesh_cc_id; + /* Synchronization Protocol Identifier */ + u8 mesh_sp_id; + /* Authentication Protocol Identifier */ + u8 mesh_auth_id; + + struct mesh_plink_pool plink_ctl; + + u32 mesh_seqnum; + /* MSTA's own hwmp sequence number */ + u32 sn; + systime last_preq; + systime last_sn_update; + systime next_perr; + /* Last used Path Discovery ID */ + u32 preq_id; + + ATOMIC_T mpaths; + struct rtw_mesh_table *mesh_paths; + struct rtw_mesh_table *mpp_paths; + int mesh_paths_generation; + int mpp_paths_generation; + + int num_gates; + struct rtw_mesh_path *max_addr_gate; + bool max_addr_gate_is_larger_than_self; + + struct rtw_mesh_stats mshstats; + + _queue mpath_tx_queue; + u32 mpath_tx_queue_len; + _tasklet mpath_tx_tasklet; + + struct rtw_mrc *mrc; + + _lock mesh_preq_queue_lock; + struct rtw_mesh_preq_queue preq_queue; + int preq_queue_len; +}; + +extern const char *_action_self_protected_str[]; +#define action_self_protected_str(action) ((action < RTW_ACT_SELF_PROTECTED_NUM) ? _action_self_protected_str[action] : _action_self_protected_str[0]) + +u8 *rtw_set_ie_mesh_id(u8 *buf, u32 *buf_len, const char *mesh_id, u8 id_len); +u8 *rtw_set_ie_mesh_config(u8 *buf, u32 *buf_len + , u8 path_sel_proto, u8 path_sel_metric, u8 congest_ctl_mode, u8 sync_method, u8 auth_proto + , u8 num_of_peerings, bool cto_mgate, bool cto_as + , bool accept_peerings, bool mcca_sup, bool mcca_en, bool forwarding + , bool mbca_en, bool tbtt_adj, bool ps_level); + +int rtw_bss_is_same_mbss(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b); +int rtw_bss_is_candidate_mesh_peer(_adapter *adapter, WLAN_BSSID_EX *target, u8 ch, u8 add_peer); + +void rtw_chk_candidate_peer_notify(_adapter *adapter, struct wlan_network *scanned); + +void rtw_mesh_peer_status_chk(_adapter *adapter); + +#if CONFIG_RTW_MESH_ACNODE_PREVENT +void rtw_mesh_update_scanned_acnode_status(_adapter *adapter, struct wlan_network *scanned); +bool rtw_mesh_scanned_is_acnode_confirmed(_adapter *adapter, struct wlan_network *scanned); +bool rtw_mesh_acnode_prevent_allow_sacrifice(_adapter *adapter); +struct sta_info *rtw_mesh_acnode_prevent_pick_sacrifice(_adapter *adapter); +void dump_mesh_acnode_prevent_settings(void *sel, _adapter *adapter); +#endif + +#if CONFIG_RTW_MESH_OFFCH_CAND +u8 rtw_mesh_offch_candidate_accepted(_adapter *adapter); +u8 rtw_mesh_select_operating_ch(_adapter *adapter); +void dump_mesh_offch_cand_settings(void *sel, _adapter *adapter); +#endif + +#if CONFIG_RTW_MESH_PEER_BLACKLIST +int rtw_mesh_peer_blacklist_add(_adapter *adapter, const u8 *addr); +int rtw_mesh_peer_blacklist_del(_adapter *adapter, const u8 *addr); +int rtw_mesh_peer_blacklist_search(_adapter *adapter, const u8 *addr); +void rtw_mesh_peer_blacklist_flush(_adapter *adapter); +void dump_mesh_peer_blacklist(void *sel, _adapter *adapter); +void dump_mesh_peer_blacklist_settings(void *sel, _adapter *adapter); +#endif +#if CONFIG_RTW_MESH_CTO_MGATE_BLACKLIST +u8 rtw_mesh_cto_mgate_required(_adapter *adapter); +u8 rtw_mesh_cto_mgate_network_filter(_adapter *adapter, struct wlan_network *scanned); +int rtw_mesh_cto_mgate_blacklist_add(_adapter *adapter, const u8 *addr); +int rtw_mesh_cto_mgate_blacklist_del(_adapter *adapter, const u8 *addr); +int rtw_mesh_cto_mgate_blacklist_search(_adapter *adapter, const u8 *addr); +void rtw_mesh_cto_mgate_blacklist_flush(_adapter *adapter); +void dump_mesh_cto_mgate_blacklist(void *sel, _adapter *adapter); +void dump_mesh_cto_mgate_blacklist_settings(void *sel, _adapter *adapter); +#endif +void dump_mesh_peer_sel_policy(void *sel, _adapter *adapter); +void dump_mesh_networks(void *sel, _adapter *adapter); + +void rtw_mesh_adjust_chbw(u8 req_ch, u8 *req_bw, u8 *req_offset); + +void rtw_mesh_sae_check_frames(_adapter *adapter, const u8 *buf, u32 len, u8 tx, u16 alg, u16 seq, u16 status); +int rtw_mesh_check_frames_tx(_adapter *adapter, const u8 **buf, size_t *len); +int rtw_mesh_check_frames_rx(_adapter *adapter, const u8 *buf, size_t len); + +int rtw_mesh_on_auth(_adapter *adapter, union recv_frame *rframe); +unsigned int on_action_self_protected(_adapter *adapter, union recv_frame *rframe); + +bool rtw_mesh_update_bss_peering_status(_adapter *adapter, WLAN_BSSID_EX *bss); +bool rtw_mesh_update_bss_formation_info(_adapter *adapter, WLAN_BSSID_EX *bss); +bool rtw_mesh_update_bss_forwarding_state(_adapter *adapter, WLAN_BSSID_EX *bss); + +struct mesh_plink_ent *_rtw_mesh_plink_get(_adapter *adapter, const u8 *hwaddr); +struct mesh_plink_ent *rtw_mesh_plink_get(_adapter *adapter, const u8 *hwaddr); +struct mesh_plink_ent *rtw_mesh_plink_get_no_estab_by_idx(_adapter *adapter, u8 idx); +int _rtw_mesh_plink_add(_adapter *adapter, const u8 *hwaddr); +int rtw_mesh_plink_add(_adapter *adapter, const u8 *hwaddr); +int rtw_mesh_plink_set_state(_adapter *adapter, const u8 *hwaddr, u8 state); +#ifdef CONFIG_RTW_MESH_AEK +int rtw_mesh_plink_set_aek(_adapter *adapter, const u8 *hwaddr, const u8 *aek); +#endif +#if CONFIG_RTW_MESH_PEER_BLACKLIST +int rtw_mesh_plink_set_peer_conf_timeout(_adapter *adapter, const u8 *hwaddr); +#endif +void _rtw_mesh_plink_del_ent(_adapter *adapter, struct mesh_plink_ent *ent); +int rtw_mesh_plink_del(_adapter *adapter, const u8 *hwaddr); +void rtw_mesh_plink_ctl_init(_adapter *adapter); +void rtw_mesh_plink_ctl_deinit(_adapter *adapter); +void dump_mesh_plink_ctl(void *sel, _adapter *adapter); + +u8 rtw_mesh_set_plink_state_cmd(_adapter *adapter, const u8 *mac, u8 plink_state); + +void _rtw_mesh_expire_peer_ent(_adapter *adapter, struct mesh_plink_ent *plink); +void rtw_mesh_expire_peer(_adapter *adapter, const u8 *peer_addr); +u8 rtw_mesh_ps_annc(_adapter *adapter, u8 ps); + +unsigned int on_action_mesh(_adapter *adapter, union recv_frame *rframe); + +void rtw_mesh_cfg_init(_adapter *adapter); +void rtw_mesh_cfg_init_max_peer_links(_adapter *adapter, u8 stack_conf); +void rtw_mesh_cfg_init_plink_timeout(_adapter *adapter, u32 stack_conf); +void rtw_mesh_init_mesh_info(_adapter *adapter); +void rtw_mesh_deinit_mesh_info(_adapter *adapter); + +#if CONFIG_RTW_MESH_DATA_BMC_TO_UC +void dump_mesh_b2u_flags(void *sel, _adapter *adapter); +#endif + +int rtw_mesh_addr_resolve(_adapter *adapter, u16 os_qid, struct xmit_frame *xframe, _pkt *pkt, _list *b2u_list); + +s8 rtw_mesh_tx_set_whdr_mctrl_len(u8 mesh_frame_mode, struct pkt_attrib *attrib); +void rtw_mesh_tx_build_mctrl(_adapter *adapter, struct pkt_attrib *attrib, u8 *buf); +u8 rtw_mesh_tx_build_whdr(_adapter *adapter, struct pkt_attrib *attrib + , u16 *fctrl, struct rtw_ieee80211_hdr *whdr); + +int rtw_mesh_rx_data_validate_hdr(_adapter *adapter, union recv_frame *rframe, struct sta_info **sta); +int rtw_mesh_rx_data_validate_mctrl(_adapter *adapter, union recv_frame *rframe + , const struct rtw_ieee80211s_hdr *mctrl, const u8 *mda, const u8 *msa + , u8 *mctrl_len, const u8 **da, const u8 **sa); +int rtw_mesh_rx_validate_mctrl_non_amsdu(_adapter *adapter, union recv_frame *rframe); + +int rtw_mesh_rx_msdu_act_check(union recv_frame *rframe + , const u8 *mda, const u8 *msa + , const u8 *da, const u8 *sa + , struct rtw_ieee80211s_hdr *mctrl + , u8 *msdu, enum rtw_rx_llc_hdl llc_hdl + , struct xmit_frame **fwd_frame, _list *b2u_list); + +void dump_mesh_stats(void *sel, _adapter *adapter); + +#if defined(PLATFORM_LINUX) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 32)) +#define rtw_lockdep_assert_held(l) lockdep_assert_held(l) +#define rtw_lockdep_is_held(l) lockdep_is_held(l) +#else +#error "TBD\n" +#endif + +#include "rtw_mesh_pathtbl.h" +#include "rtw_mesh_hwmp.h" +#endif /* __RTW_MESH_H_ */ + diff --git a/core/mesh/rtw_mesh_hwmp.c b/core/mesh/rtw_mesh_hwmp.c new file mode 100644 index 0000000..04be425 --- /dev/null +++ b/core/mesh/rtw_mesh_hwmp.c @@ -0,0 +1,1518 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_HWMP_C_ + +#ifdef CONFIG_RTW_MESH +#include +#include + +#define RTW_TEST_FRAME_LEN 8192 +#define RTW_MAX_METRIC 0xffffffff +#define RTW_ARITH_SHIFT 8 +#define RTW_LINK_FAIL_THRESH 95 +#define RTW_MAX_PREQ_QUEUE_LEN 64 +#define RTW_ATLM_REQ_CYCLE 1000 + +#define rtw_ilog2(n) \ +( \ + (n) < 2 ? 0 : \ + (n) & (1ULL << 63) ? 63 : \ + (n) & (1ULL << 62) ? 62 : \ + (n) & (1ULL << 61) ? 61 : \ + (n) & (1ULL << 60) ? 60 : \ + (n) & (1ULL << 59) ? 59 : \ + (n) & (1ULL << 58) ? 58 : \ + (n) & (1ULL << 57) ? 57 : \ + (n) & (1ULL << 56) ? 56 : \ + (n) & (1ULL << 55) ? 55 : \ + (n) & (1ULL << 54) ? 54 : \ + (n) & (1ULL << 53) ? 53 : \ + (n) & (1ULL << 52) ? 52 : \ + (n) & (1ULL << 51) ? 51 : \ + (n) & (1ULL << 50) ? 50 : \ + (n) & (1ULL << 49) ? 49 : \ + (n) & (1ULL << 48) ? 48 : \ + (n) & (1ULL << 47) ? 47 : \ + (n) & (1ULL << 46) ? 46 : \ + (n) & (1ULL << 45) ? 45 : \ + (n) & (1ULL << 44) ? 44 : \ + (n) & (1ULL << 43) ? 43 : \ + (n) & (1ULL << 42) ? 42 : \ + (n) & (1ULL << 41) ? 41 : \ + (n) & (1ULL << 40) ? 40 : \ + (n) & (1ULL << 39) ? 39 : \ + (n) & (1ULL << 38) ? 38 : \ + (n) & (1ULL << 37) ? 37 : \ + (n) & (1ULL << 36) ? 36 : \ + (n) & (1ULL << 35) ? 35 : \ + (n) & (1ULL << 34) ? 34 : \ + (n) & (1ULL << 33) ? 33 : \ + (n) & (1ULL << 32) ? 32 : \ + (n) & (1ULL << 31) ? 31 : \ + (n) & (1ULL << 30) ? 30 : \ + (n) & (1ULL << 29) ? 29 : \ + (n) & (1ULL << 28) ? 28 : \ + (n) & (1ULL << 27) ? 27 : \ + (n) & (1ULL << 26) ? 26 : \ + (n) & (1ULL << 25) ? 25 : \ + (n) & (1ULL << 24) ? 24 : \ + (n) & (1ULL << 23) ? 23 : \ + (n) & (1ULL << 22) ? 22 : \ + (n) & (1ULL << 21) ? 21 : \ + (n) & (1ULL << 20) ? 20 : \ + (n) & (1ULL << 19) ? 19 : \ + (n) & (1ULL << 18) ? 18 : \ + (n) & (1ULL << 17) ? 17 : \ + (n) & (1ULL << 16) ? 16 : \ + (n) & (1ULL << 15) ? 15 : \ + (n) & (1ULL << 14) ? 14 : \ + (n) & (1ULL << 13) ? 13 : \ + (n) & (1ULL << 12) ? 12 : \ + (n) & (1ULL << 11) ? 11 : \ + (n) & (1ULL << 10) ? 10 : \ + (n) & (1ULL << 9) ? 9 : \ + (n) & (1ULL << 8) ? 8 : \ + (n) & (1ULL << 7) ? 7 : \ + (n) & (1ULL << 6) ? 6 : \ + (n) & (1ULL << 5) ? 5 : \ + (n) & (1ULL << 4) ? 4 : \ + (n) & (1ULL << 3) ? 3 : \ + (n) & (1ULL << 2) ? 2 : \ + 1 \ +) + +enum rtw_mpath_frame_type { + RTW_MPATH_PREQ = 0, + RTW_MPATH_PREP, + RTW_MPATH_PERR, + RTW_MPATH_RANN +}; + +static inline u32 rtw_u32_field_get(const u8 *preq_elem, int shift, BOOLEAN ae) +{ + if (ae) + shift += 6; + return LE_BITS_TO_4BYTE(preq_elem + shift, 0, 32); +} + +static inline u16 rtw_u16_field_get(const u8 *preq_elem, int shift, BOOLEAN ae) +{ + if (ae) + shift += 6; + return LE_BITS_TO_2BYTE(preq_elem + shift, 0, 16); +} + +/* HWMP IE processing macros */ +#define RTW_AE_F (1<<6) +#define RTW_AE_F_SET(x) (*x & RTW_AE_F) +#define RTW_PREQ_IE_FLAGS(x) (*(x)) +#define RTW_PREQ_IE_HOPCOUNT(x) (*(x + 1)) +#define RTW_PREQ_IE_TTL(x) (*(x + 2)) +#define RTW_PREQ_IE_PREQ_ID(x) rtw_u32_field_get(x, 3, 0) +#define RTW_PREQ_IE_ORIG_ADDR(x) (x + 7) +#define RTW_PREQ_IE_ORIG_SN(x) rtw_u32_field_get(x, 13, 0) +#define RTW_PREQ_IE_LIFETIME(x) rtw_u32_field_get(x, 17, RTW_AE_F_SET(x)) +#define RTW_PREQ_IE_METRIC(x) rtw_u32_field_get(x, 21, RTW_AE_F_SET(x)) +#define RTW_PREQ_IE_TARGET_F(x) (*(RTW_AE_F_SET(x) ? x + 32 : x + 26)) +#define RTW_PREQ_IE_TARGET_ADDR(x) (RTW_AE_F_SET(x) ? x + 33 : x + 27) +#define RTW_PREQ_IE_TARGET_SN(x) rtw_u32_field_get(x, 33, RTW_AE_F_SET(x)) + +#define RTW_PREP_IE_FLAGS(x) RTW_PREQ_IE_FLAGS(x) +#define RTW_PREP_IE_HOPCOUNT(x) RTW_PREQ_IE_HOPCOUNT(x) +#define RTW_PREP_IE_TTL(x) RTW_PREQ_IE_TTL(x) +#define RTW_PREP_IE_ORIG_ADDR(x) (RTW_AE_F_SET(x) ? x + 27 : x + 21) +#define RTW_PREP_IE_ORIG_SN(x) rtw_u32_field_get(x, 27, RTW_AE_F_SET(x)) +#define RTW_PREP_IE_LIFETIME(x) rtw_u32_field_get(x, 13, RTW_AE_F_SET(x)) +#define RTW_PREP_IE_METRIC(x) rtw_u32_field_get(x, 17, RTW_AE_F_SET(x)) +#define RTW_PREP_IE_TARGET_ADDR(x) (x + 3) +#define RTW_PREP_IE_TARGET_SN(x) rtw_u32_field_get(x, 9, 0) + +#define RTW_PERR_IE_TTL(x) (*(x)) +#define RTW_PERR_IE_TARGET_FLAGS(x) (*(x + 2)) +#define RTW_PERR_IE_TARGET_ADDR(x) (x + 3) +#define RTW_PERR_IE_TARGET_SN(x) rtw_u32_field_get(x, 9, 0) +#define RTW_PERR_IE_TARGET_RCODE(x) rtw_u16_field_get(x, 13, 0) + +#define RTW_TU_TO_SYSTIME(x) (rtw_us_to_systime((x) * 1024)) +#define RTW_TU_TO_EXP_TIME(x) (rtw_get_current_time() + RTW_TU_TO_SYSTIME(x)) +#define RTW_MSEC_TO_TU(x) (x*1000/1024) +#define RTW_SN_GT(x, y) ((s32)(y - x) < 0) +#define RTW_SN_LT(x, y) ((s32)(x - y) < 0) +#define RTW_MAX_SANE_SN_DELTA 32 + +static inline u32 RTW_SN_DELTA(u32 x, u32 y) +{ + return x >= y ? x - y : y - x; +} + +#define rtw_net_traversal_jiffies(adapter) \ + rtw_ms_to_systime(adapter->mesh_cfg.dot11MeshHWMPnetDiameterTraversalTime) +#define rtw_default_lifetime(adapter) \ + RTW_MSEC_TO_TU(adapter->mesh_cfg.dot11MeshHWMPactivePathTimeout) +#define rtw_min_preq_int_jiff(adapter) \ + (rtw_ms_to_systime(adapter->mesh_cfg.dot11MeshHWMPpreqMinInterval)) +#define rtw_max_preq_retries(adapter) (adapter->mesh_cfg.dot11MeshHWMPmaxPREQretries) +#define rtw_disc_timeout_jiff(adapter) \ + rtw_ms_to_systime(adapter->mesh_cfg.min_discovery_timeout) +#define rtw_root_path_confirmation_jiffies(adapter) \ + rtw_ms_to_systime(adapter->mesh_cfg.dot11MeshHWMPconfirmationInterval) + +static inline BOOLEAN rtw_ether_addr_equal(const u8 *addr1, const u8 *addr2) +{ + return _rtw_memcmp(addr1, addr2, ETH_ALEN); +} + +#ifdef PLATFORM_LINUX +#define rtw_print_ratelimit() printk_ratelimit() +#define rtw_mod_timer(ptimer, expires) mod_timer(&(ptimer)->timer, expires) +#else + +#endif + +#define RTW_MESH_EWMA_PRECISION 20 +#define RTW_MESH_EWMA_WEIGHT_RCP 8 +#define RTW_TOTAL_PKT_MIN_THRESHOLD 1 +inline void rtw_ewma_err_rate_init(struct rtw_ewma_err_rate *e) +{ + e->internal = 0; +} +inline unsigned long rtw_ewma_err_rate_read(struct rtw_ewma_err_rate *e) +{ + return e->internal >> (RTW_MESH_EWMA_PRECISION); +} +inline void rtw_ewma_err_rate_add(struct rtw_ewma_err_rate *e, + unsigned long val) +{ + unsigned long internal = e->internal; + unsigned long weight_rcp = rtw_ilog2(RTW_MESH_EWMA_WEIGHT_RCP); + unsigned long precision = RTW_MESH_EWMA_PRECISION; + + (e->internal) = internal ? (((internal << weight_rcp) - internal) + + (val << precision)) >> weight_rcp : + (val << precision); +} + +static const u8 bcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + +static int rtw_mesh_path_sel_frame_tx(enum rtw_mpath_frame_type mpath_action, u8 flags, + const u8 *originator_addr, u32 originator_sn, + u8 target_flags, const u8 *target, + u32 target_sn, const u8 *da, u8 hopcount, u8 ttl, + u32 lifetime, u32 metric, u32 preq_id, + _adapter *adapter) +{ + struct xmit_priv *pxmitpriv = &(adapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct xmit_frame *pmgntframe = NULL; + struct rtw_ieee80211_hdr *pwlanhdr = NULL; + struct pkt_attrib *pattrib = NULL; + u8 category = RTW_WLAN_CATEGORY_MESH; + u8 action = RTW_ACT_MESH_HWMP_PATH_SELECTION; + u16 *fctrl = NULL; + u8 *pos, ie_len; + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return -1; + + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, pattrib); + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pos = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pos; + + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(adapter), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pos, WIFI_ACTION); + + pos += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pos = rtw_set_fixed_ie(pos, 1, &(category), &(pattrib->pktlen)); + pos = rtw_set_fixed_ie(pos, 1, &(action), &(pattrib->pktlen)); + + switch (mpath_action) { + case RTW_MPATH_PREQ: + RTW_HWMP_DBG("sending PREQ to "MAC_FMT"\n", MAC_ARG(target)); + ie_len = 37; + pattrib->pktlen += (ie_len + 2); + *pos++ = WLAN_EID_PREQ; + break; + case RTW_MPATH_PREP: + RTW_HWMP_DBG("sending PREP to "MAC_FMT"\n", MAC_ARG(originator_addr)); + ie_len = 31; + pattrib->pktlen += (ie_len + 2); + *pos++ = WLAN_EID_PREP; + break; + case RTW_MPATH_RANN: + RTW_HWMP_DBG("sending RANN from "MAC_FMT"\n", MAC_ARG(originator_addr)); + ie_len = sizeof(struct rtw_ieee80211_rann_ie); + pattrib->pktlen += (ie_len + 2); + *pos++ = WLAN_EID_RANN; + break; + default: + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + return _FAIL; + } + *pos++ = ie_len; + *pos++ = flags; + *pos++ = hopcount; + *pos++ = ttl; + if (mpath_action == RTW_MPATH_PREP) { + _rtw_memcpy(pos, target, ETH_ALEN); + pos += ETH_ALEN; + *(u32 *)pos = cpu_to_le32(target_sn); + pos += 4; + } else { + if (mpath_action == RTW_MPATH_PREQ) { + *(u32 *)pos = cpu_to_le32(preq_id); + pos += 4; + } + _rtw_memcpy(pos, originator_addr, ETH_ALEN); + pos += ETH_ALEN; + *(u32 *)pos = cpu_to_le32(originator_sn); + pos += 4; + } + *(u32 *)pos = cpu_to_le32(lifetime); + pos += 4; + *(u32 *)pos = cpu_to_le32(metric); + pos += 4; + if (mpath_action == RTW_MPATH_PREQ) { + *pos++ = 1; /* support only 1 destination now */ + *pos++ = target_flags; + _rtw_memcpy(pos, target, ETH_ALEN); + pos += ETH_ALEN; + *(u32 *)pos = cpu_to_le32(target_sn); + pos += 4; + } else if (mpath_action == RTW_MPATH_PREP) { + _rtw_memcpy(pos, originator_addr, ETH_ALEN); + pos += ETH_ALEN; + *(u32 *)pos = cpu_to_le32(originator_sn); + pos += 4; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(adapter, pmgntframe); + return 0; +} + +int rtw_mesh_path_error_tx(_adapter *adapter, + u8 ttl, const u8 *target, u32 target_sn, + u16 perr_reason_code, const u8 *ra) +{ + + struct xmit_priv *pxmitpriv = &(adapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct xmit_frame *pmgntframe = NULL; + struct rtw_ieee80211_hdr *pwlanhdr = NULL; + struct pkt_attrib *pattrib = NULL; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + u8 category = RTW_WLAN_CATEGORY_MESH; + u8 action = RTW_ACT_MESH_HWMP_PATH_SELECTION; + u8 *pos, ie_len; + u16 *fctrl = NULL; + + if (rtw_time_before(rtw_get_current_time(), minfo->next_perr)) + return -1; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return -1; + + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, pattrib); + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pos = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pos; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(adapter), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pos, WIFI_ACTION); + + pos += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pos = rtw_set_fixed_ie(pos, 1, &(category), &(pattrib->pktlen)); + pos = rtw_set_fixed_ie(pos, 1, &(action), &(pattrib->pktlen)); + + ie_len = 15; + pattrib->pktlen += (2 + ie_len); + *pos++ = WLAN_EID_PERR; + *pos++ = ie_len; + /* ttl */ + *pos++ = ttl; + /* The Number of Destinations N */ + *pos++ = 1; + /* Flags format | B7 | B6 | B5:B0 | = | rsvd | AE | rsvd | */ + *pos = 0; + pos++; + _rtw_memcpy(pos, target, ETH_ALEN); + pos += ETH_ALEN; + *(u32 *)pos = cpu_to_le32(target_sn); + pos += 4; + *(u16 *)pos = cpu_to_le16(perr_reason_code); + + adapter->mesh_info.next_perr = RTW_TU_TO_EXP_TIME( + adapter->mesh_cfg.dot11MeshHWMPperrMinInterval); + pattrib->last_txcmdsz = pattrib->pktlen; + /* Send directly. Rewrite it if deferred tx is needed */ + dump_mgntframe(adapter, pmgntframe); + + RTW_HWMP_DBG("TX PERR toward "MAC_FMT", ra = "MAC_FMT"\n", MAC_ARG(target), MAC_ARG(ra)); + + return 0; +} + +static u32 rtw_airtime_link_metric_get(_adapter *adapter, struct sta_info *sta) +{ + struct dm_struct *dm = adapter_to_phydm(adapter); + int device_constant = phydm_get_plcp(dm, sta->cmn.mac_id) << RTW_ARITH_SHIFT; + u32 test_frame_len = RTW_TEST_FRAME_LEN << RTW_ARITH_SHIFT; + u32 s_unit = 1 << RTW_ARITH_SHIFT; + u32 err; + u16 rate; + u32 tx_time, estimated_retx; + u64 result; + /* The fail_avg should <= 100 here */ + u32 fail_avg = (u32)rtw_ewma_err_rate_read(&sta->metrics.err_rate); + + if (fail_avg > RTW_LINK_FAIL_THRESH) + return RTW_MAX_METRIC; + + rate = sta->metrics.data_rate; + /* rate unit is 100Kbps, min rate = 10 */ + if (rate < 10) { + RTW_HWMP_INFO("rate = %d\n", rate); + return RTW_MAX_METRIC; + } + + err = (fail_avg << RTW_ARITH_SHIFT) / 100; + + /* test_frame_len*10 to adjust the unit of rate(100kbps/unit) */ + tx_time = (device_constant + 10 * test_frame_len / rate); + estimated_retx = ((1 << (2 * RTW_ARITH_SHIFT)) / (s_unit - err)); + result = (tx_time * estimated_retx) >> (2 * RTW_ARITH_SHIFT); + /* Convert us to 0.01 TU(10.24us). x/10.24 = x*100/1024 */ + result = (result * 100) >> 10; + + return (u32)result; +} + +void rtw_ieee80211s_update_metric(_adapter *adapter, u8 mac_id, + u8 per, u8 rate, + u8 bw, u8 total_pkt) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct sta_info *sta; + u8 rate_idx; + u8 sgi; + + sta = macid_ctl->sta[mac_id]; + if (!sta) + return; + + /* if RA, use reported rate */ + if (adapter->fix_rate == 0xff) { + rate_idx = rate & 0x7f; + sgi = rate >> 7; + } else { + rate_idx = adapter->fix_rate & 0x7f; + sgi = adapter->fix_rate >> 7; + } + sta->metrics.data_rate = rtw_desc_rate_to_bitrate(bw, rate_idx, sgi); + + if (total_pkt < RTW_TOTAL_PKT_MIN_THRESHOLD) + return; + + /* TBD: sta->metrics.overhead = phydm_get_plcp(void *dm_void, u16 macid); */ + sta->metrics.total_pkt = total_pkt; + + rtw_ewma_err_rate_add(&sta->metrics.err_rate, per); + if (rtw_ewma_err_rate_read(&sta->metrics.err_rate) > + RTW_LINK_FAIL_THRESH) + rtw_mesh_plink_broken(sta); +} + +static void rtw_hwmp_preq_frame_process(_adapter *adapter, + struct rtw_ieee80211_hdr_3addr *mgmt, + const u8 *preq_elem, u32 originator_metric) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; + struct rtw_mesh_path *path = NULL; + const u8 *target_addr, *originator_addr; + const u8 *da; + u8 target_flags, ttl, flags, to_gate_ask = 0; + u32 originator_sn, target_sn, lifetime, target_metric = 0; + BOOLEAN reply = _FALSE; + BOOLEAN forward = _TRUE; + BOOLEAN preq_is_gate; + + /* Update target SN, if present */ + target_addr = RTW_PREQ_IE_TARGET_ADDR(preq_elem); + originator_addr = RTW_PREQ_IE_ORIG_ADDR(preq_elem); + target_sn = RTW_PREQ_IE_TARGET_SN(preq_elem); + originator_sn = RTW_PREQ_IE_ORIG_SN(preq_elem); + target_flags = RTW_PREQ_IE_TARGET_F(preq_elem); + /* PREQ gate announcements */ + flags = RTW_PREQ_IE_FLAGS(preq_elem); + preq_is_gate = !!(flags & RTW_IEEE80211_PREQ_IS_GATE_FLAG); + + RTW_HWMP_DBG("received PREQ from "MAC_FMT"\n", MAC_ARG(originator_addr)); + + if (rtw_ether_addr_equal(target_addr, adapter_mac_addr(adapter))) { + RTW_HWMP_DBG("PREQ is for us\n"); +#ifdef CONFIG_RTW_MESH_ON_DMD_GANN + rtw_rcu_read_lock(); + path = rtw_mesh_path_lookup(adapter, originator_addr); + if (path) { + if (preq_is_gate) + rtw_mesh_path_add_gate(path); + else if (path->is_gate) { + enter_critical_bh(&path->state_lock); + rtw_mesh_gate_del(adapter->mesh_info.mesh_paths, path); + exit_critical_bh(&path->state_lock); + } + } + path = NULL; + rtw_rcu_read_unlock(); +#endif + forward = _FALSE; + reply = _TRUE; + to_gate_ask = 1; + target_metric = 0; + if (rtw_time_after(rtw_get_current_time(), minfo->last_sn_update + + rtw_net_traversal_jiffies(adapter)) || + rtw_time_before(rtw_get_current_time(), minfo->last_sn_update)) { + ++minfo->sn; + minfo->last_sn_update = rtw_get_current_time(); + } + target_sn = minfo->sn; + } else if (is_broadcast_mac_addr(target_addr) && + (target_flags & RTW_IEEE80211_PREQ_TO_FLAG)) { + rtw_rcu_read_lock(); + path = rtw_mesh_path_lookup(adapter, originator_addr); + if (path) { + if (flags & RTW_IEEE80211_PREQ_PROACTIVE_PREP_FLAG) { + reply = _TRUE; + target_addr = adapter_mac_addr(adapter); + target_sn = ++minfo->sn; + target_metric = 0; + minfo->last_sn_update = rtw_get_current_time(); + } + + if (preq_is_gate) { + lifetime = RTW_PREQ_IE_LIFETIME(preq_elem); + path->gate_ann_int = lifetime; + path->gate_asked = false; + rtw_mesh_path_add_gate(path); + } else if (path->is_gate) { + enter_critical_bh(&path->state_lock); + rtw_mesh_gate_del(adapter->mesh_info.mesh_paths, path); + exit_critical_bh(&path->state_lock); + } + } + rtw_rcu_read_unlock(); + } else { + rtw_rcu_read_lock(); +#ifdef CONFIG_RTW_MESH_ON_DMD_GANN + path = rtw_mesh_path_lookup(adapter, originator_addr); + if (path) { + if (preq_is_gate) + rtw_mesh_path_add_gate(path); + else if (path->is_gate) { + enter_critical_bh(&path->state_lock); + rtw_mesh_gate_del(adapter->mesh_info.mesh_paths, path); + exit_critical_bh(&path->state_lock); + } + } + path = NULL; +#endif + path = rtw_mesh_path_lookup(adapter, target_addr); + if (path) { + if ((!(path->flags & RTW_MESH_PATH_SN_VALID)) || + RTW_SN_LT(path->sn, target_sn)) { + path->sn = target_sn; + path->flags |= RTW_MESH_PATH_SN_VALID; + } else if ((!(target_flags & RTW_IEEE80211_PREQ_TO_FLAG)) && + (path->flags & RTW_MESH_PATH_ACTIVE)) { + reply = _TRUE; + target_metric = path->metric; + target_sn = path->sn; + /* Case E2 of sec 13.10.9.3 IEEE 802.11-2012*/ + target_flags |= RTW_IEEE80211_PREQ_TO_FLAG; + } + } + rtw_rcu_read_unlock(); + } + + if (reply) { + lifetime = RTW_PREQ_IE_LIFETIME(preq_elem); + ttl = mshcfg->element_ttl; + if (ttl != 0 && !to_gate_ask) { + RTW_HWMP_DBG("replying to the PREQ\n"); + rtw_mesh_path_sel_frame_tx(RTW_MPATH_PREP, 0, originator_addr, + originator_sn, 0, target_addr, + target_sn, mgmt->addr2, 0, ttl, + lifetime, target_metric, 0, + adapter); + } else if (ttl != 0 && to_gate_ask) { + RTW_HWMP_DBG("replying to the PREQ (PREQ for us)\n"); + if (mshcfg->dot11MeshGateAnnouncementProtocol) { + /* BIT 7 is used to identify the prep is from mesh gate */ + to_gate_ask = RTW_IEEE80211_PREQ_IS_GATE_FLAG | BIT(7); + } else { + to_gate_ask = 0; + } + + rtw_mesh_path_sel_frame_tx(RTW_MPATH_PREP, to_gate_ask, originator_addr, + originator_sn, 0, target_addr, + target_sn, mgmt->addr2, 0, ttl, + lifetime, target_metric, 0, + adapter); + } else { + minfo->mshstats.dropped_frames_ttl++; + } + } + + if (forward && mshcfg->dot11MeshForwarding) { + u32 preq_id; + u8 hopcount; + + ttl = RTW_PREQ_IE_TTL(preq_elem); + lifetime = RTW_PREQ_IE_LIFETIME(preq_elem); + if (ttl <= 1) { + minfo->mshstats.dropped_frames_ttl++; + return; + } + RTW_HWMP_DBG("forwarding the PREQ from "MAC_FMT"\n", MAC_ARG(originator_addr)); + --ttl; + preq_id = RTW_PREQ_IE_PREQ_ID(preq_elem); + hopcount = RTW_PREQ_IE_HOPCOUNT(preq_elem) + 1; + da = (path && path->is_root) ? + path->rann_snd_addr : bcast_addr; + + if (flags & RTW_IEEE80211_PREQ_PROACTIVE_PREP_FLAG) { + target_addr = RTW_PREQ_IE_TARGET_ADDR(preq_elem); + target_sn = RTW_PREQ_IE_TARGET_SN(preq_elem); + } + + rtw_mesh_path_sel_frame_tx(RTW_MPATH_PREQ, flags, originator_addr, + originator_sn, target_flags, target_addr, + target_sn, da, hopcount, ttl, lifetime, + originator_metric, preq_id, adapter); + if (!is_multicast_mac_addr(da)) + minfo->mshstats.fwded_unicast++; + else + minfo->mshstats.fwded_mcast++; + minfo->mshstats.fwded_frames++; + } +} + +static inline struct sta_info * +rtw_next_hop_deref_protected(struct rtw_mesh_path *path) +{ + return rtw_rcu_dereference_protected(path->next_hop, + rtw_lockdep_is_held(&path->state_lock)); +} + +static void rtw_hwmp_prep_frame_process(_adapter *adapter, + struct rtw_ieee80211_hdr_3addr *mgmt, + const u8 *prep_elem, u32 metric) +{ + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; + struct rtw_mesh_stats *mshstats = &adapter->mesh_info.mshstats; + struct rtw_mesh_path *path; + const u8 *target_addr, *originator_addr; + u8 ttl, hopcount, flags; + u8 next_hop[ETH_ALEN]; + u32 target_sn, originator_sn, lifetime; + + RTW_HWMP_DBG("received PREP from "MAC_FMT"\n", + MAC_ARG(RTW_PREP_IE_TARGET_ADDR(prep_elem))); + + originator_addr = RTW_PREP_IE_ORIG_ADDR(prep_elem); + if (rtw_ether_addr_equal(originator_addr, adapter_mac_addr(adapter))) { + /* destination, no forwarding required */ + rtw_rcu_read_lock(); + target_addr = RTW_PREP_IE_TARGET_ADDR(prep_elem); + path = rtw_mesh_path_lookup(adapter, target_addr); + if (path && path->gate_asked) { + flags = RTW_PREP_IE_FLAGS(prep_elem); + if (flags & BIT(7)) { + enter_critical_bh(&path->state_lock); + path->gate_asked = false; + exit_critical_bh(&path->state_lock); + if (!(flags & RTW_IEEE80211_PREQ_IS_GATE_FLAG)) { + enter_critical_bh(&path->state_lock); + rtw_mesh_gate_del(adapter->mesh_info.mesh_paths, path); + exit_critical_bh(&path->state_lock); + } + } + } + + rtw_rcu_read_unlock(); + return; + } + + if (!mshcfg->dot11MeshForwarding) + return; + + ttl = RTW_PREP_IE_TTL(prep_elem); + if (ttl <= 1) { + mshstats->dropped_frames_ttl++; + return; + } + + rtw_rcu_read_lock(); + path = rtw_mesh_path_lookup(adapter, originator_addr); + if (path) + enter_critical_bh(&path->state_lock); + else + goto fail; + if (!(path->flags & RTW_MESH_PATH_ACTIVE)) { + exit_critical_bh(&path->state_lock); + goto fail; + } + _rtw_memcpy(next_hop, rtw_next_hop_deref_protected(path)->cmn.mac_addr, ETH_ALEN); + exit_critical_bh(&path->state_lock); + --ttl; + flags = RTW_PREP_IE_FLAGS(prep_elem); + lifetime = RTW_PREP_IE_LIFETIME(prep_elem); + hopcount = RTW_PREP_IE_HOPCOUNT(prep_elem) + 1; + target_addr = RTW_PREP_IE_TARGET_ADDR(prep_elem); + target_sn = RTW_PREP_IE_TARGET_SN(prep_elem); + originator_sn = RTW_PREP_IE_ORIG_SN(prep_elem); + + rtw_mesh_path_sel_frame_tx(RTW_MPATH_PREP, flags, originator_addr, originator_sn, 0, + target_addr, target_sn, next_hop, hopcount, + ttl, lifetime, metric, 0, adapter); + rtw_rcu_read_unlock(); + + mshstats->fwded_unicast++; + mshstats->fwded_frames++; + return; + +fail: + rtw_rcu_read_unlock(); + mshstats->dropped_frames_no_route++; +} + +static void rtw_hwmp_perr_frame_process(_adapter *adapter, + struct rtw_ieee80211_hdr_3addr *mgmt, + const u8 *perr_elem) +{ + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; + struct rtw_mesh_stats *mshstats = &adapter->mesh_info.mshstats; + struct rtw_mesh_path *path; + u8 ttl; + const u8 *ta, *target_addr; + u32 target_sn; + u16 perr_reason_code; + + ta = mgmt->addr2; + ttl = RTW_PERR_IE_TTL(perr_elem); + if (ttl <= 1) { + mshstats->dropped_frames_ttl++; + return; + } + ttl--; + target_addr = RTW_PERR_IE_TARGET_ADDR(perr_elem); + target_sn = RTW_PERR_IE_TARGET_SN(perr_elem); + perr_reason_code = RTW_PERR_IE_TARGET_RCODE(perr_elem); + + RTW_HWMP_DBG("received PERR toward target "MAC_FMT"\n", MAC_ARG(target_addr)); + + rtw_rcu_read_lock(); + path = rtw_mesh_path_lookup(adapter, target_addr); + if (path) { + struct sta_info *sta; + + enter_critical_bh(&path->state_lock); + sta = rtw_next_hop_deref_protected(path); + if (path->flags & RTW_MESH_PATH_ACTIVE && + rtw_ether_addr_equal(ta, sta->cmn.mac_addr) && + !(path->flags & RTW_MESH_PATH_FIXED) && + (!(path->flags & RTW_MESH_PATH_SN_VALID) || + RTW_SN_GT(target_sn, path->sn) || target_sn == 0)) { + path->flags &= ~RTW_MESH_PATH_ACTIVE; + if (target_sn != 0) + path->sn = target_sn; + else + path->sn += 1; + exit_critical_bh(&path->state_lock); + if (!mshcfg->dot11MeshForwarding) + goto endperr; + rtw_mesh_path_error_tx(adapter, ttl, target_addr, + target_sn, perr_reason_code, + bcast_addr); + } else + exit_critical_bh(&path->state_lock); + } +endperr: + rtw_rcu_read_unlock(); +} + +static void rtw_hwmp_rann_frame_process(_adapter *adapter, + struct rtw_ieee80211_hdr_3addr *mgmt, + const struct rtw_ieee80211_rann_ie *rann) +{ + struct sta_info *sta; + struct sta_priv *pstapriv = &adapter->stapriv; + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; + struct rtw_mesh_stats *mshstats = &adapter->mesh_info.mshstats; + struct rtw_mesh_path *path; + u8 ttl, flags, hopcount; + const u8 *originator_addr; + u32 originator_sn, metric, metric_txsta, interval; + BOOLEAN root_is_gate; + + ttl = rann->rann_ttl; + flags = rann->rann_flags; + root_is_gate = !!(flags & RTW_RANN_FLAG_IS_GATE); + originator_addr = rann->rann_addr; + originator_sn = le32_to_cpu(rann->rann_seq); + interval = le32_to_cpu(rann->rann_interval); + hopcount = rann->rann_hopcount; + hopcount++; + metric = le32_to_cpu(rann->rann_metric); + + /* Ignore our own RANNs */ + if (rtw_ether_addr_equal(originator_addr, adapter_mac_addr(adapter))) + return; + + RTW_HWMP_DBG("received RANN from "MAC_FMT" via neighbour "MAC_FMT" (is_gate=%d)\n", + MAC_ARG(originator_addr), MAC_ARG(mgmt->addr2), root_is_gate); + + rtw_rcu_read_lock(); + sta = rtw_get_stainfo(pstapriv, mgmt->addr2); + if (!sta) { + rtw_rcu_read_unlock(); + return; + } + + metric_txsta = rtw_airtime_link_metric_get(adapter, sta); + + path = rtw_mesh_path_lookup(adapter, originator_addr); + if (!path) { + path = rtw_mesh_path_add(adapter, originator_addr); + if (IS_ERR(path)) { + rtw_rcu_read_unlock(); + mshstats->dropped_frames_no_route++; + return; + } + } + + if (!(RTW_SN_LT(path->sn, originator_sn)) && + !(path->sn == originator_sn && metric < path->rann_metric)) { + rtw_rcu_read_unlock(); + return; + } + + if ((!(path->flags & (RTW_MESH_PATH_ACTIVE | RTW_MESH_PATH_RESOLVING)) || + (rtw_time_after(rtw_get_current_time(), path->last_preq_to_root + + rtw_root_path_confirmation_jiffies(adapter)) || + rtw_time_before(rtw_get_current_time(), path->last_preq_to_root))) && + !(path->flags & RTW_MESH_PATH_FIXED) && (ttl != 0)) { + u8 preq_node_flag = RTW_PREQ_Q_F_START | RTW_PREQ_Q_F_REFRESH; + + RTW_HWMP_DBG("time to refresh root path "MAC_FMT"\n", + MAC_ARG(originator_addr)); +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + if (RTW_SN_LT(path->sn, originator_sn) && + (path->rann_metric + mshcfg->sane_metric_delta < metric) && + _rtw_memcmp(bcast_addr, path->rann_snd_addr, ETH_ALEN) == _FALSE) { + RTW_HWMP_DBG("Trigger additional check for root " + "confirm PREQ. rann_snd_addr = "MAC_FMT + "add_chk_rann_snd_addr= "MAC_FMT"\n", + MAC_ARG(mgmt->addr2), + MAC_ARG(path->rann_snd_addr)); + _rtw_memcpy(path->add_chk_rann_snd_addr, + path->rann_snd_addr, ETH_ALEN); + preq_node_flag |= RTW_PREQ_Q_F_CHK; + + } +#endif + rtw_mesh_queue_preq(path, preq_node_flag); + path->last_preq_to_root = rtw_get_current_time(); + } + + path->sn = originator_sn; + path->rann_metric = metric + metric_txsta; + path->is_root = _TRUE; + /* Recording RANNs sender address to send individually + * addressed PREQs destined for root mesh STA */ + _rtw_memcpy(path->rann_snd_addr, mgmt->addr2, ETH_ALEN); + + if (root_is_gate) { + path->gate_ann_int = interval; + path->gate_asked = false; + rtw_mesh_path_add_gate(path); + } else if (path->is_gate) { + enter_critical_bh(&path->state_lock); + rtw_mesh_gate_del(adapter->mesh_info.mesh_paths, path); + exit_critical_bh(&path->state_lock); + } + + if (ttl <= 1) { + mshstats->dropped_frames_ttl++; + rtw_rcu_read_unlock(); + return; + } + ttl--; + + if (mshcfg->dot11MeshForwarding) { + rtw_mesh_path_sel_frame_tx(RTW_MPATH_RANN, flags, originator_addr, + originator_sn, 0, NULL, 0, bcast_addr, + hopcount, ttl, interval, + metric + metric_txsta, 0, adapter); + } + + rtw_rcu_read_unlock(); +} + +static u32 rtw_hwmp_route_info_get(_adapter *adapter, + struct rtw_ieee80211_hdr_3addr *mgmt, + const u8 *hwmp_ie, enum rtw_mpath_frame_type action) +{ + struct rtw_mesh_path *path; + struct sta_priv *pstapriv = &adapter->stapriv; + struct sta_info *sta; + BOOLEAN fresh_info; + const u8 *originator_addr, *ta; + u32 originator_sn, originator_metric; + unsigned long originator_lifetime, exp_time; + u32 last_hop_metric, new_metric; + BOOLEAN process = _TRUE; + + rtw_rcu_read_lock(); + sta = rtw_get_stainfo(pstapriv, mgmt->addr2); + if (!sta) { + rtw_rcu_read_unlock(); + return 0; + } + + last_hop_metric = rtw_airtime_link_metric_get(adapter, sta); + /* Update and check originator routing info */ + fresh_info = _TRUE; + + switch (action) { + case RTW_MPATH_PREQ: + originator_addr = RTW_PREQ_IE_ORIG_ADDR(hwmp_ie); + originator_sn = RTW_PREQ_IE_ORIG_SN(hwmp_ie); + originator_lifetime = RTW_PREQ_IE_LIFETIME(hwmp_ie); + originator_metric = RTW_PREQ_IE_METRIC(hwmp_ie); + break; + case RTW_MPATH_PREP: + /* Note: For coding, the naming is not consist with spec */ + originator_addr = RTW_PREP_IE_TARGET_ADDR(hwmp_ie); + originator_sn = RTW_PREP_IE_TARGET_SN(hwmp_ie); + originator_lifetime = RTW_PREP_IE_LIFETIME(hwmp_ie); + originator_metric = RTW_PREP_IE_METRIC(hwmp_ie); + break; + default: + rtw_rcu_read_unlock(); + return 0; + } + new_metric = originator_metric + last_hop_metric; + if (new_metric < originator_metric) + new_metric = RTW_MAX_METRIC; + exp_time = RTW_TU_TO_EXP_TIME(originator_lifetime); + + if (rtw_ether_addr_equal(originator_addr, adapter_mac_addr(adapter))) { + process = _FALSE; + fresh_info = _FALSE; + } else { + path = rtw_mesh_path_lookup(adapter, originator_addr); + if (path) { + enter_critical_bh(&path->state_lock); + if (path->flags & RTW_MESH_PATH_FIXED) + fresh_info = _FALSE; + else if ((path->flags & RTW_MESH_PATH_ACTIVE) && + (path->flags & RTW_MESH_PATH_SN_VALID)) { + if (RTW_SN_GT(path->sn, originator_sn) || + (path->sn == originator_sn && + new_metric >= path->metric)) { + process = _FALSE; + fresh_info = _FALSE; + } + } else if (!(path->flags & RTW_MESH_PATH_ACTIVE)) { + BOOLEAN have_sn, newer_sn, bounced; + + have_sn = path->flags & RTW_MESH_PATH_SN_VALID; + newer_sn = have_sn && RTW_SN_GT(originator_sn, path->sn); + bounced = have_sn && + (RTW_SN_DELTA(originator_sn, path->sn) > + RTW_MAX_SANE_SN_DELTA); + + if (!have_sn || newer_sn) { + } else if (bounced) { + } else { + process = _FALSE; + fresh_info = _FALSE; + } + } + } else { + path = rtw_mesh_path_add(adapter, originator_addr); + if (IS_ERR(path)) { + rtw_rcu_read_unlock(); + return 0; + } + enter_critical_bh(&path->state_lock); + } + + if (fresh_info) { + rtw_mesh_path_assign_nexthop(path, sta); + path->flags |= RTW_MESH_PATH_SN_VALID; + path->metric = new_metric; + path->sn = originator_sn; + path->exp_time = rtw_time_after(path->exp_time, exp_time) + ? path->exp_time : exp_time; + rtw_mesh_path_activate(path); +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + if (path->is_root && (action == RTW_MPATH_PREP)) { + _rtw_memcpy(path->rann_snd_addr, + mgmt->addr2, ETH_ALEN); + path->rann_metric = new_metric; + } +#endif + exit_critical_bh(&path->state_lock); + rtw_mesh_path_tx_pending(path); + } else + exit_critical_bh(&path->state_lock); + } + + /* Update and check transmitter routing info */ + ta = mgmt->addr2; + if (rtw_ether_addr_equal(originator_addr, ta)) + fresh_info = _FALSE; + else { + fresh_info = _TRUE; + + path = rtw_mesh_path_lookup(adapter, ta); + if (path) { + enter_critical_bh(&path->state_lock); + if ((path->flags & RTW_MESH_PATH_FIXED) || + ((path->flags & RTW_MESH_PATH_ACTIVE) && + (last_hop_metric > path->metric))) + fresh_info = _FALSE; + } else { + path = rtw_mesh_path_add(adapter, ta); + if (IS_ERR(path)) { + rtw_rcu_read_unlock(); + return 0; + } + enter_critical_bh(&path->state_lock); + } + + if (fresh_info) { + rtw_mesh_path_assign_nexthop(path, sta); + path->metric = last_hop_metric; + path->exp_time = rtw_time_after(path->exp_time, exp_time) + ? path->exp_time : exp_time; + rtw_mesh_path_activate(path); + exit_critical_bh(&path->state_lock); + rtw_mesh_path_tx_pending(path); + } else + exit_critical_bh(&path->state_lock); + } + + rtw_rcu_read_unlock(); + + return process ? new_metric : 0; +} + +static void rtw_mesh_rx_hwmp_frame_cnts(_adapter *adapter, u8 *addr) +{ + struct sta_info *sta; + + sta = rtw_get_stainfo(&adapter->stapriv, addr); + if (sta) + sta->sta_stats.rx_hwmp_pkts++; +} + +void rtw_mesh_rx_path_sel_frame(_adapter *adapter, union recv_frame *rframe) +{ + struct mesh_plink_ent *plink = NULL; + struct rtw_ieee802_11_elems elems; + u32 path_metric; + struct rx_pkt_attrib *attrib = &rframe->u.hdr.attrib; + u8 *pframe = rframe->u.hdr.rx_data, *start; + uint frame_len = rframe->u.hdr.len, left; + struct rtw_ieee80211_hdr_3addr *frame_hdr = (struct rtw_ieee80211_hdr_3addr *)pframe; + u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + ParseRes parse_res; + + plink = rtw_mesh_plink_get(adapter, get_addr2_ptr(pframe)); + if (!plink || plink->plink_state != RTW_MESH_PLINK_ESTAB) + return; + + rtw_mesh_rx_hwmp_frame_cnts(adapter, get_addr2_ptr(pframe)); + + /* Mesh action frame IE offset = 2 */ + attrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr); + left = frame_len - attrib->hdrlen - attrib->iv_len - attrib->icv_len - 2; + start = pframe + attrib->hdrlen + 2; + + parse_res = rtw_ieee802_11_parse_elems(start, left, &elems, 1); + if (parse_res == ParseFailed) + RTW_HWMP_INFO(FUNC_ADPT_FMT" Path Select Frame ParseFailed\n" + , FUNC_ADPT_ARG(adapter)); + else if (parse_res == ParseUnknown) + RTW_HWMP_INFO(FUNC_ADPT_FMT" Path Select Frame ParseUnknown\n" + , FUNC_ADPT_ARG(adapter)); + + if (elems.preq) { + if (elems.preq_len != 37) + /* Right now we support just 1 destination and no AE */ + return; + path_metric = rtw_hwmp_route_info_get(adapter, frame_hdr, elems.preq, + MPATH_PREQ); + if (path_metric) + rtw_hwmp_preq_frame_process(adapter, frame_hdr, elems.preq, + path_metric); + } + if (elems.prep) { + if (elems.prep_len != 31) + /* Right now we support no AE */ + return; + path_metric = rtw_hwmp_route_info_get(adapter, frame_hdr, elems.prep, + MPATH_PREP); + if (path_metric) + rtw_hwmp_prep_frame_process(adapter, frame_hdr, elems.prep, + path_metric); + } + if (elems.perr) { + if (elems.perr_len != 15) + /* Right now we support only one destination per PERR */ + return; + rtw_hwmp_perr_frame_process(adapter, frame_hdr, elems.perr); + } + if (elems.rann) + rtw_hwmp_rann_frame_process(adapter, frame_hdr, (struct rtw_ieee80211_rann_ie *)elems.rann); +} + +void rtw_mesh_queue_preq(struct rtw_mesh_path *path, u8 flags) +{ + _adapter *adapter = path->adapter; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mesh_preq_queue *preq_node; + + preq_node = rtw_malloc(sizeof(struct rtw_mesh_preq_queue)); + if (!preq_node) { + RTW_HWMP_INFO("could not allocate PREQ node\n"); + return; + } + + enter_critical_bh(&minfo->mesh_preq_queue_lock); + if (minfo->preq_queue_len == RTW_MAX_PREQ_QUEUE_LEN) { + exit_critical_bh(&minfo->mesh_preq_queue_lock); + rtw_mfree(preq_node, sizeof(struct rtw_mesh_preq_queue)); + if (rtw_print_ratelimit()) + RTW_HWMP_INFO("PREQ node queue full\n"); + return; + } + + _rtw_spinlock(&path->state_lock); + if (path->flags & RTW_MESH_PATH_REQ_QUEUED) { + _rtw_spinunlock(&path->state_lock); + exit_critical_bh(&minfo->mesh_preq_queue_lock); + rtw_mfree(preq_node, sizeof(struct rtw_mesh_preq_queue)); + return; + } + + _rtw_memcpy(preq_node->dst, path->dst, ETH_ALEN); + preq_node->flags = flags; + + path->flags |= RTW_MESH_PATH_REQ_QUEUED; +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + if (flags & RTW_PREQ_Q_F_CHK) + path->flags |= RTW_MESH_PATH_ROOT_ADD_CHK; +#endif + if (flags & RTW_PREQ_Q_F_PEER_AKA) + path->flags |= RTW_MESH_PATH_PEER_AKA; + if (flags & RTW_PREQ_Q_F_BCAST_PREQ) + path->flags |= RTW_MESH_PATH_BCAST_PREQ; + _rtw_spinunlock(&path->state_lock); + + rtw_list_insert_tail(&preq_node->list, &minfo->preq_queue.list); + ++minfo->preq_queue_len; + exit_critical_bh(&minfo->mesh_preq_queue_lock); + + if (rtw_time_after(rtw_get_current_time(), minfo->last_preq + rtw_min_preq_int_jiff(adapter))) + rtw_mesh_work(&adapter->mesh_work); + + else if (rtw_time_before(rtw_get_current_time(), minfo->last_preq)) { + /* systime wrapped around issue */ + minfo->last_preq = rtw_get_current_time() - rtw_min_preq_int_jiff(adapter) - 1; + rtw_mesh_work(&adapter->mesh_work); + } else + rtw_mod_timer(&adapter->mesh_path_timer, minfo->last_preq + + rtw_min_preq_int_jiff(adapter) + 1); +} + +static const u8 *rtw_hwmp_preq_da(struct rtw_mesh_path *path, + BOOLEAN is_root_add_chk, BOOLEAN da_is_peer, + BOOLEAN force_preq_bcast) +{ + const u8 *da; + + if (da_is_peer) + da = path->dst; + else if (force_preq_bcast) + da = bcast_addr; + else if (path->is_root) +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + da = is_root_add_chk ? path->add_chk_rann_snd_addr: + path->rann_snd_addr; +#else + da = path->rann_snd_addr; +#endif + else + da = bcast_addr; + + return da; +} + +void rtw_mesh_path_start_discovery(_adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; + struct rtw_mesh_preq_queue *preq_node; + struct rtw_mesh_path *path; + u8 ttl, target_flags = 0; + const u8 *da; + u32 lifetime; + u8 flags = 0; + BOOLEAN is_root_add_chk = _FALSE; + BOOLEAN da_is_peer, force_preq_bcast; + + enter_critical_bh(&minfo->mesh_preq_queue_lock); + if (!minfo->preq_queue_len || + rtw_time_before(rtw_get_current_time(), minfo->last_preq + + rtw_min_preq_int_jiff(adapter))) { + exit_critical_bh(&minfo->mesh_preq_queue_lock); + return; + } + + preq_node = rtw_list_first_entry(&minfo->preq_queue.list, + struct rtw_mesh_preq_queue, list); + rtw_list_delete(&preq_node->list); /* list_del_init(&preq_node->list); */ + --minfo->preq_queue_len; + exit_critical_bh(&minfo->mesh_preq_queue_lock); + + rtw_rcu_read_lock(); + path = rtw_mesh_path_lookup(adapter, preq_node->dst); + if (!path) + goto enddiscovery; + + enter_critical_bh(&path->state_lock); + if (path->flags & (RTW_MESH_PATH_DELETED | RTW_MESH_PATH_FIXED)) { + exit_critical_bh(&path->state_lock); + goto enddiscovery; + } + path->flags &= ~RTW_MESH_PATH_REQ_QUEUED; + if (preq_node->flags & RTW_PREQ_Q_F_START) { + if (path->flags & RTW_MESH_PATH_RESOLVING) { + exit_critical_bh(&path->state_lock); + goto enddiscovery; + } else { + path->flags &= ~RTW_MESH_PATH_RESOLVED; + path->flags |= RTW_MESH_PATH_RESOLVING; + path->discovery_retries = 0; + path->discovery_timeout = rtw_disc_timeout_jiff(adapter); + } + } else if (!(path->flags & RTW_MESH_PATH_RESOLVING) || + path->flags & RTW_MESH_PATH_RESOLVED) { + path->flags &= ~RTW_MESH_PATH_RESOLVING; + exit_critical_bh(&path->state_lock); + goto enddiscovery; + } + + minfo->last_preq = rtw_get_current_time(); + + if (rtw_time_after(rtw_get_current_time(), minfo->last_sn_update + + rtw_net_traversal_jiffies(adapter)) || + rtw_time_before(rtw_get_current_time(), minfo->last_sn_update)) { + ++minfo->sn; + minfo->last_sn_update = rtw_get_current_time(); + } + lifetime = rtw_default_lifetime(adapter); + ttl = mshcfg->element_ttl; + if (ttl == 0) { + minfo->mshstats.dropped_frames_ttl++; + exit_critical_bh(&path->state_lock); + goto enddiscovery; + } + + if (preq_node->flags & RTW_PREQ_Q_F_REFRESH) + target_flags |= RTW_IEEE80211_PREQ_TO_FLAG; + else + target_flags &= ~RTW_IEEE80211_PREQ_TO_FLAG; + +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + is_root_add_chk = !!(path->flags & RTW_MESH_PATH_ROOT_ADD_CHK); +#endif + da_is_peer = !!(path->flags & RTW_MESH_PATH_PEER_AKA); + force_preq_bcast = !!(path->flags & RTW_MESH_PATH_BCAST_PREQ); + exit_critical_bh(&path->state_lock); + + da = rtw_hwmp_preq_da(path, is_root_add_chk, + da_is_peer, force_preq_bcast); + +#ifdef CONFIG_RTW_MESH_ON_DMD_GANN + flags = (mshcfg->dot11MeshGateAnnouncementProtocol) + ? RTW_IEEE80211_PREQ_IS_GATE_FLAG : 0; +#endif + rtw_mesh_path_sel_frame_tx(RTW_MPATH_PREQ, flags, adapter_mac_addr(adapter), minfo->sn, + target_flags, path->dst, path->sn, da, 0, + ttl, lifetime, 0, minfo->preq_id++, adapter); + rtw_mod_timer(&path->timer, rtw_get_current_time() + path->discovery_timeout); + +enddiscovery: + rtw_rcu_read_unlock(); + rtw_mfree(preq_node, sizeof(struct rtw_mesh_preq_queue)); +} + +void rtw_mesh_path_timer(void *ctx) +{ + struct rtw_mesh_path *path = (void *) ctx; + _adapter *adapter = path->adapter; + int ret; + u8 retry = 0; +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; +#endif + /* TBD: Proctect for suspend */ +#if 0 + if (suspending) + return; +#endif + enter_critical_bh(&path->state_lock); + if (path->flags & RTW_MESH_PATH_RESOLVED || + (!(path->flags & RTW_MESH_PATH_RESOLVING))) { + path->flags &= ~(RTW_MESH_PATH_RESOLVING | + RTW_MESH_PATH_RESOLVED | + RTW_MESH_PATH_ROOT_ADD_CHK | + RTW_MESH_PATH_PEER_AKA | + RTW_MESH_PATH_BCAST_PREQ); + exit_critical_bh(&path->state_lock); + } else if (path->discovery_retries < rtw_max_preq_retries(adapter)) { + ++path->discovery_retries; + path->discovery_timeout *= 2; + path->flags &= ~RTW_MESH_PATH_REQ_QUEUED; +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + if (path->discovery_retries > mshcfg->max_root_add_chk_cnt) + path->flags &= ~RTW_MESH_PATH_ROOT_ADD_CHK; +#endif + if (path->gate_asked) + retry |= RTW_PREQ_Q_F_REFRESH; + + exit_critical_bh(&path->state_lock); + rtw_mesh_queue_preq(path, retry); + } else { + path->flags &= ~(RTW_MESH_PATH_RESOLVING | + RTW_MESH_PATH_RESOLVED | + RTW_MESH_PATH_REQ_QUEUED | + RTW_MESH_PATH_ROOT_ADD_CHK | + RTW_MESH_PATH_PEER_AKA | + RTW_MESH_PATH_BCAST_PREQ); + path->exp_time = rtw_get_current_time(); + exit_critical_bh(&path->state_lock); + if (!path->is_gate && rtw_mesh_gate_num(adapter) > 0) { + ret = rtw_mesh_path_send_to_gates(path); + if (ret) + RTW_HWMP_DBG("no gate was reachable\n"); + } else + rtw_mesh_path_flush_pending(path); + } +} + + +void rtw_mesh_path_tx_root_frame(_adapter *adapter) +{ + struct rtw_mesh_cfg *mshcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + u32 interval = mshcfg->dot11MeshHWMPRannInterval; + u8 flags, target_flags = 0; + + flags = (mshcfg->dot11MeshGateAnnouncementProtocol) + ? RTW_RANN_FLAG_IS_GATE : 0; + + switch (mshcfg->dot11MeshHWMPRootMode) { + case RTW_IEEE80211_PROACTIVE_RANN: + rtw_mesh_path_sel_frame_tx(RTW_MPATH_RANN, flags, adapter_mac_addr(adapter), + ++minfo->sn, 0, NULL, 0, bcast_addr, + 0, mshcfg->element_ttl, + interval, 0, 0, adapter); + break; + case RTW_IEEE80211_PROACTIVE_PREQ_WITH_PREP: + flags |= RTW_IEEE80211_PREQ_PROACTIVE_PREP_FLAG; + case RTW_IEEE80211_PROACTIVE_PREQ_NO_PREP: + interval = mshcfg->dot11MeshHWMPactivePathToRootTimeout; + target_flags |= RTW_IEEE80211_PREQ_TO_FLAG | + RTW_IEEE80211_PREQ_USN_FLAG; + rtw_mesh_path_sel_frame_tx(RTW_MPATH_PREQ, flags, adapter_mac_addr(adapter), + ++minfo->sn, target_flags, + (u8 *) bcast_addr, 0, bcast_addr, + 0, mshcfg->element_ttl, interval, + 0, minfo->preq_id++, adapter); + break; + default: + RTW_HWMP_INFO("Proactive mechanism not supported\n"); + return; + } +} + +void rtw_mesh_work(_workitem *work) +{ + /* use kernel global workqueue */ + _set_workitem(work); +} + +void rtw_ieee80211_mesh_path_timer(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + rtw_mesh_work(&adapter->mesh_work); +} + +void rtw_ieee80211_mesh_path_root_timer(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + + rtw_set_bit(RTW_MESH_WORK_ROOT, &adapter->wrkq_flags); + + rtw_mesh_work(&adapter->mesh_work); +} + +static void rtw_ieee80211_mesh_rootpath(_adapter *adapter) +{ + u32 interval; + + rtw_mesh_path_tx_root_frame(adapter); + + if (adapter->mesh_cfg.dot11MeshHWMPRootMode == RTW_IEEE80211_PROACTIVE_RANN) + interval = adapter->mesh_cfg.dot11MeshHWMPRannInterval; + else + interval = adapter->mesh_cfg.dot11MeshHWMProotInterval; + + rtw_mod_timer(&adapter->mesh_path_root_timer, + RTW_TU_TO_EXP_TIME(interval)); +} + +BOOLEAN rtw_ieee80211_mesh_root_setup(_adapter *adapter) +{ + BOOLEAN root_enabled = _FALSE; + + if (adapter->mesh_cfg.dot11MeshHWMPRootMode > RTW_IEEE80211_ROOTMODE_ROOT) { + rtw_set_bit(RTW_MESH_WORK_ROOT, &adapter->wrkq_flags); + root_enabled = _TRUE; + } + else { + rtw_clear_bit(RTW_MESH_WORK_ROOT, &adapter->wrkq_flags); + /* stop running timer */ + _cancel_timer_ex(&adapter->mesh_path_root_timer); + root_enabled = _FALSE; + } + + return root_enabled; +} + +void rtw_mesh_work_hdl(_workitem *work) +{ + _adapter *adapter = container_of(work, _adapter, mesh_work); + + while(adapter->mesh_info.preq_queue_len) { + if (rtw_time_after(rtw_get_current_time(), + adapter->mesh_info.last_preq + rtw_min_preq_int_jiff(adapter))) + /* It will consume preq_queue_len */ + rtw_mesh_path_start_discovery(adapter); + else { + struct rtw_mesh_info *minfo = &adapter->mesh_info; + + rtw_mod_timer(&adapter->mesh_path_timer, + minfo->last_preq + rtw_min_preq_int_jiff(adapter) + 1); + break; + } + } + + if (rtw_test_and_clear_bit(RTW_MESH_WORK_ROOT, &adapter->wrkq_flags)) + rtw_ieee80211_mesh_rootpath(adapter); +} + +#ifndef RTW_PER_CMD_SUPPORT_FW +static void rtw_update_metric_directly(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + u8 i; + + for (i = 0; i < macid_ctl->num; i++) { + u8 role; + role = GET_H2CCMD_MSRRPT_PARM_ROLE(&macid_ctl->h2c_msr[i]); + if (role == H2C_MSR_ROLE_MESH) { + struct sta_info *sta = macid_ctl->sta[i]; + u8 rate_idx, sgi, bw; + u32 rate; + + if (!sta) + continue; + rate_idx = rtw_get_current_tx_rate(adapter, sta); + sgi = rtw_get_current_tx_sgi(adapter, sta); + bw = sta->cmn.bw_mode; + rate = rtw_desc_rate_to_bitrate(bw, rate_idx, sgi); + sta->metrics.data_rate = rate; + } + } +} +#endif + +void rtw_mesh_atlm_param_req_timer(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + u8 ret = _FAIL; + +#ifdef RTW_PER_CMD_SUPPORT_FW + ret = rtw_req_per_cmd(adapter); + if (ret == _FAIL) + RTW_HWMP_INFO("rtw_req_per_cmd fail\n"); +#else + rtw_update_metric_directly(adapter); +#endif + _set_timer(&adapter->mesh_atlm_param_req_timer, RTW_ATLM_REQ_CYCLE); +} + +#endif /* CONFIG_RTW_MESH */ + diff --git a/core/mesh/rtw_mesh_hwmp.h b/core/mesh/rtw_mesh_hwmp.h new file mode 100644 index 0000000..9433417 --- /dev/null +++ b/core/mesh/rtw_mesh_hwmp.h @@ -0,0 +1,60 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifndef __RTW_MESH_HWMP_H_ +#define __RTW_MESH_HWMP_H_ + +#ifndef DBG_RTW_HWMP +#define DBG_RTW_HWMP 0 +#endif +#if DBG_RTW_HWMP +#define RTW_HWMP_DBG(fmt, arg...) RTW_PRINT(fmt, ##arg) +#else +#define RTW_HWMP_DBG(fmt, arg...) RTW_DBG(fmt, ##arg) +#endif + +#ifndef INFO_RTW_HWMP +#define INFO_RTW_HWMP 0 +#endif +#if INFO_RTW_HWMP +#define RTW_HWMP_INFO(fmt, arg...) RTW_PRINT(fmt, ##arg) +#else +#define RTW_HWMP_INFO(fmt, arg...) RTW_INFO(fmt, ##arg) +#endif + + +void rtw_ewma_err_rate_init(struct rtw_ewma_err_rate *e); +unsigned long rtw_ewma_err_rate_read(struct rtw_ewma_err_rate *e); +void rtw_ewma_err_rate_add(struct rtw_ewma_err_rate *e, unsigned long val); +int rtw_mesh_path_error_tx(_adapter *adapter, + u8 ttl, const u8 *target, u32 target_sn, + u16 target_rcode, const u8 *ra); +void rtw_ieee80211s_update_metric(_adapter *adapter, u8 mac_id, + u8 per, u8 rate, + u8 bw, u8 total_pkt); +void rtw_mesh_rx_path_sel_frame(_adapter *adapter, union recv_frame *rframe); +void rtw_mesh_queue_preq(struct rtw_mesh_path *mpath, u8 flags); +void rtw_mesh_path_start_discovery(_adapter *adapter); +void rtw_mesh_path_timer(void *ctx); +void rtw_mesh_path_tx_root_frame(_adapter *adapter); +void rtw_mesh_work_hdl(_workitem *work); +void rtw_ieee80211_mesh_path_timer(void *ctx); +void rtw_ieee80211_mesh_path_root_timer(void *ctx); +BOOLEAN rtw_ieee80211_mesh_root_setup(_adapter *adapter); +void rtw_mesh_work(_workitem *work); +void rtw_mesh_atlm_param_req_timer(void *ctx); + +#endif /* __RTW_MESH_HWMP_H_ */ + + diff --git a/core/mesh/rtw_mesh_pathtbl.c b/core/mesh/rtw_mesh_pathtbl.c new file mode 100644 index 0000000..b74b567 --- /dev/null +++ b/core/mesh/rtw_mesh_pathtbl.c @@ -0,0 +1,1242 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_MESH_PATHTBL_C_ + +#ifdef CONFIG_RTW_MESH +#include +#include + +#ifdef PLATFORM_LINUX +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 0, 0)) +static void rtw_mpath_free_rcu(struct rtw_mesh_path *mpath) +{ + kfree_rcu(mpath, rcu); + rtw_mstat_update(MSTAT_TYPE_PHY, MSTAT_FREE, sizeof(struct rtw_mesh_path)); +} +#else +static void rtw_mpath_free_rcu_callback(rtw_rcu_head *head) +{ + struct rtw_mesh_path *mpath; + + mpath = container_of(head, struct rtw_mesh_path, rcu); + rtw_mfree(mpath, sizeof(struct rtw_mesh_path)); +} + +static void rtw_mpath_free_rcu(struct rtw_mesh_path *mpath) +{ + call_rcu(&mpath->rcu, rtw_mpath_free_rcu_callback); +} +#endif +#endif /* PLATFORM_LINUX */ + +static void rtw_mesh_path_free_rcu(struct rtw_mesh_table *tbl, struct rtw_mesh_path *mpath); + +static u32 rtw_mesh_table_hash(const void *addr, u32 len, u32 seed) +{ + /* Use last four bytes of hw addr as hash index */ + return jhash_1word(*(u32 *)(addr+2), seed); +} + +static const rtw_rhashtable_params rtw_mesh_rht_params = { + .nelem_hint = 2, + .automatic_shrinking = true, + .key_len = ETH_ALEN, + .key_offset = offsetof(struct rtw_mesh_path, dst), + .head_offset = offsetof(struct rtw_mesh_path, rhash), + .hashfn = rtw_mesh_table_hash, +}; + +static inline bool rtw_mpath_expired(struct rtw_mesh_path *mpath) +{ + return (mpath->flags & RTW_MESH_PATH_ACTIVE) && + rtw_time_after(rtw_get_current_time(), mpath->exp_time) && + !(mpath->flags & RTW_MESH_PATH_FIXED); +} + +static void rtw_mesh_path_rht_free(void *ptr, void *tblptr) +{ + struct rtw_mesh_path *mpath = ptr; + struct rtw_mesh_table *tbl = tblptr; + + rtw_mesh_path_free_rcu(tbl, mpath); +} + +static struct rtw_mesh_table *rtw_mesh_table_alloc(void) +{ + struct rtw_mesh_table *newtbl; + + newtbl = rtw_malloc(sizeof(struct rtw_mesh_table)); + if (!newtbl) + return NULL; + + rtw_hlist_head_init(&newtbl->known_gates); + ATOMIC_SET(&newtbl->entries, 0); + _rtw_spinlock_init(&newtbl->gates_lock); + + return newtbl; +} + +static void rtw_mesh_table_free(struct rtw_mesh_table *tbl) +{ + rtw_rhashtable_free_and_destroy(&tbl->rhead, + rtw_mesh_path_rht_free, tbl); + rtw_mfree(tbl, sizeof(struct rtw_mesh_table)); +} + +/** + * + * rtw_mesh_path_assign_nexthop - update mesh path next hop + * + * @mpath: mesh path to update + * @sta: next hop to assign + * + * Locking: mpath->state_lock must be held when calling this function + */ +void rtw_mesh_path_assign_nexthop(struct rtw_mesh_path *mpath, struct sta_info *sta) +{ + struct xmit_frame *xframe; + _list *list, *head; + + rtw_rcu_assign_pointer(mpath->next_hop, sta); + + enter_critical_bh(&mpath->frame_queue.lock); + head = &mpath->frame_queue.queue; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + xframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + _rtw_memcpy(xframe->attrib.ra, sta->cmn.mac_addr, ETH_ALEN); + } + + exit_critical_bh(&mpath->frame_queue.lock); +} + +static void rtw_prepare_for_gate(struct xmit_frame *xframe, char *dst_addr, + struct rtw_mesh_path *gate_mpath) +{ + struct pkt_attrib *attrib = &xframe->attrib; + char *next_hop; + + if (attrib->mesh_frame_mode == MESH_UCAST_DATA) + attrib->mesh_frame_mode = MESH_UCAST_PX_DATA; + + /* update next hop */ + rtw_rcu_read_lock(); + next_hop = rtw_rcu_dereference(gate_mpath->next_hop)->cmn.mac_addr; + _rtw_memcpy(attrib->ra, next_hop, ETH_ALEN); + rtw_rcu_read_unlock(); + _rtw_memcpy(attrib->mda, dst_addr, ETH_ALEN); +} + +/** + * + * rtw_mesh_path_move_to_queue - Move or copy frames from one mpath queue to another + * + * This function is used to transfer or copy frames from an unresolved mpath to + * a gate mpath. The function also adds the Address Extension field and + * updates the next hop. + * + * If a frame already has an Address Extension field, only the next hop and + * destination addresses are updated. + * + * The gate mpath must be an active mpath with a valid mpath->next_hop. + * + * @mpath: An active mpath the frames will be sent to (i.e. the gate) + * @from_mpath: The failed mpath + * @copy: When true, copy all the frames to the new mpath queue. When false, + * move them. + */ +static void rtw_mesh_path_move_to_queue(struct rtw_mesh_path *gate_mpath, + struct rtw_mesh_path *from_mpath, + bool copy) +{ + struct xmit_frame *fskb; + _list *list, *head; + _list failq; + u32 failq_len; + _irqL flags; + + if (rtw_warn_on(gate_mpath == from_mpath)) + return; + if (rtw_warn_on(!gate_mpath->next_hop)) + return; + + _rtw_init_listhead(&failq); + + _enter_critical_bh(&from_mpath->frame_queue.lock, &flags); + rtw_list_splice_init(&from_mpath->frame_queue.queue, &failq); + failq_len = from_mpath->frame_queue_len; + from_mpath->frame_queue_len = 0; + _exit_critical_bh(&from_mpath->frame_queue.lock, &flags); + + head = &failq; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + if (gate_mpath->frame_queue_len >= RTW_MESH_FRAME_QUEUE_LEN) { + RTW_MPATH_DBG(FUNC_ADPT_FMT" mpath queue for gate %pM is full!\n" + , FUNC_ADPT_ARG(gate_mpath->adapter), gate_mpath->dst); + break; + } + + fskb = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + + rtw_list_delete(&fskb->list); + failq_len--; + rtw_prepare_for_gate(fskb, gate_mpath->dst, gate_mpath); + _enter_critical_bh(&gate_mpath->frame_queue.lock, &flags); + rtw_list_insert_tail(&fskb->list, get_list_head(&gate_mpath->frame_queue)); + gate_mpath->frame_queue_len++; + _exit_critical_bh(&gate_mpath->frame_queue.lock, &flags); + + #if 0 /* TODO: copy */ + skb = rtw_skb_copy(fskb); + if (rtw_warn_on(!skb)) + break; + + rtw_prepare_for_gate(skb, gate_mpath->dst, gate_mpath); + skb_queue_tail(&gate_mpath->frame_queue, skb); + + if (copy) + continue; + + __skb_unlink(fskb, &failq); + rtw_skb_free(fskb); + #endif + } + + RTW_MPATH_DBG(FUNC_ADPT_FMT" mpath queue for gate %pM has %d frames\n" + , FUNC_ADPT_ARG(gate_mpath->adapter), gate_mpath->dst, gate_mpath->frame_queue_len); + + if (!copy) + return; + + _enter_critical_bh(&from_mpath->frame_queue.lock, &flags); + rtw_list_splice(&failq, &from_mpath->frame_queue.queue); + from_mpath->frame_queue_len += failq_len; + _exit_critical_bh(&from_mpath->frame_queue.lock, &flags); +} + + +static struct rtw_mesh_path *rtw_mpath_lookup(struct rtw_mesh_table *tbl, const u8 *dst) +{ + struct rtw_mesh_path *mpath; + + if (!tbl) + return NULL; + + mpath = rtw_rhashtable_lookup_fast(&tbl->rhead, dst, rtw_mesh_rht_params); + + if (mpath && rtw_mpath_expired(mpath)) { + enter_critical_bh(&mpath->state_lock); + mpath->flags &= ~RTW_MESH_PATH_ACTIVE; + exit_critical_bh(&mpath->state_lock); + } + return mpath; +} + +/** + * rtw_mesh_path_lookup - look up a path in the mesh path table + * @sdata: local subif + * @dst: hardware address (ETH_ALEN length) of destination + * + * Returns: pointer to the mesh path structure, or NULL if not found + * + * Locking: must be called within a read rcu section. + */ +struct rtw_mesh_path * +rtw_mesh_path_lookup(_adapter *adapter, const u8 *dst) +{ + return rtw_mpath_lookup(adapter->mesh_info.mesh_paths, dst); +} + +struct rtw_mesh_path * +rtw_mpp_path_lookup(_adapter *adapter, const u8 *dst) +{ + return rtw_mpath_lookup(adapter->mesh_info.mpp_paths, dst); +} + +static struct rtw_mesh_path * +__rtw_mesh_path_lookup_by_idx(struct rtw_mesh_table *tbl, int idx) +{ + int i = 0, ret; + struct rtw_mesh_path *mpath = NULL; + rtw_rhashtable_iter iter; + + if (!tbl) + return NULL; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return NULL; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto err; + + while ((mpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(mpath) && PTR_ERR(mpath) == -EAGAIN) + continue; + if (IS_ERR(mpath)) + break; + if (i++ == idx) + break; + } +err: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); + + if (IS_ERR(mpath) || !mpath) + return NULL; + + if (rtw_mpath_expired(mpath)) { + enter_critical_bh(&mpath->state_lock); + mpath->flags &= ~RTW_MESH_PATH_ACTIVE; + exit_critical_bh(&mpath->state_lock); + } + return mpath; +} + +/** + * rtw_mesh_path_lookup_by_idx - look up a path in the mesh path table by its index + * @idx: index + * @sdata: local subif, or NULL for all entries + * + * Returns: pointer to the mesh path structure, or NULL if not found. + * + * Locking: must be called within a read rcu section. + */ +struct rtw_mesh_path * +rtw_mesh_path_lookup_by_idx(_adapter *adapter, int idx) +{ + return __rtw_mesh_path_lookup_by_idx(adapter->mesh_info.mesh_paths, idx); +} + +void dump_mpath(void *sel, _adapter *adapter) +{ + struct rtw_mesh_path *mpath; + int idx = 0; + char dst[ETH_ALEN]; + char next_hop[ETH_ALEN]; + u32 sn, metric, qlen; + u32 exp_ms = 0, dto_ms; + u8 drty; + enum rtw_mesh_path_flags flags; + + RTW_PRINT_SEL(sel, "%-17s %-17s %-10s %-10s %-4s %-6s %-6s %-4s flags\n" + , "dst", "next_hop", "sn", "metric", "qlen", "exp_ms", "dto_ms", "drty" + ); + + do { + rtw_rcu_read_lock(); + + mpath = rtw_mesh_path_lookup_by_idx(adapter, idx); + if (mpath) { + _rtw_memcpy(dst, mpath->dst, ETH_ALEN); + _rtw_memcpy(next_hop, mpath->next_hop->cmn.mac_addr, ETH_ALEN); + sn = mpath->sn; + metric = mpath->metric; + qlen = mpath->frame_queue_len; + if (rtw_time_after(mpath->exp_time, rtw_get_current_time())) + exp_ms = rtw_get_remaining_time_ms(mpath->exp_time); + dto_ms = rtw_systime_to_ms(mpath->discovery_timeout); + drty = mpath->discovery_retries; + flags = mpath->flags; + } + + rtw_rcu_read_unlock(); + + if (mpath) { + RTW_PRINT_SEL(sel, MAC_FMT" "MAC_FMT" %10u %10u %4u %6u %6u %4u%s%s%s%s%s%s%s%s%s%s\n" + , MAC_ARG(dst), MAC_ARG(next_hop), sn, metric, qlen + , exp_ms < 999999 ? exp_ms : 999999 + , dto_ms < 999999 ? dto_ms : 999999 + , drty + , (flags & RTW_MESH_PATH_ACTIVE) ? " ACT" : "" + , (flags & RTW_MESH_PATH_RESOLVING) ? " RSVING" : "" + , (flags & RTW_MESH_PATH_SN_VALID) ? " SN_VALID" : "" + , (flags & RTW_MESH_PATH_FIXED) ? " FIXED" : "" + , (flags & RTW_MESH_PATH_RESOLVED) ? " RSVED" : "" + , (flags & RTW_MESH_PATH_REQ_QUEUED) ? " REQ_IN_Q" : "" + , (flags & RTW_MESH_PATH_DELETED) ? " DELETED" : "" + , (flags & RTW_MESH_PATH_ROOT_ADD_CHK) ? " R_ADD_CHK" : "" + , (flags & RTW_MESH_PATH_PEER_AKA) ? " PEER_AKA" : "" + , (flags & RTW_MESH_PATH_BCAST_PREQ) ? " BC_PREQ" : "" + ); + } + + idx++; + } while (mpath); +} + +/** + * rtw_mpp_path_lookup_by_idx - look up a path in the proxy path table by its index + * @idx: index + * @sdata: local subif, or NULL for all entries + * + * Returns: pointer to the proxy path structure, or NULL if not found. + * + * Locking: must be called within a read rcu section. + */ +struct rtw_mesh_path * +rtw_mpp_path_lookup_by_idx(_adapter *adapter, int idx) +{ + return __rtw_mesh_path_lookup_by_idx(adapter->mesh_info.mpp_paths, idx); +} + +/** + * rtw_mesh_path_add_gate - add the given mpath to a mesh gate to our path table + * @mpath: gate path to add to table + */ +int rtw_mesh_path_add_gate(struct rtw_mesh_path *mpath) +{ + struct rtw_mesh_cfg *mcfg; + struct rtw_mesh_info *minfo; + struct rtw_mesh_table *tbl; + int err, ori_num_gates; + + rtw_rcu_read_lock(); + tbl = mpath->adapter->mesh_info.mesh_paths; + if (!tbl) { + err = -ENOENT; + goto err_rcu; + } + + enter_critical_bh(&mpath->state_lock); + mcfg = &mpath->adapter->mesh_cfg; + mpath->gate_timeout = rtw_get_current_time() + + rtw_ms_to_systime(mcfg->path_gate_timeout_factor * + mpath->gate_ann_int); + if (mpath->is_gate) { + err = -EEXIST; + exit_critical_bh(&mpath->state_lock); + goto err_rcu; + } + + minfo = &mpath->adapter->mesh_info; + mpath->is_gate = true; + _rtw_spinlock(&tbl->gates_lock); + ori_num_gates = minfo->num_gates; + minfo->num_gates++; + rtw_hlist_add_head_rcu(&mpath->gate_list, &tbl->known_gates); + + if (ori_num_gates == 0 + || rtw_macaddr_is_larger(mpath->dst, minfo->max_addr_gate->dst) + ) { + minfo->max_addr_gate = mpath; + minfo->max_addr_gate_is_larger_than_self = + rtw_macaddr_is_larger(mpath->dst, adapter_mac_addr(mpath->adapter)); + } + + _rtw_spinunlock(&tbl->gates_lock); + + exit_critical_bh(&mpath->state_lock); + + if (ori_num_gates == 0) { + update_beacon(mpath->adapter, WLAN_EID_MESH_CONFIG, NULL, _TRUE, 0); + #if CONFIG_RTW_MESH_CTO_MGATE_CARRIER + if (!rtw_mesh_cto_mgate_required(mpath->adapter)) + rtw_netif_carrier_on(mpath->adapter->pnetdev); + #endif + } + + RTW_MPATH_DBG( + FUNC_ADPT_FMT" Mesh path: Recorded new gate: %pM. %d known gates\n", + FUNC_ADPT_ARG(mpath->adapter), + mpath->dst, mpath->adapter->mesh_info.num_gates); + err = 0; +err_rcu: + rtw_rcu_read_unlock(); + return err; +} + +/** + * rtw_mesh_gate_del - remove a mesh gate from the list of known gates + * @tbl: table which holds our list of known gates + * @mpath: gate mpath + */ +void rtw_mesh_gate_del(struct rtw_mesh_table *tbl, struct rtw_mesh_path *mpath) +{ + struct rtw_mesh_cfg *mcfg; + struct rtw_mesh_info *minfo; + int ori_num_gates; + + rtw_lockdep_assert_held(&mpath->state_lock); + if (!mpath->is_gate) + return; + + mcfg = &mpath->adapter->mesh_cfg; + minfo = &mpath->adapter->mesh_info; + + mpath->is_gate = false; + enter_critical_bh(&tbl->gates_lock); + rtw_hlist_del_rcu(&mpath->gate_list); + ori_num_gates = minfo->num_gates; + minfo->num_gates--; + + if (ori_num_gates == 1) { + minfo->max_addr_gate = NULL; + minfo->max_addr_gate_is_larger_than_self = 0; + } else if (minfo->max_addr_gate == mpath) { + struct rtw_mesh_path *gate, *max_addr_gate = NULL; + rtw_hlist_node *node; + + rtw_hlist_for_each_entry_rcu(gate, node, &tbl->known_gates, gate_list) { + if (!max_addr_gate || rtw_macaddr_is_larger(gate->dst, max_addr_gate->dst)) + max_addr_gate = gate; + } + minfo->max_addr_gate = max_addr_gate; + minfo->max_addr_gate_is_larger_than_self = + rtw_macaddr_is_larger(max_addr_gate->dst, adapter_mac_addr(mpath->adapter)); + } + + exit_critical_bh(&tbl->gates_lock); + + if (ori_num_gates == 1) { + update_beacon(mpath->adapter, WLAN_EID_MESH_CONFIG, NULL, _TRUE, 0); + #if CONFIG_RTW_MESH_CTO_MGATE_CARRIER + if (rtw_mesh_cto_mgate_required(mpath->adapter)) + rtw_netif_carrier_off(mpath->adapter->pnetdev); + #endif + } + + RTW_MPATH_DBG( + FUNC_ADPT_FMT" Mesh path: Deleted gate: %pM. %d known gates\n", + FUNC_ADPT_ARG(mpath->adapter), + mpath->dst, mpath->adapter->mesh_info.num_gates); +} + +/** + * rtw_mesh_gate_search - search a mesh gate from the list of known gates + * @tbl: table which holds our list of known gates + * @addr: address of gate + */ +bool rtw_mesh_gate_search(struct rtw_mesh_table *tbl, const u8 *addr) +{ + struct rtw_mesh_path *gate; + rtw_hlist_node *node; + bool exist = 0; + + rtw_rcu_read_lock(); + rtw_hlist_for_each_entry_rcu(gate, node, &tbl->known_gates, gate_list) { + if (_rtw_memcmp(gate->dst, addr, ETH_ALEN) == _TRUE) { + exist = 1; + break; + } + } + + rtw_rcu_read_unlock(); + + return exist; +} + +/** + * rtw_mesh_gate_num - number of gates known to this interface + * @sdata: subif data + */ +int rtw_mesh_gate_num(_adapter *adapter) +{ + return adapter->mesh_info.num_gates; +} + +bool rtw_mesh_is_primary_gate(_adapter *adapter) +{ + struct rtw_mesh_cfg *mcfg = &adapter->mesh_cfg; + struct rtw_mesh_info *minfo = &adapter->mesh_info; + + return mcfg->dot11MeshGateAnnouncementProtocol + && !minfo->max_addr_gate_is_larger_than_self; +} + +void dump_known_gates(void *sel, _adapter *adapter) +{ + struct rtw_mesh_info *minfo = &adapter->mesh_info; + struct rtw_mesh_table *tbl; + struct rtw_mesh_path *gate; + rtw_hlist_node *node; + + if (!rtw_mesh_gate_num(adapter)) + goto exit; + + rtw_rcu_read_lock(); + + tbl = minfo->mesh_paths; + if (!tbl) + goto unlock; + + RTW_PRINT_SEL(sel, "num:%d\n", rtw_mesh_gate_num(adapter)); + + rtw_hlist_for_each_entry_rcu(gate, node, &tbl->known_gates, gate_list) { + RTW_PRINT_SEL(sel, "%c"MAC_FMT"\n" + , gate == minfo->max_addr_gate ? '*' : ' ' + , MAC_ARG(gate->dst)); + } + +unlock: + rtw_rcu_read_unlock(); +exit: + return; +} + +static +struct rtw_mesh_path *rtw_mesh_path_new(_adapter *adapter, + const u8 *dst) +{ + struct rtw_mesh_path *new_mpath; + + new_mpath = rtw_zmalloc(sizeof(struct rtw_mesh_path)); + if (!new_mpath) + return NULL; + + _rtw_memcpy(new_mpath->dst, dst, ETH_ALEN); + _rtw_memset(new_mpath->rann_snd_addr, 0xFF, ETH_ALEN); + new_mpath->is_root = false; + new_mpath->adapter = adapter; + new_mpath->flags = 0; + new_mpath->gate_asked = false; + _rtw_init_queue(&new_mpath->frame_queue); + new_mpath->frame_queue_len = 0; + new_mpath->exp_time = rtw_get_current_time(); + _rtw_spinlock_init(&new_mpath->state_lock); + rtw_init_timer(&new_mpath->timer, adapter, rtw_mesh_path_timer, new_mpath); + + return new_mpath; +} + +/** + * rtw_mesh_path_add - allocate and add a new path to the mesh path table + * @dst: destination address of the path (ETH_ALEN length) + * @sdata: local subif + * + * Returns: 0 on success + * + * State: the initial state of the new path is set to 0 + */ +struct rtw_mesh_path *rtw_mesh_path_add(_adapter *adapter, + const u8 *dst) +{ + struct rtw_mesh_table *tbl = adapter->mesh_info.mesh_paths; + struct rtw_mesh_path *mpath, *new_mpath; + int ret; + + if (!tbl) + return ERR_PTR(-ENOTSUPP); + + if (_rtw_memcmp(dst, adapter_mac_addr(adapter), ETH_ALEN) == _TRUE) + /* never add ourselves as neighbours */ + return ERR_PTR(-ENOTSUPP); + + if (is_multicast_mac_addr(dst)) + return ERR_PTR(-ENOTSUPP); + + if (ATOMIC_INC_UNLESS(&adapter->mesh_info.mpaths, RTW_MESH_MAX_MPATHS) == 0) + return ERR_PTR(-ENOSPC); + + new_mpath = rtw_mesh_path_new(adapter, dst); + if (!new_mpath) + return ERR_PTR(-ENOMEM); + + do { + ret = rtw_rhashtable_lookup_insert_fast(&tbl->rhead, + &new_mpath->rhash, + rtw_mesh_rht_params); + + if (ret == -EEXIST) + mpath = rtw_rhashtable_lookup_fast(&tbl->rhead, + dst, + rtw_mesh_rht_params); + + } while (unlikely(ret == -EEXIST && !mpath)); + + if (ret && ret != -EEXIST) + return ERR_PTR(ret); + + /* At this point either new_mpath was added, or we found a + * matching entry already in the table; in the latter case + * free the unnecessary new entry. + */ + if (ret == -EEXIST) { + rtw_mfree(new_mpath, sizeof(struct rtw_mesh_path)); + new_mpath = mpath; + } + adapter->mesh_info.mesh_paths_generation++; + return new_mpath; +} + +int rtw_mpp_path_add(_adapter *adapter, + const u8 *dst, const u8 *mpp) +{ + struct rtw_mesh_table *tbl = adapter->mesh_info.mpp_paths; + struct rtw_mesh_path *new_mpath; + int ret; + + if (!tbl) + return -ENOTSUPP; + + if (_rtw_memcmp(dst, adapter_mac_addr(adapter), ETH_ALEN) == _TRUE) + /* never add ourselves as neighbours */ + return -ENOTSUPP; + + if (is_multicast_mac_addr(dst)) + return -ENOTSUPP; + + new_mpath = rtw_mesh_path_new(adapter, dst); + + if (!new_mpath) + return -ENOMEM; + + _rtw_memcpy(new_mpath->mpp, mpp, ETH_ALEN); + ret = rtw_rhashtable_lookup_insert_fast(&tbl->rhead, + &new_mpath->rhash, + rtw_mesh_rht_params); + + adapter->mesh_info.mpp_paths_generation++; + return ret; +} + +void dump_mpp(void *sel, _adapter *adapter) +{ + struct rtw_mesh_path *mpath; + int idx = 0; + char dst[ETH_ALEN]; + char mpp[ETH_ALEN]; + + RTW_PRINT_SEL(sel, "%-17s %-17s\n", "dst", "mpp"); + + do { + rtw_rcu_read_lock(); + + mpath = rtw_mpp_path_lookup_by_idx(adapter, idx); + if (mpath) { + _rtw_memcpy(dst, mpath->dst, ETH_ALEN); + _rtw_memcpy(mpp, mpath->mpp, ETH_ALEN); + } + + rtw_rcu_read_unlock(); + + if (mpath) { + RTW_PRINT_SEL(sel, MAC_FMT" "MAC_FMT"\n" + , MAC_ARG(dst), MAC_ARG(mpp)); + } + + idx++; + } while (mpath); +} + +/** + * rtw_mesh_plink_broken - deactivates paths and sends perr when a link breaks + * + * @sta: broken peer link + * + * This function must be called from the rate control algorithm if enough + * delivery errors suggest that a peer link is no longer usable. + */ +void rtw_mesh_plink_broken(struct sta_info *sta) +{ + _adapter *adapter = sta->padapter; + struct rtw_mesh_table *tbl = adapter->mesh_info.mesh_paths; + static const u8 bcast[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct rtw_mesh_path *mpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((mpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(mpath) && PTR_ERR(mpath) == -EAGAIN) + continue; + if (IS_ERR(mpath)) + break; + if (rtw_rcu_access_pointer(mpath->next_hop) == sta && + mpath->flags & RTW_MESH_PATH_ACTIVE && + !(mpath->flags & RTW_MESH_PATH_FIXED)) { + enter_critical_bh(&mpath->state_lock); + mpath->flags &= ~RTW_MESH_PATH_ACTIVE; + ++mpath->sn; + exit_critical_bh(&mpath->state_lock); + rtw_mesh_path_error_tx(adapter, + adapter->mesh_cfg.element_ttl, + mpath->dst, mpath->sn, + WLAN_REASON_MESH_PATH_DEST_UNREACHABLE, bcast); + } + } +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +static void rtw_mesh_path_free_rcu(struct rtw_mesh_table *tbl, + struct rtw_mesh_path *mpath) +{ + _adapter *adapter = mpath->adapter; + + enter_critical_bh(&mpath->state_lock); + mpath->flags |= RTW_MESH_PATH_RESOLVING | RTW_MESH_PATH_DELETED; + rtw_mesh_gate_del(tbl, mpath); + exit_critical_bh(&mpath->state_lock); + _cancel_timer_ex(&mpath->timer); + ATOMIC_DEC(&adapter->mesh_info.mpaths); + ATOMIC_DEC(&tbl->entries); + _rtw_spinlock_free(&mpath->state_lock); + + rtw_mesh_path_flush_pending(mpath); + + rtw_mpath_free_rcu(mpath); +} + +static void __rtw_mesh_path_del(struct rtw_mesh_table *tbl, struct rtw_mesh_path *mpath) +{ + rtw_rhashtable_remove_fast(&tbl->rhead, &mpath->rhash, rtw_mesh_rht_params); + rtw_mesh_path_free_rcu(tbl, mpath); +} + +/** + * rtw_mesh_path_flush_by_nexthop - Deletes mesh paths if their next hop matches + * + * @sta: mesh peer to match + * + * RCU notes: this function is called when a mesh plink transitions from + * PLINK_ESTAB to any other state, since PLINK_ESTAB state is the only one that + * allows path creation. This will happen before the sta can be freed (because + * sta_info_destroy() calls this) so any reader in a rcu read block will be + * protected against the plink disappearing. + */ +void rtw_mesh_path_flush_by_nexthop(struct sta_info *sta) +{ + _adapter *adapter = sta->padapter; + struct rtw_mesh_table *tbl = adapter->mesh_info.mesh_paths; + struct rtw_mesh_path *mpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((mpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(mpath) && PTR_ERR(mpath) == -EAGAIN) + continue; + if (IS_ERR(mpath)) + break; + + if (rtw_rcu_access_pointer(mpath->next_hop) == sta) + __rtw_mesh_path_del(tbl, mpath); + } +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +static void rtw_mpp_flush_by_proxy(_adapter *adapter, + const u8 *proxy) +{ + struct rtw_mesh_table *tbl = adapter->mesh_info.mpp_paths; + struct rtw_mesh_path *mpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((mpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(mpath) && PTR_ERR(mpath) == -EAGAIN) + continue; + if (IS_ERR(mpath)) + break; + + if (_rtw_memcmp(mpath->mpp, proxy, ETH_ALEN) == _TRUE) + __rtw_mesh_path_del(tbl, mpath); + } +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +static void rtw_table_flush_by_iface(struct rtw_mesh_table *tbl) +{ + struct rtw_mesh_path *mpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((mpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(mpath) && PTR_ERR(mpath) == -EAGAIN) + continue; + if (IS_ERR(mpath)) + break; + __rtw_mesh_path_del(tbl, mpath); + } +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +/** + * rtw_mesh_path_flush_by_iface - Deletes all mesh paths associated with a given iface + * + * This function deletes both mesh paths as well as mesh portal paths. + * + * @sdata: interface data to match + * + */ +void rtw_mesh_path_flush_by_iface(_adapter *adapter) +{ + rtw_table_flush_by_iface(adapter->mesh_info.mesh_paths); + rtw_table_flush_by_iface(adapter->mesh_info.mpp_paths); +} + +/** + * rtw_table_path_del - delete a path from the mesh or mpp table + * + * @tbl: mesh or mpp path table + * @sdata: local subif + * @addr: dst address (ETH_ALEN length) + * + * Returns: 0 if successful + */ +static int rtw_table_path_del(struct rtw_mesh_table *tbl, + const u8 *addr) +{ + struct rtw_mesh_path *mpath; + + if (!tbl) + return -ENXIO; + + rtw_rcu_read_lock(); + mpath = rtw_rhashtable_lookup_fast(&tbl->rhead, addr, rtw_mesh_rht_params); + if (!mpath) { + rtw_rcu_read_unlock(); + return -ENXIO; + } + + __rtw_mesh_path_del(tbl, mpath); + rtw_rcu_read_unlock(); + return 0; +} + + +/** + * rtw_mesh_path_del - delete a mesh path from the table + * + * @addr: dst address (ETH_ALEN length) + * @sdata: local subif + * + * Returns: 0 if successful + */ +int rtw_mesh_path_del(_adapter *adapter, const u8 *addr) +{ + int err; + + /* flush relevant mpp entries first */ + rtw_mpp_flush_by_proxy(adapter, addr); + + err = rtw_table_path_del(adapter->mesh_info.mesh_paths, addr); + adapter->mesh_info.mesh_paths_generation++; + return err; +} + +/** + * rtw_mesh_path_tx_pending - sends pending frames in a mesh path queue + * + * @mpath: mesh path to activate + * + * Locking: the state_lock of the mpath structure must NOT be held when calling + * this function. + */ +void rtw_mesh_path_tx_pending(struct rtw_mesh_path *mpath) +{ + if (mpath->flags & RTW_MESH_PATH_ACTIVE) { + struct rtw_mesh_info *minfo = &mpath->adapter->mesh_info; + _list q; + u32 q_len = 0; + + _rtw_init_listhead(&q); + + /* move to local queue */ + enter_critical_bh(&mpath->frame_queue.lock); + if (mpath->frame_queue_len) { + rtw_list_splice_init(&mpath->frame_queue.queue, &q); + q_len = mpath->frame_queue_len; + mpath->frame_queue_len = 0; + } + exit_critical_bh(&mpath->frame_queue.lock); + + if (q_len) { + /* move to mpath_tx_queue */ + enter_critical_bh(&minfo->mpath_tx_queue.lock); + rtw_list_splice_tail(&q, &minfo->mpath_tx_queue.queue); + minfo->mpath_tx_queue_len += q_len; + exit_critical_bh(&minfo->mpath_tx_queue.lock); + + /* schedule mpath_tx_tasklet */ + tasklet_hi_schedule(&minfo->mpath_tx_tasklet); + } + } +} + +/** + * rtw_mesh_path_send_to_gates - sends pending frames to all known mesh gates + * + * @mpath: mesh path whose queue will be emptied + * + * If there is only one gate, the frames are transferred from the failed mpath + * queue to that gate's queue. If there are more than one gates, the frames + * are copied from each gate to the next. After frames are copied, the + * mpath queues are emptied onto the transmission queue. + */ +int rtw_mesh_path_send_to_gates(struct rtw_mesh_path *mpath) +{ + _adapter *adapter = mpath->adapter; + struct rtw_mesh_table *tbl; + struct rtw_mesh_path *from_mpath = mpath; + struct rtw_mesh_path *gate; + bool copy = false; + rtw_hlist_node *node; + + tbl = adapter->mesh_info.mesh_paths; + if (!tbl) + return 0; + + rtw_rcu_read_lock(); + rtw_hlist_for_each_entry_rcu(gate, node, &tbl->known_gates, gate_list) { + if (gate->flags & RTW_MESH_PATH_ACTIVE) { + RTW_MPATH_DBG(FUNC_ADPT_FMT" Forwarding to %pM\n", + FUNC_ADPT_ARG(adapter), gate->dst); + rtw_mesh_path_move_to_queue(gate, from_mpath, copy); + from_mpath = gate; + copy = true; + } else { + RTW_MPATH_DBG( + FUNC_ADPT_FMT" Not forwarding to %pM (flags %#x)\n", + FUNC_ADPT_ARG(adapter), gate->dst, gate->flags); + } + } + + rtw_hlist_for_each_entry_rcu(gate, node, &tbl->known_gates, gate_list) { + RTW_MPATH_DBG(FUNC_ADPT_FMT" Sending to %pM\n", + FUNC_ADPT_ARG(adapter), gate->dst); + rtw_mesh_path_tx_pending(gate); + } + rtw_rcu_read_unlock(); + + return (from_mpath == mpath) ? -EHOSTUNREACH : 0; +} + +/** + * rtw_mesh_path_discard_frame - discard a frame whose path could not be resolved + * + * @skb: frame to discard + * @sdata: network subif the frame was to be sent through + * + * Locking: the function must me called within a rcu_read_lock region + */ +void rtw_mesh_path_discard_frame(_adapter *adapter, + struct xmit_frame *xframe) +{ + rtw_free_xmitframe(&adapter->xmitpriv, xframe); + adapter->mesh_info.mshstats.dropped_frames_no_route++; +} + +/** + * rtw_mesh_path_flush_pending - free the pending queue of a mesh path + * + * @mpath: mesh path whose queue has to be freed + * + * Locking: the function must me called within a rcu_read_lock region + */ +void rtw_mesh_path_flush_pending(struct rtw_mesh_path *mpath) +{ + struct xmit_frame *xframe; + _list *list, *head; + _list tmp; + + _rtw_init_listhead(&tmp); + + enter_critical_bh(&mpath->frame_queue.lock); + rtw_list_splice_init(&mpath->frame_queue.queue, &tmp); + mpath->frame_queue_len = 0; + exit_critical_bh(&mpath->frame_queue.lock); + + head = &tmp; + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + xframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + rtw_list_delete(&xframe->list); + rtw_mesh_path_discard_frame(mpath->adapter, xframe); + } +} + +/** + * rtw_mesh_path_fix_nexthop - force a specific next hop for a mesh path + * + * @mpath: the mesh path to modify + * @next_hop: the next hop to force + * + * Locking: this function must be called holding mpath->state_lock + */ +void rtw_mesh_path_fix_nexthop(struct rtw_mesh_path *mpath, struct sta_info *next_hop) +{ + enter_critical_bh(&mpath->state_lock); + rtw_mesh_path_assign_nexthop(mpath, next_hop); + mpath->sn = 0xffff; + mpath->metric = 0; + mpath->hop_count = 0; + mpath->exp_time = 0; + mpath->flags = RTW_MESH_PATH_FIXED | RTW_MESH_PATH_SN_VALID; + rtw_mesh_path_activate(mpath); + exit_critical_bh(&mpath->state_lock); + rtw_ewma_err_rate_init(&next_hop->metrics.err_rate); + /* init it at a low value - 0 start is tricky */ + rtw_ewma_err_rate_add(&next_hop->metrics.err_rate, 1); + rtw_mesh_path_tx_pending(mpath); +} + +int rtw_mesh_pathtbl_init(_adapter *adapter) +{ + struct rtw_mesh_table *tbl_path, *tbl_mpp; + int ret; + + tbl_path = rtw_mesh_table_alloc(); + if (!tbl_path) + return -ENOMEM; + + tbl_mpp = rtw_mesh_table_alloc(); + if (!tbl_mpp) { + ret = -ENOMEM; + goto free_path; + } + + rtw_rhashtable_init(&tbl_path->rhead, &rtw_mesh_rht_params); + rtw_rhashtable_init(&tbl_mpp->rhead, &rtw_mesh_rht_params); + + adapter->mesh_info.mesh_paths = tbl_path; + adapter->mesh_info.mpp_paths = tbl_mpp; + + return 0; + +free_path: + rtw_mesh_table_free(tbl_path); + return ret; +} + +static +void rtw_mesh_path_tbl_expire(_adapter *adapter, + struct rtw_mesh_table *tbl) +{ + struct rtw_mesh_path *mpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((mpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(mpath) && PTR_ERR(mpath) == -EAGAIN) + continue; + if (IS_ERR(mpath)) + break; + if ((!(mpath->flags & RTW_MESH_PATH_RESOLVING)) && + (!(mpath->flags & RTW_MESH_PATH_FIXED)) && + rtw_time_after(rtw_get_current_time(), mpath->exp_time + RTW_MESH_PATH_EXPIRE)) + __rtw_mesh_path_del(tbl, mpath); + + if (mpath->is_gate && /* need not to deal with non-gate case */ + rtw_time_after(rtw_get_current_time(), mpath->gate_timeout)) { + RTW_MPATH_DBG(FUNC_ADPT_FMT"mpath [%pM] expired systime is %lu systime is %lu\n", + FUNC_ADPT_ARG(adapter), mpath->dst, + mpath->gate_timeout, rtw_get_current_time()); + enter_critical_bh(&mpath->state_lock); + if (mpath->gate_asked) { /* asked gate before */ + rtw_mesh_gate_del(tbl, mpath); + exit_critical_bh(&mpath->state_lock); + } else { + mpath->gate_asked = true; + mpath->gate_timeout = rtw_get_current_time() + rtw_ms_to_systime(mpath->gate_ann_int); + exit_critical_bh(&mpath->state_lock); + rtw_mesh_queue_preq(mpath, RTW_PREQ_Q_F_START | RTW_PREQ_Q_F_REFRESH); + RTW_MPATH_DBG(FUNC_ADPT_FMT"mpath [%pM] ask mesh gate existence (is_root=%d)\n", + FUNC_ADPT_ARG(adapter), mpath->dst, mpath->is_root); + } + } + } + +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +void rtw_mesh_path_expire(_adapter *adapter) +{ + rtw_mesh_path_tbl_expire(adapter, adapter->mesh_info.mesh_paths); + rtw_mesh_path_tbl_expire(adapter, adapter->mesh_info.mpp_paths); +} + +void rtw_mesh_pathtbl_unregister(_adapter *adapter) +{ + if (adapter->mesh_info.mesh_paths) { + rtw_mesh_table_free(adapter->mesh_info.mesh_paths); + adapter->mesh_info.mesh_paths = NULL; + } + + if (adapter->mesh_info.mpp_paths) { + rtw_mesh_table_free(adapter->mesh_info.mpp_paths); + adapter->mesh_info.mpp_paths = NULL; + } +} +#endif /* CONFIG_RTW_MESH */ + diff --git a/core/mesh/rtw_mesh_pathtbl.h b/core/mesh/rtw_mesh_pathtbl.h new file mode 100644 index 0000000..be0c409 --- /dev/null +++ b/core/mesh/rtw_mesh_pathtbl.h @@ -0,0 +1,211 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifndef __RTW_MESH_PATHTBL_H_ +#define __RTW_MESH_PATHTBL_H_ + +#ifndef DBG_RTW_MPATH +#define DBG_RTW_MPATH 1 +#endif +#if DBG_RTW_MPATH +#define RTW_MPATH_DBG(fmt, arg...) RTW_PRINT(fmt, ##arg) +#else +#define RTW_MPATH_DBG(fmt, arg...) do {} while (0) +#endif + +/** + * enum rtw_mesh_path_flags - mesh path flags + * + * @RTW_MESH_PATH_ACTIVE: the mesh path can be used for forwarding + * @RTW_MESH_PATH_RESOLVING: the discovery process is running for this mesh path + * @RTW_MESH_PATH_SN_VALID: the mesh path contains a valid destination sequence + * number + * @RTW_MESH_PATH_FIXED: the mesh path has been manually set and should not be + * modified + * @RTW_MESH_PATH_RESOLVED: the mesh path can has been resolved + * @RTW_MESH_PATH_REQ_QUEUED: there is an unsent path request for this destination + * already queued up, waiting for the discovery process to start. + * @RTW_MESH_PATH_DELETED: the mesh path has been deleted and should no longer + * be used + * @RTW_MESH_PATH_ROOT_ADD_CHK: root additional check in root mode. + * With this flag, It will try the last used rann_snd_addr + * @RTW_MESH_PATH_PEER_AKA: only used toward a peer, only used in active keep + * alive mechanism. PREQ's da = path dst + * @RTW_MESH_PATH_BCAST_PREQ: for re-checking next hop resolve toward root. + * Use it to force path_discover sending broadcast PREQ for root. + * + * RTW_MESH_PATH_RESOLVED is used by the mesh path timer to + * decide when to stop or cancel the mesh path discovery. + */ +enum rtw_mesh_path_flags { + RTW_MESH_PATH_ACTIVE = BIT(0), + RTW_MESH_PATH_RESOLVING = BIT(1), + RTW_MESH_PATH_SN_VALID = BIT(2), + RTW_MESH_PATH_FIXED = BIT(3), + RTW_MESH_PATH_RESOLVED = BIT(4), + RTW_MESH_PATH_REQ_QUEUED = BIT(5), + RTW_MESH_PATH_DELETED = BIT(6), + RTW_MESH_PATH_ROOT_ADD_CHK = BIT(7), + RTW_MESH_PATH_PEER_AKA = BIT(8), + RTW_MESH_PATH_BCAST_PREQ = BIT(9), +}; + +/** + * struct rtw_mesh_path - mesh path structure + * + * @dst: mesh path destination mac address + * @mpp: mesh proxy mac address + * @rhash: rhashtable list pointer + * @gate_list: list pointer for known gates list + * @sdata: mesh subif + * @next_hop: mesh neighbor to which frames for this destination will be + * forwarded + * @timer: mesh path discovery timer + * @frame_queue: pending queue for frames sent to this destination while the + * path is unresolved + * @rcu: rcu head for freeing mesh path + * @sn: target sequence number + * @metric: current metric to this destination + * @hop_count: hops to destination + * @exp_time: in jiffies, when the path will expire or when it expired + * @discovery_timeout: timeout (lapse in jiffies) used for the last discovery + * retry + * @discovery_retries: number of discovery retries + * @flags: mesh path flags, as specified on &enum rtw_mesh_path_flags + * @state_lock: mesh path state lock used to protect changes to the + * mpath itself. No need to take this lock when adding or removing + * an mpath to a hash bucket on a path table. + * @rann_snd_addr: the RANN sender address + * @rann_metric: the aggregated path metric towards the root node + * @last_preq_to_root: Timestamp of last PREQ sent to root + * @is_root: the destination station of this path is a root node + * @is_gate: the destination station of this path is a mesh gate + * + * + * The dst address is unique in the mesh path table. Since the mesh_path is + * protected by RCU, deleting the next_hop STA must remove / substitute the + * mesh_path structure and wait until that is no longer reachable before + * destroying the STA completely. + */ +struct rtw_mesh_path { + u8 dst[ETH_ALEN]; + u8 mpp[ETH_ALEN]; /* used for MPP or MAP */ + rtw_rhash_head rhash; + rtw_hlist_node gate_list; + _adapter *adapter; + struct sta_info __rcu *next_hop; + _timer timer; + _queue frame_queue; + u32 frame_queue_len; + rtw_rcu_head rcu; + u32 sn; + u32 metric; + u8 hop_count; + systime exp_time; + systime discovery_timeout; + systime gate_timeout; + u32 gate_ann_int; /* gate announce interval */ + u8 discovery_retries; + enum rtw_mesh_path_flags flags; + _lock state_lock; + u8 rann_snd_addr[ETH_ALEN]; +#ifdef CONFIG_RTW_MESH_ADD_ROOT_CHK + u8 add_chk_rann_snd_addr[ETH_ALEN]; +#endif + u32 rann_metric; + unsigned long last_preq_to_root; + bool is_root; + bool is_gate; + bool gate_asked; +}; + +/** + * struct rtw_mesh_table + * + * @known_gates: list of known mesh gates and their mpaths by the station. The + * gate's mpath may or may not be resolved and active. + * @gates_lock: protects updates to known_gates + * @rhead: the rhashtable containing struct mesh_paths, keyed by dest addr + * @entries: number of entries in the table + */ +struct rtw_mesh_table { + rtw_hlist_head known_gates; + _lock gates_lock; + rtw_rhashtable rhead; + ATOMIC_T entries; +}; + +#define RTW_MESH_PATH_EXPIRE (600 * HZ) + +/* Maximum number of paths per interface */ +#define RTW_MESH_MAX_MPATHS 1024 + +/* Number of frames buffered per destination for unresolved destinations */ +#define RTW_MESH_FRAME_QUEUE_LEN 10 + +int rtw_mesh_nexthop_lookup(_adapter *adapter, + const u8 *mda, const u8 *msa, u8 *ra); +int rtw_mesh_nexthop_resolve(_adapter *adapter, + struct xmit_frame *xframe); + +struct rtw_mesh_path *rtw_mesh_path_lookup(_adapter *adapter, + const u8 *dst); +struct rtw_mesh_path *rtw_mpp_path_lookup(_adapter *adapter, + const u8 *dst); +int rtw_mpp_path_add(_adapter *adapter, + const u8 *dst, const u8 *mpp); +void dump_mpp(void *sel, _adapter *adapter); + +struct rtw_mesh_path * +rtw_mesh_path_lookup_by_idx(_adapter *adapter, int idx); +void dump_mpath(void *sel, _adapter *adapter); + +struct rtw_mesh_path * +rtw_mpp_path_lookup_by_idx(_adapter *adapter, int idx); +void rtw_mesh_path_fix_nexthop(struct rtw_mesh_path *mpath, struct sta_info *next_hop); +void rtw_mesh_path_expire(_adapter *adapter); + +struct rtw_mesh_path * +rtw_mesh_path_add(_adapter *adapter, const u8 *dst); + +int rtw_mesh_path_add_gate(struct rtw_mesh_path *mpath); +void rtw_mesh_gate_del(struct rtw_mesh_table *tbl, struct rtw_mesh_path *mpath); +bool rtw_mesh_gate_search(struct rtw_mesh_table *tbl, const u8 *addr); +int rtw_mesh_path_send_to_gates(struct rtw_mesh_path *mpath); +int rtw_mesh_gate_num(_adapter *adapter); +bool rtw_mesh_is_primary_gate(_adapter *adapter); +void dump_known_gates(void *sel, _adapter *adapter); + +void rtw_mesh_plink_broken(struct sta_info *sta); + +void rtw_mesh_path_assign_nexthop(struct rtw_mesh_path *mpath, struct sta_info *sta); +void rtw_mesh_path_flush_pending(struct rtw_mesh_path *mpath); +void rtw_mesh_path_tx_pending(struct rtw_mesh_path *mpath); +int rtw_mesh_pathtbl_init(_adapter *adapter); +void rtw_mesh_pathtbl_unregister(_adapter *adapter); +int rtw_mesh_path_del(_adapter *adapter, const u8 *addr); + +void rtw_mesh_path_flush_by_nexthop(struct sta_info *sta); +void rtw_mesh_path_discard_frame(_adapter *adapter, + struct xmit_frame *xframe); + +static inline void rtw_mesh_path_activate(struct rtw_mesh_path *mpath) +{ + mpath->flags |= RTW_MESH_PATH_ACTIVE | RTW_MESH_PATH_RESOLVED; +} + +void rtw_mesh_path_flush_by_iface(_adapter *adapter); + +#endif /* __RTW_MESH_PATHTBL_H_ */ + diff --git a/core/monitor/rtw_radiotap.c b/core/monitor/rtw_radiotap.c new file mode 100644 index 0000000..1f9fc49 --- /dev/null +++ b/core/monitor/rtw_radiotap.c @@ -0,0 +1,617 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_RADIOTAP_C_ + +#ifdef CONFIG_WIFI_MONITOR + +#include +#include + +#define CHAN2FREQ(a) ((a < 14) ? (2407+5*a) : (5000+5*a)) + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 20, 0)) +#define IEEE80211_RADIOTAP_ZERO_LEN_PSDU 26 +#define IEEE80211_RADIOTAP_LSIG 27 +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 9, 0)) +#define IEEE80211_RADIOTAP_TIMESTAMP 22 +/* For IEEE80211_RADIOTAP_TIMESTAMP */ +#define IEEE80211_RADIOTAP_TIMESTAMP_UNIT_MASK 0x000F +#define IEEE80211_RADIOTAP_TIMESTAMP_UNIT_MS 0x0000 +#define IEEE80211_RADIOTAP_TIMESTAMP_UNIT_US 0x0001 +#define IEEE80211_RADIOTAP_TIMESTAMP_UNIT_NS 0x0003 +#define IEEE80211_RADIOTAP_TIMESTAMP_SPOS_MASK 0x00F0 +#define IEEE80211_RADIOTAP_TIMESTAMP_SPOS_BEGIN_MDPU 0x0000 +#define IEEE80211_RADIOTAP_TIMESTAMP_SPOS_EO_MPDU 0x0010 +#define IEEE80211_RADIOTAP_TIMESTAMP_SPOS_EO_PPDU 0x0020 +#define IEEE80211_RADIOTAP_TIMESTAMP_SPOS_PLCP_SIG_ACQ 0x0030 +#define IEEE80211_RADIOTAP_TIMESTAMP_SPOS_UNKNOWN 0x00F0 + +#define IEEE80211_RADIOTAP_TIMESTAMP_FLAG_64BIT 0x00 +#define IEEE80211_RADIOTAP_TIMESTAMP_FLAG_32BIT 0x01 +#define IEEE80211_RADIOTAP_TIMESTAMP_FLAG_ACCURACY 0x02 +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 12, 0)) +/* for IEEE80211_RADIOTAP_CHANNEL */ +#define IEEE80211_CHAN_GSM 0x1000 /* GSM (900 MHz) */ +#define IEEE80211_CHAN_STURBO 0x2000 /* Static Turbo */ +#define IEEE80211_CHAN_HALF 0x4000 /* Half channel (10 MHz wide) */ +#define IEEE80211_CHAN_QUARTER 0x8000 /* Quarter channel (5 MHz wide) */ +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 8, 0)) +#define IEEE80211_RADIOTAP_VHT 21 +/* For IEEE80211_RADIOTAP_VHT */ +#define IEEE80211_RADIOTAP_VHT_KNOWN_STBC 0x0001 +#define IEEE80211_RADIOTAP_VHT_KNOWN_TXOP_PS_NA 0x0002 +#define IEEE80211_RADIOTAP_VHT_KNOWN_GI 0x0004 +#define IEEE80211_RADIOTAP_VHT_KNOWN_SGI_NSYM_DIS 0x0008 +#define IEEE80211_RADIOTAP_VHT_KNOWN_LDPC_EXTRA_OFDM_SYM 0x0010 +#define IEEE80211_RADIOTAP_VHT_KNOWN_BEAMFORMED 0x0020 +#define IEEE80211_RADIOTAP_VHT_KNOWN_BANDWIDTH 0x0040 +#define IEEE80211_RADIOTAP_VHT_KNOWN_GROUP_ID 0x0080 +#define IEEE80211_RADIOTAP_VHT_KNOWN_PARTIAL_AID 0x0100 + +#define IEEE80211_RADIOTAP_VHT_FLAG_STBC 0x01 +#define IEEE80211_RADIOTAP_VHT_FLAG_TXOP_PS_NA 0x02 +#define IEEE80211_RADIOTAP_VHT_FLAG_SGI 0x04 +#define IEEE80211_RADIOTAP_VHT_FLAG_SGI_NSYM_M10_9 0x08 +#define IEEE80211_RADIOTAP_VHT_FLAG_LDPC_EXTRA_OFDM_SYM 0x10 +#define IEEE80211_RADIOTAP_VHT_FLAG_BEAMFORMED 0x20 +#elif (LINUX_VERSION_CODE < KERNEL_VERSION(3, 15, 0)) +#define IEEE80211_RADIOTAP_CODING_LDPC_USER0 0x01 +#define IEEE80211_RADIOTAP_CODING_LDPC_USER1 0x02 +#define IEEE80211_RADIOTAP_CODING_LDPC_USER2 0x04 +#define IEEE80211_RADIOTAP_CODING_LDPC_USER3 0x08 +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 7, 0)) +#define IEEE80211_RADIOTAP_AMPDU_STATUS 20 +/* For IEEE80211_RADIOTAP_AMPDU_STATUS */ +#define IEEE80211_RADIOTAP_AMPDU_REPORT_ZEROLEN 0x0001 +#define IEEE80211_RADIOTAP_AMPDU_IS_ZEROLEN 0x0002 +#define IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN 0x0004 +#define IEEE80211_RADIOTAP_AMPDU_IS_LAST 0x0008 +#define IEEE80211_RADIOTAP_AMPDU_DELIM_CRC_ERR 0x0010 +#define IEEE80211_RADIOTAP_AMPDU_DELIM_CRC_KNOWN 0x0020 +#elif (LINUX_VERSION_CODE < KERNEL_VERSION(4, 17, 0)) +#define IEEE80211_RADIOTAP_AMPDU_EOF 0x0040 +#define IEEE80211_RADIOTAP_AMPDU_EOF_KNOWN 0x0080 +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 39)) +#define IEEE80211_RADIOTAP_MCS 19 +/* For IEEE80211_RADIOTAP_MCS */ +#define IEEE80211_RADIOTAP_MCS_HAVE_BW 0x01 +#define IEEE80211_RADIOTAP_MCS_HAVE_MCS 0x02 +#define IEEE80211_RADIOTAP_MCS_HAVE_GI 0x04 +#define IEEE80211_RADIOTAP_MCS_HAVE_FMT 0x08 +#define IEEE80211_RADIOTAP_MCS_HAVE_FEC 0x10 + +#define IEEE80211_RADIOTAP_MCS_BW_MASK 0x03 +#define IEEE80211_RADIOTAP_MCS_BW_20 0 +#define IEEE80211_RADIOTAP_MCS_BW_40 1 +#define IEEE80211_RADIOTAP_MCS_BW_20L 2 +#define IEEE80211_RADIOTAP_MCS_BW_20U 3 +#define IEEE80211_RADIOTAP_MCS_SGI 0x04 +#define IEEE80211_RADIOTAP_MCS_FMT_GF 0x08 +#define IEEE80211_RADIOTAP_MCS_FEC_LDPC 0x10 +#elif (LINUX_VERSION_CODE < KERNEL_VERSION(3, 11, 0)) +#define IEEE80211_RADIOTAP_MCS_HAVE_STBC 0x20 + +#define IEEE80211_RADIOTAP_MCS_STBC_MASK 0x60 +#define IEEE80211_RADIOTAP_MCS_STBC_1 1 +#define IEEE80211_RADIOTAP_MCS_STBC_2 2 +#define IEEE80211_RADIOTAP_MCS_STBC_3 3 +#define IEEE80211_RADIOTAP_MCS_STBC_SHIFT 5 +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 34)) +#define IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE 29 +#define IEEE80211_RADIOTAP_VENDOR_NAMESPACE 30 +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 30)) +#define IEEE80211_RADIOTAP_F_BADFCS 0x40 +#endif + +static inline void _rtw_radiotap_fill_flags(struct rx_pkt_attrib *a, u8 *flags) +{ + struct moinfo *moif = (struct moinfo *)&a->moif; + + if (0) + *flags |= IEEE80211_RADIOTAP_F_CFP; + + if (0) + *flags |= IEEE80211_RADIOTAP_F_SHORTPRE; + + if ((a->encrypt == 1) || (a->encrypt == 5)) + *flags |= IEEE80211_RADIOTAP_F_WEP; + + if (a->mfrag) + *flags |= IEEE80211_RADIOTAP_F_FRAG; + + if (1) + *flags |= IEEE80211_RADIOTAP_F_FCS; + + if (0) + *flags |= IEEE80211_RADIOTAP_F_DATAPAD; + + if (a->crc_err) + *flags |= IEEE80211_RADIOTAP_F_BADFCS; + + /* Currently unspecified but used + for short guard interval (HT) */ + if (moif->u.snif_info.sgi || a->sgi) + *flags |= 0x80; + +} + +sint rtw_fill_radiotap_hdr(_adapter *padapter, struct rx_pkt_attrib *a, u8 *buf) +{ +#define RTAP_HDR_MAX 64 + + sint ret = _SUCCESS; + struct moinfo *moif = (struct moinfo *)&a->moif; + + u8 rx_cnt = 0; + + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + int i = 0; + u8 tmp_8bit = 0; + u16 tmp_16bit = 0; + u32 tmp_32bit = 0; + u64 tmp_64bit = 0; + + _pkt *pskb = NULL; + + struct ieee80211_radiotap_header *rtap_hdr = NULL; + u8 *ptr = NULL; + + /* + radiotap length (include header 8) + 11G length: 36 (0x0040002f) + 11N length: + 11AC length: 60 (0x0070002b) + */ + u8 hdr_buf[RTAP_HDR_MAX] = { 0 }; + u16 rt_len = 8; + + /* create header */ + rtap_hdr = (struct ieee80211_radiotap_header *)&hdr_buf[0]; + rtap_hdr->it_version = PKTHDR_RADIOTAP_VERSION; + + /* each antenna information */ + rx_cnt = rf_type_to_rf_rx_cnt(pHalData->rf_type); +#if 0 + if (rx_cnt > 1) { + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE) | + BIT(IEEE80211_RADIOTAP_EXT); + + for (i = 1; i < rx_cnt; i++) { + tmp_32bit = (BIT(IEEE80211_RADIOTAP_DBM_ANTSIGNAL) | + BIT(IEEE80211_RADIOTAP_LOCK_QUALITY) | + BIT(IEEE80211_RADIOTAP_ANTENNA) | + BIT(IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE) | + BIT(IEEE80211_RADIOTAP_EXT)); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_32bit, 4); + rt_len += 4; + } + + tmp_32bit = (BIT(IEEE80211_RADIOTAP_DBM_ANTSIGNAL) | + BIT(IEEE80211_RADIOTAP_LOCK_QUALITY) | + BIT(IEEE80211_RADIOTAP_ANTENNA)); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_32bit, 4); + rt_len += 4; + } +#endif + + /* tsft, Required Alignment: 8 bytes */ + if (0) { //(a->free_cnt) { + /* TSFT + free_cnt */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_TSFT); + if (!IS_ALIGNED(rt_len, 8)) + rt_len = ((rt_len + 7) & 0xFFF8); /* Alignment */ + + tmp_64bit = cpu_to_le64(a->free_cnt); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_64bit, 8); + rt_len += 8; + } + + /* flags */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_FLAGS); + _rtw_radiotap_fill_flags(a, &hdr_buf[rt_len]); + rt_len += 1; + + /* rate */ + if (a->data_rate <= DESC_RATE54M) { + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_RATE); + hdr_buf[rt_len] = hw_rate_to_m_rate(a->data_rate); + rt_len += 1; + } + + /* channel & flags, Required Alignment: 2 bytes */ + if (1) { + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_CHANNEL); + rt_len += (rt_len % 2); /* Alignment */ + + tmp_16bit = CHAN2FREQ(rtw_get_oper_ch(padapter)); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + + /* channel flags */ + tmp_16bit = 0; + if (pHalData->current_band_type == 0) + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_2GHZ); + else + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_5GHZ); + + if (a->data_rate <= DESC_RATE11M) { + /* CCK */ + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_CCK); + } else { + /* OFDM */ + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_OFDM); + } + + if (rtw_get_oper_bw(padapter) == CHANNEL_WIDTH_10) { + /* 10Mhz Channel Width */ + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_HALF); + } + + if (rtw_get_oper_bw(padapter) == CHANNEL_WIDTH_5) { + /* 5Mhz Channel Width */ + tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_QUARTER); + } + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + } + + /* dBm Antenna Signal */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_DBM_ANTSIGNAL); + hdr_buf[rt_len] = a->phy_info.recv_signal_power; + rt_len += 1; + +#if 0 + /* dBm Antenna Noise */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_DBM_ANTNOISE); + hdr_buf[rt_len] = 0; + rt_len += 1; +#endif +#if 0 + /* Signal Quality, Required Alignment: 2 bytes */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_LOCK_QUALITY); + if (!IS_ALIGNED(rt_len, 2)) + rt_len++; + hdr_buf[rt_len] = a->phy_info.signal_quality; + rt_len += 2; + +#endif + +#if 0 + /* Antenna */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_ANTENNA); + hdr_buf[rt_len] = 0; /* pHalData->rf_type; */ + rt_len += 1; +#endif +#if 0 + /* RX flags, Required Alignment: 2 bytes */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_RX_FLAGS); + tmp_16bit = 0; + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; +#endif + + /* MCS information, Required Alignment: 1 bytes */ + if (a->data_rate >= DESC_RATEMCS0 && a->data_rate <= DESC_RATEMCS31) { + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_MCS); + /* Structure u8 known, u8 flags, u8 mcs */ + + /* known.bandwidth */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_MCS_HAVE_BW; + if (moif->u.snif_info.ofdm_bw) + hdr_buf[rt_len + 1] |= IEEE80211_RADIOTAP_MCS_BW_40; + if (a->bw == CHANNEL_WIDTH_40) + hdr_buf[rt_len + 1] |= IEEE80211_RADIOTAP_MCS_BW_40; + else + hdr_buf[rt_len + 1] |= IEEE80211_RADIOTAP_MCS_BW_20; + + + /* known.guard interval */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_MCS_HAVE_GI; + if (moif->u.snif_info.sgi) { + hdr_buf[rt_len + 1] |= IEEE80211_RADIOTAP_MCS_SGI; + } else { + hdr_buf[rt_len + 1] |= ((a->sgi & 0x01) << 2); + } + + /* FEC Type */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_MCS_HAVE_FEC; + if (moif->u.snif_info.ldpc) { + hdr_buf[rt_len + 1] |= ((moif->u.snif_info.ldpc & 0x01) << 4); + } else { + hdr_buf[rt_len + 1] |= ((a->ldpc & 0x01) << 4); + } + + /* STBC */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_MCS_HAVE_STBC; + if (moif->u.snif_info.stbc) { + hdr_buf[rt_len + 1] |= ((moif->u.snif_info.stbc & 0x03) << 5); + } else { + hdr_buf[rt_len + 1] |= ((a->stbc & 0x03) << 5); + } + + /* known.MCS index */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_MCS_HAVE_MCS; + + /* u8 mcs */ + hdr_buf[rt_len + 2] = a->data_rate - DESC_RATEMCS0; + + rt_len += 3; + } + + /* AMPDU, Required Alignment: 4 bytes */ + if (a->ampdu) { + static u32 ref_num = 0x10000000; + static u8 ppdu_cnt = 0; + + /* Structure u32 reference number, u16 flags, u8 delimiter CRC value, u8 reserved */ + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_AMPDU_STATUS); + if (!IS_ALIGNED(rt_len, 4)) + rt_len = ((rt_len + 3) & 0xFFFC); /* Alignment */ + + /* u32 reference number */ + if (a->ppdu_cnt != ppdu_cnt) { + ppdu_cnt = a->ppdu_cnt; + ref_num += 1; + } + tmp_32bit = cpu_to_le32(ref_num); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_32bit, 4); + rt_len += 4; + + /* u16 flags */ + tmp_16bit = 0; + if (0) { + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_REPORT_ZEROLEN); + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_IS_ZEROLEN); + } + + if (0) { + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_IS_LAST); + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN); + } + + if (0) { + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_DELIM_CRC_ERR); + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_DELIM_CRC_KNOWN); + } + + if (a->ampdu_eof) { +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 8, 0)) + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_EOF_KNOWN); + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_AMPDU_EOF); +#endif + } + + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + + /* u8 delimiter CRC value, u8 reserved */ + rt_len += 2; + } + + /* VHT, Required Alignment: 2 bytes */ + if (a->data_rate >= DESC_RATEVHTSS1MCS0 && a->data_rate <= DESC_RATEVHTSS4MCS9) { + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_VHT); + + rt_len += (rt_len % 2); /* Alignment */ + + /* Structure + u16 known, u8 flags, u8 bandwidth, u8 mcs_nss[4], + u8 coding, u8 group_id, u16 partial_aid */ + + tmp_16bit = 0; + + /* STBC */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_STBC); + if (moif->u.snif_info.stbc) { + hdr_buf[rt_len + 2] |= IEEE80211_RADIOTAP_VHT_FLAG_STBC; + } else { + hdr_buf[rt_len + 2] |= (a->stbc & 0x01); + } + + /* TXOP_PS_NOT_ALLOWED */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_TXOP_PS_NA); + if (moif->u.snif_info.vht_txop_not_allow) { + hdr_buf[rt_len + 2] |= IEEE80211_RADIOTAP_VHT_FLAG_TXOP_PS_NA; + } + + + /* Guard interval */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_GI); + if (moif->u.snif_info.sgi) { + hdr_buf[rt_len + 2] |= IEEE80211_RADIOTAP_VHT_FLAG_SGI; + } else { + hdr_buf[rt_len + 2] |= ((a->sgi & 0x01) << 2); + } + + /* Short GI NSYM */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_SGI_NSYM_DIS); + if (moif->u.snif_info.vht_nsym_dis) { + hdr_buf[rt_len + 2] |= IEEE80211_RADIOTAP_VHT_FLAG_SGI_NSYM_M10_9; + } + + /* LDPC extra OFDM symbol */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_LDPC_EXTRA_OFDM_SYM); + if (moif->u.snif_info.vht_ldpc_extra) { + hdr_buf[rt_len + 2] |= IEEE80211_RADIOTAP_VHT_FLAG_LDPC_EXTRA_OFDM_SYM; + } else { + hdr_buf[rt_len + 2] |= ((a->ldpc & 0x01) << 4); + } + + /* Short GI NSYM */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_BEAMFORMED); + if (moif->u.snif_info.vht_beamformed) { + hdr_buf[rt_len + 2] |= IEEE80211_RADIOTAP_VHT_FLAG_BEAMFORMED; + } + + /* know.Bandwidth */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_BANDWIDTH); + + /* Group ID */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_GROUP_ID); + + /* Partial AID */ + tmp_16bit |= cpu_to_le16(IEEE80211_RADIOTAP_VHT_KNOWN_PARTIAL_AID); + + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 3; + + /* u8 bandwidth */ + if (moif->u.snif_info.ofdm_bw) + tmp_8bit = moif->u.snif_info.ofdm_bw; + else + tmp_8bit = a->bw; + + switch (tmp_8bit) { + case CHANNEL_WIDTH_20: + hdr_buf[rt_len] |= 0; + break; + case CHANNEL_WIDTH_40: + hdr_buf[rt_len] |= 1; + break; + case CHANNEL_WIDTH_80: + hdr_buf[rt_len] |= 4; + break; + case CHANNEL_WIDTH_160: + hdr_buf[rt_len] |= 11; + break; + default: + hdr_buf[rt_len] |= 0; + } + rt_len += 1; + + /* u8 mcs_nss[4] */ + if ((DESC_RATEVHTSS1MCS0 <= a->data_rate) && + (a->data_rate <= DESC_RATEVHTSS4MCS9)) { + /* User 0 */ + /* MCS */ + hdr_buf[rt_len] = ((a->data_rate - DESC_RATEVHTSS1MCS0) % 10) << 4; + /* NSS */ + hdr_buf[rt_len] |= (((a->data_rate - DESC_RATEVHTSS1MCS0) / 10) + 1); + } + rt_len += 4; + + /* u8 coding, phystat? */ + hdr_buf[rt_len] = 0; + rt_len += 1; + + /* u8 group_id */ + hdr_buf[rt_len] = moif->u.snif_info.vht_group_id; + rt_len += 1; + + /* u16 partial_aid */ + tmp_16bit = cpu_to_le16(moif->u.snif_info.vht_nsts_aid); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + } + + /* frame timestamp, Required Alignment: 8 bytes */ + if (0) { //(a->free_cnt) { + rtap_hdr->it_present |= BIT(IEEE80211_RADIOTAP_TIMESTAMP); + if (!IS_ALIGNED(rt_len, 8)) + rt_len = ((rt_len + 7) & 0xFFF8); /* Alignment */ + + /* u64 timestamp */ + tmp_64bit = cpu_to_le64(a->free_cnt); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_64bit, 8); + rt_len += 8; + + /* u16 accuracy */ + tmp_16bit = cpu_to_le16(22); + _rtw_memcpy(&hdr_buf[rt_len], &tmp_16bit, 2); + rt_len += 2; + + /* u8 unit/position */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_TIMESTAMP_UNIT_US; + rt_len += 1; + + /* u8 flags */ + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_TIMESTAMP_FLAG_32BIT; + hdr_buf[rt_len] |= IEEE80211_RADIOTAP_TIMESTAMP_FLAG_ACCURACY; + rt_len += 1; + } + + /* each antenna information */ +#if 0 + if (rx_cnt > 1) { + for (i = 0; i <= rx_cnt; i++) { + /* dBm Antenna Signal */ + hdr_buf[rt_len] = a->phy_info.rx_mimo_signal_strength[i]; + rt_len += 1; + + /* Signal Quality */ + if (!IS_ALIGNED(rt_len, 2)) + rt_len++; + hdr_buf[rt_len] = cpu_to_le16(a->phy_info.rx_mimo_signal_quality[i]); + rt_len += 2; + + /* Antenna */ + hdr_buf[rt_len] = i; /* pHalData->rf_type; */ + rt_len += 1; + } + } +#endif + + /* push to skb */ + pskb = (_pkt *)buf; + if (skb_headroom(pskb) < rt_len) { + RTW_INFO("%s:%d %s headroom is too small.\n", __FILE__, __LINE__, __func__); + ret = _FAIL; + return ret; + } + + ptr = skb_push(pskb, rt_len); + if (ptr) { + rtap_hdr->it_len = cpu_to_le16(rt_len); + rtap_hdr->it_present = cpu_to_le32(rtap_hdr->it_present); + memcpy(ptr, rtap_hdr, rt_len); + } else + ret = _FAIL; + + return ret; + +} + +void rx_query_moinfo(struct rx_pkt_attrib *a, u8 *desc) +{ + switch (a->drvinfo_sz) { + case 40: + _rtw_memcpy(a->moif, &desc[32], 8); + break; + case 48: + _rtw_memcpy(a->moif, &desc[32], 12); + break; + case 32: + /* passthrough */ + default: + break; + } +} + +#endif /* CONFIG_WIFI_MONITOR */ diff --git a/core/monitor/rtw_radiotap.h b/core/monitor/rtw_radiotap.h new file mode 100644 index 0000000..affacd1 --- /dev/null +++ b/core/monitor/rtw_radiotap.h @@ -0,0 +1,63 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifndef __RTW_RADIOTAP_H_ +#define __RTW_RADIOTAP_H_ + +struct mon_reg_backup { + /* flags */ + u8 known_rcr:1; + u8 known_drvinfo:1; + u8 known_rxfilter:1; + u8 known_misc0:1; + /* data */ + u8 drvinfo; + u16 rxfilter0; + u16 rxfilter1; + u16 rxfilter2; + u32 rcr; + u32 misc0; +}; + +struct moinfo { + union { + struct { + u32 sgi:1; + u32 ldpc:1; + u32 stbc:2; + u32 not_sounding:1; + u32 ofdm_bw:2; + u32 vht_group_id:2; + u32 vht_nsts_aid:12; + u32 vht_txop_not_allow:1; + u32 vht_nsym_dis:1; + u32 vht_ldpc_extra:1; + u32 vht_su_mcs:12; + u32 vht_beamformed:1; + }snif_info; + + struct { + u32 A; + u32 B; + u32 C; + }plcp_info; + }u; +}; + +sint rtw_fill_radiotap_hdr(_adapter *padapter, struct rx_pkt_attrib *a, u8 *buf); + +void rx_query_moinfo(struct rx_pkt_attrib *a, u8 *desc); + +#endif /* __RTW_RADIOTAP_H_ */ + diff --git a/core/rtw_ap.c b/core/rtw_ap.c new file mode 100644 index 0000000..ec72564 --- /dev/null +++ b/core/rtw_ap.c @@ -0,0 +1,5998 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_AP_C_ + +#include +#include + +#ifdef CONFIG_AP_MODE + +extern unsigned char RTW_WPA_OUI[]; +extern unsigned char WMM_OUI[]; +extern unsigned char WPS_OUI[]; +extern unsigned char P2P_OUI[]; +extern unsigned char WFD_OUI[]; + +void init_mlme_ap_info(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + _rtw_spinlock_init(&pmlmepriv->bcn_update_lock); + /* pmlmeext->bstart_bss = _FALSE; */ +} + +void free_mlme_ap_info(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + stop_ap_mode(padapter); + _rtw_spinlock_free(&pmlmepriv->bcn_update_lock); + +} + +/* +* Set TIM IE +* return length of total TIM IE +*/ +u8 rtw_set_tim_ie(u8 dtim_cnt, u8 dtim_period + , const u8 *tim_bmp, u8 tim_bmp_len, u8 *tim_ie) +{ + u8 *p = tim_ie; + u8 i, n1, n2; + u8 bmp_len; + + if (rtw_bmp_not_empty(tim_bmp, tim_bmp_len)) { + /* find the first nonzero octet in tim_bitmap */ + for (i = 0; i < tim_bmp_len; i++) + if (tim_bmp[i]) + break; + n1 = i & 0xFE; + + /* find the last nonzero octet in tim_bitmap, except octet 0 */ + for (i = tim_bmp_len - 1; i > 0; i--) + if (tim_bmp[i]) + break; + n2 = i; + bmp_len = n2 - n1 + 1; + } else { + n1 = n2 = 0; + bmp_len = 1; + } + + *p++ = WLAN_EID_TIM; + *p++ = 2 + 1 + bmp_len; + *p++ = dtim_cnt; + *p++ = dtim_period; + *p++ = (rtw_bmp_is_set(tim_bmp, tim_bmp_len, 0) ? BIT0 : 0) | n1; + _rtw_memcpy(p, tim_bmp + n1, bmp_len); + +#if 0 + RTW_INFO("n1:%u, n2:%u, bmp_offset:%u, bmp_len:%u\n", n1, n2, n1 / 2, bmp_len); + RTW_INFO_DUMP("tim_ie: ", tim_ie + 2, 2 + 1 + bmp_len); +#endif + return 2 + 2 + 1 + bmp_len; +} + +static void update_BCNTIM(_adapter *padapter) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork_mlmeext = &(pmlmeinfo->network); + unsigned char *pie = pnetwork_mlmeext->IEs; + +#if 0 + + + /* update TIM IE */ + /* if(rtw_tim_map_anyone_be_set(padapter, pstapriv->tim_bitmap)) */ +#endif + if (_TRUE) { + u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL; + uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen; + + p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_); + if (p != NULL && tim_ielen > 0) { + tim_ielen += 2; + + premainder_ie = p + tim_ielen; + + tim_ie_offset = (sint)(p - pie); + + remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen; + + /*append TIM IE from dst_ie offset*/ + dst_ie = p; + } else { + tim_ielen = 0; + + /*calculate head_len*/ + offset = _FIXED_IE_LENGTH_; + + /* get ssid_ie len */ + p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SSID_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_)); + if (p != NULL) + offset += tmp_len + 2; + + /*get supported rates len*/ + p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_)); + if (p != NULL) + offset += tmp_len + 2; + + /*DS Parameter Set IE, len=3*/ + offset += 3; + + premainder_ie = pie + offset; + + remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen; + + /*append TIM IE from offset*/ + dst_ie = pie + offset; + + } + + if (remainder_ielen > 0) { + pbackup_remainder_ie = rtw_malloc(remainder_ielen); + if (pbackup_remainder_ie && premainder_ie) + _rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen); + } + + /* append TIM IE */ + dst_ie += rtw_set_tim_ie(0, 1, pstapriv->tim_bitmap, pstapriv->aid_bmp_len, dst_ie); + + /*copy remainder IE*/ + if (pbackup_remainder_ie) { + _rtw_memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen); + + rtw_mfree(pbackup_remainder_ie, remainder_ielen); + } + + offset = (uint)(dst_ie - pie); + pnetwork_mlmeext->IELength = offset + remainder_ielen; + + } +} + +void rtw_add_bcn_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 index, u8 *data, u8 len) +{ + PNDIS_802_11_VARIABLE_IEs pIE; + u8 bmatch = _FALSE; + u8 *pie = pnetwork->IEs; + u8 *p = NULL, *dst_ie = NULL, *premainder_ie = NULL, *pbackup_remainder_ie = NULL; + u32 i, offset, ielen = 0, ie_offset, remainder_ielen = 0; + + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pnetwork->IELength;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i); + + if (pIE->ElementID > index) + break; + else if (pIE->ElementID == index) { /* already exist the same IE */ + p = (u8 *)pIE; + ielen = pIE->Length; + bmatch = _TRUE; + break; + } + + p = (u8 *)pIE; + ielen = pIE->Length; + i += (pIE->Length + 2); + } + + if (p != NULL && ielen > 0) { + ielen += 2; + + premainder_ie = p + ielen; + + ie_offset = (sint)(p - pie); + + remainder_ielen = pnetwork->IELength - ie_offset - ielen; + + if (bmatch) + dst_ie = p; + else + dst_ie = (p + ielen); + } + + if (dst_ie == NULL) + return; + + if (remainder_ielen > 0) { + pbackup_remainder_ie = rtw_malloc(remainder_ielen); + if (pbackup_remainder_ie && premainder_ie) + _rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen); + } + + *dst_ie++ = index; + *dst_ie++ = len; + + _rtw_memcpy(dst_ie, data, len); + dst_ie += len; + + /* copy remainder IE */ + if (pbackup_remainder_ie) { + _rtw_memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen); + + rtw_mfree(pbackup_remainder_ie, remainder_ielen); + } + + offset = (uint)(dst_ie - pie); + pnetwork->IELength = offset + remainder_ielen; +} + +void rtw_remove_bcn_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 index) +{ + u8 *p, *dst_ie = NULL, *premainder_ie = NULL, *pbackup_remainder_ie = NULL; + uint offset, ielen, ie_offset, remainder_ielen = 0; + u8 *pie = pnetwork->IEs; + + p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, index, &ielen, pnetwork->IELength - _FIXED_IE_LENGTH_); + if (p != NULL && ielen > 0) { + ielen += 2; + + premainder_ie = p + ielen; + + ie_offset = (sint)(p - pie); + + remainder_ielen = pnetwork->IELength - ie_offset - ielen; + + dst_ie = p; + } else + return; + + if (remainder_ielen > 0) { + pbackup_remainder_ie = rtw_malloc(remainder_ielen); + if (pbackup_remainder_ie && premainder_ie) + _rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen); + } + + /* copy remainder IE */ + if (pbackup_remainder_ie) { + _rtw_memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen); + + rtw_mfree(pbackup_remainder_ie, remainder_ielen); + } + + offset = (uint)(dst_ie - pie); + pnetwork->IELength = offset + remainder_ielen; +} + + +u8 chk_sta_is_alive(struct sta_info *psta); +u8 chk_sta_is_alive(struct sta_info *psta) +{ + u8 ret = _FALSE; +#ifdef DBG_EXPIRATION_CHK + RTW_INFO("sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", expire_to:%u, %s%ssq_len:%u\n" + , MAC_ARG(psta->cmn.mac_addr) + , psta->cmn.rssi_stat.rssi + /* , STA_RX_PKTS_ARG(psta) */ + , STA_RX_PKTS_DIFF_ARG(psta) + , psta->expire_to + , psta->state & WIFI_SLEEP_STATE ? "PS, " : "" + , psta->state & WIFI_STA_ALIVE_CHK_STATE ? "SAC, " : "" + , psta->sleepq_len + ); +#endif + + /* if(sta_last_rx_pkts(psta) == sta_rx_pkts(psta)) */ + if ((psta->sta_stats.last_rx_data_pkts + psta->sta_stats.last_rx_ctrl_pkts) == (psta->sta_stats.rx_data_pkts + psta->sta_stats.rx_ctrl_pkts)) { +#if 0 + if (psta->state & WIFI_SLEEP_STATE) + ret = _TRUE; +#endif + } else + ret = _TRUE; + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(psta->padapter)) { + u8 bcn_alive, hwmp_alive; + + hwmp_alive = (psta->sta_stats.rx_hwmp_pkts != + psta->sta_stats.last_rx_hwmp_pkts); + bcn_alive = (psta->sta_stats.rx_beacon_pkts != + psta->sta_stats.last_rx_beacon_pkts); + /* The reference for nexthop_lookup */ + psta->alive = ret || hwmp_alive || bcn_alive; + /* The reference for expire_timeout_chk */ + /* Exclude bcn_alive to avoid a misjudge condition + that a peer unexpectedly leave and restart quickly*/ + ret = ret || hwmp_alive; + } +#endif + + sta_update_last_rx_pkts(psta); + + return ret; +} + +/** + * issue_aka_chk_frame - issue active keep alive check frame + * aka = active keep alive + */ +#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK +static int issue_aka_chk_frame(_adapter *adapter, struct sta_info *psta) +{ + int ret = _FAIL; + u8 *target_addr = psta->cmn.mac_addr; + + if (MLME_IS_AP(adapter)) { + /* issue null data to check sta alive */ + if (psta->state & WIFI_SLEEP_STATE) + ret = issue_nulldata(adapter, target_addr, 0, 1, 50); + else + ret = issue_nulldata(adapter, target_addr, 0, 3, 50); + } + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + struct rtw_mesh_path *mpath; + + rtw_rcu_read_lock(); + mpath = rtw_mesh_path_lookup(adapter, target_addr); + if (!mpath) { + mpath = rtw_mesh_path_add(adapter, target_addr); + if (IS_ERR(mpath)) { + rtw_rcu_read_unlock(); + RTW_ERR(FUNC_ADPT_FMT" rtw_mesh_path_add for "MAC_FMT" fail.\n", + FUNC_ADPT_ARG(adapter), MAC_ARG(target_addr)); + return _FAIL; + } + } + if (mpath->flags & RTW_MESH_PATH_ACTIVE) + ret = _SUCCESS; + else { + u8 flags = RTW_PREQ_Q_F_START | RTW_PREQ_Q_F_PEER_AKA; + /* issue PREQ to check peer alive */ + rtw_mesh_queue_preq(mpath, flags); + ret = _FALSE; + } + rtw_rcu_read_unlock(); + } +#endif + return ret; +} +#endif + +#ifdef RTW_CONFIG_RFREG18_WA +static void rtw_check_restore_rf18(_adapter *padapter) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + u32 reg; + u8 union_ch = 0, union_bw = 0, union_offset = 0, setchbw = _FALSE; + + reg = rtw_hal_read_rfreg(padapter, 0, 0x18, 0x3FF); + if ((reg & 0xFF) == 0) + setchbw = _TRUE; + reg = rtw_hal_read_rfreg(padapter, 1, 0x18, 0x3FF); + if ((reg & 0xFF) == 0) + setchbw = _TRUE; + + if (setchbw) { + if (!rtw_mi_get_ch_setting_union(padapter, &union_ch, &union_bw, &union_offset)) { + RTW_INFO("Hit RF(0x18)=0!! restore original channel setting.\n"); + union_ch = pmlmeext->cur_channel; + union_offset = pmlmeext->cur_ch_offset ; + union_bw = pmlmeext->cur_bwmode; + } else { + RTW_INFO("Hit RF(0x18)=0!! set ch(%x) offset(%x) bwmode(%x)\n", union_ch, union_offset, union_bw); + } + /* Initial the channel_bw setting procedure. */ + pHalData->current_channel = 0; + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + } +} +#endif + +void expire_timeout_chk(_adapter *padapter) +{ + _irqL irqL; + _list *phead, *plist; + u8 updated = _FALSE; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 chk_alive_num = 0; + char chk_alive_list[NUM_STA]; + int i; + int stainfo_offset; + u8 flush_num = 0; + char flush_list[NUM_STA]={0}; + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter) + && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) + ) { + struct rtw_mesh_cfg *mcfg = &padapter->mesh_cfg; + + rtw_mesh_path_expire(padapter); + + /* TBD: up layer timeout mechanism */ + /* if (!mcfg->plink_timeout) + return; */ +#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + return; +#endif + } +#endif + +#ifdef CONFIG_RTW_WDS + rtw_wds_path_expire(padapter); +#endif + +#ifdef CONFIG_MCC_MODE + /* then driver may check fail due to not recv client's frame under sitesurvey, + * don't expire timeout chk under MCC under sitesurvey */ + + if (rtw_hal_mcc_link_status_chk(padapter, __func__) == _FALSE) + return; +#endif + + _enter_critical_bh(&pstapriv->auth_list_lock, &irqL); + + phead = &pstapriv->auth_list; + plist = get_next(phead); + + /* check auth_queue */ +#ifdef DBG_EXPIRATION_CHK + if (rtw_end_of_queue_search(phead, plist) == _FALSE) { + RTW_INFO(FUNC_ADPT_FMT" auth_list, cnt:%u\n" + , FUNC_ADPT_ARG(padapter), pstapriv->auth_list_cnt); + } +#endif + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, auth_list); + + plist = get_next(plist); + + +#ifdef CONFIG_ATMEL_RC_PATCH + if (_rtw_memcmp((void *)(pstapriv->atmel_rc_pattern), (void *)(psta->cmn.mac_addr), ETH_ALEN) == _TRUE) + continue; + if (psta->flag_atmel_rc) + continue; +#endif + if (psta->expire_to > 0) { + psta->expire_to--; + if (psta->expire_to == 0) { + stainfo_offset = rtw_stainfo_offset(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) + flush_list[flush_num++] = stainfo_offset; + else + rtw_warn_on(1); + } + } + + } + + _exit_critical_bh(&pstapriv->auth_list_lock, &irqL); + for (i = 0; i < flush_num; i++) { + psta = rtw_get_stainfo_by_offset(pstapriv, flush_list[i]); + RTW_INFO(FUNC_ADPT_FMT" auth expire "MAC_FMT"\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(psta->cmn.mac_addr)); + rtw_free_stainfo(padapter, psta); + psta = NULL; + } + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* check asoc_queue */ +#ifdef DBG_EXPIRATION_CHK + if (rtw_end_of_queue_search(phead, plist) == _FALSE) { + RTW_INFO(FUNC_ADPT_FMT" asoc_list, cnt:%u\n" + , FUNC_ADPT_ARG(padapter), pstapriv->asoc_list_cnt); + } +#endif + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); +#ifdef CONFIG_ATMEL_RC_PATCH + RTW_INFO("%s:%d psta=%p, %02x,%02x||%02x,%02x \n\n", __func__, __LINE__, + psta, pstapriv->atmel_rc_pattern[0], pstapriv->atmel_rc_pattern[5], psta->cmn.mac_addr[0], psta->cmn.mac_addr[5]); + if (_rtw_memcmp((void *)pstapriv->atmel_rc_pattern, (void *)(psta->cmn.mac_addr), ETH_ALEN) == _TRUE) + continue; + if (psta->flag_atmel_rc) + continue; + RTW_INFO("%s: debug line:%d\n", __func__, __LINE__); +#endif +#ifdef CONFIG_AUTO_AP_MODE + if (psta->isrc) + continue; +#endif + if (chk_sta_is_alive(psta) || !psta->expire_to) { + psta->expire_to = pstapriv->expire_to; + psta->keep_alive_trycnt = 0; + #if !defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && defined(CONFIG_80211N_HT) + psta->under_exist_checking = 0; + #endif + } else + psta->expire_to--; + +#if !defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && defined(CONFIG_80211N_HT) + if ((psta->flags & WLAN_STA_HT) && (psta->htpriv.agg_enable_bitmap || psta->under_exist_checking)) { + /* check sta by delba(addba) for 11n STA */ + /* ToDo: use CCX report to check for all STAs */ + /* RTW_INFO("asoc check by DELBA/ADDBA! (pstapriv->expire_to=%d s)(psta->expire_to=%d s), [%02x, %d]\n", pstapriv->expire_to*2, psta->expire_to*2, psta->htpriv.agg_enable_bitmap, psta->under_exist_checking); */ + if (psta->expire_to <= (pstapriv->expire_to - 50)) { + RTW_INFO("asoc expire by DELBA/ADDBA! (%d s)\n", (pstapriv->expire_to - psta->expire_to) * 2); + psta->under_exist_checking = 0; + psta->expire_to = 0; + } else if (psta->expire_to <= (pstapriv->expire_to - 3) && (psta->under_exist_checking == 0)) { + RTW_INFO("asoc check by DELBA/ADDBA! (%d s)\n", (pstapriv->expire_to - psta->expire_to) * 2); + psta->under_exist_checking = 1; + /* tear down TX AMPDU */ + send_delba(padapter, 1, psta->cmn.mac_addr);/* */ /* originator */ + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ + } + } +#endif /* !defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && defined(CONFIG_80211N_HT) */ + + if (psta->expire_to <= 0) { + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (padapter->registrypriv.wifi_spec == 1) { + psta->expire_to = pstapriv->expire_to; + continue; + } + +#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK +#ifdef CONFIG_80211N_HT + +#define KEEP_ALIVE_TRYCNT (3) + + if (psta->keep_alive_trycnt > 0 && psta->keep_alive_trycnt <= KEEP_ALIVE_TRYCNT) { + if (psta->state & WIFI_STA_ALIVE_CHK_STATE) + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + else + psta->keep_alive_trycnt = 0; + + } else if ((psta->keep_alive_trycnt > KEEP_ALIVE_TRYCNT) && !(psta->state & WIFI_STA_ALIVE_CHK_STATE)) + psta->keep_alive_trycnt = 0; + if ((psta->htpriv.ht_option == _TRUE) && (psta->htpriv.ampdu_enable == _TRUE)) { + uint priority = 1; /* test using BK */ + u8 issued = 0; + + /* issued = (psta->htpriv.agg_enable_bitmap>>priority)&0x1; */ + issued |= (psta->htpriv.candidate_tid_bitmap >> priority) & 0x1; + + if (0 == issued) { + if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) { + psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority); + + if (psta->state & WIFI_SLEEP_STATE) + psta->expire_to = 2; /* 2x2=4 sec */ + else + psta->expire_to = 1; /* 2 sec */ + + psta->state |= WIFI_STA_ALIVE_CHK_STATE; + + /* add_ba_hdl(padapter, (u8*)paddbareq_parm); */ + + RTW_INFO("issue addba_req to check if sta alive, keep_alive_trycnt=%d\n", psta->keep_alive_trycnt); + + issue_addba_req(padapter, psta->cmn.mac_addr, (u8)priority); + + _set_timer(&psta->addba_retry_timer, ADDBA_TO); + + psta->keep_alive_trycnt++; + + continue; + } + } + } + if (psta->keep_alive_trycnt > 0 && psta->state & WIFI_STA_ALIVE_CHK_STATE) { + psta->keep_alive_trycnt = 0; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + RTW_INFO("change to another methods to check alive if staion is at ps mode\n"); + } + +#endif /* CONFIG_80211N_HT */ +#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */ + if (psta->state & WIFI_SLEEP_STATE) { + if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) { + /* to check if alive by another methods if staion is at ps mode. */ + psta->expire_to = pstapriv->expire_to; + psta->state |= WIFI_STA_ALIVE_CHK_STATE; + + /* RTW_INFO("alive chk, sta:" MAC_FMT " is at ps mode!\n", MAC_ARG(psta->cmn.mac_addr)); */ + + /* to update bcn with tim_bitmap for this station */ + rtw_tim_map_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0); + + if (!pmlmeext->active_keep_alive_check) + continue; + } + } + + { + int stainfo_offset; + + stainfo_offset = rtw_stainfo_offset(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) + chk_alive_list[chk_alive_num++] = stainfo_offset; + continue; + } + } else { + /* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */ + if (psta->sleepq_len > (NR_XMITFRAME / pstapriv->asoc_list_cnt) + && padapter->xmitpriv.free_xmitframe_cnt < ((NR_XMITFRAME / pstapriv->asoc_list_cnt) / 2) + ) { + RTW_INFO(FUNC_ADPT_FMT" sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(psta->cmn.mac_addr) + , psta->sleepq_len, padapter->xmitpriv.free_xmitframe_cnt, pstapriv->asoc_list_cnt); + wakeup_sta_to_xmit(padapter, psta); + } + } + } + + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + if (chk_alive_num) { +#if defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) + u8 backup_ch = 0, backup_bw = 0, backup_offset = 0; + u8 union_ch = 0, union_bw = 0, union_offset = 0; + u8 switch_channel_by_drv = _TRUE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#endif + char del_asoc_list[NUM_STA]; + + _rtw_memset(del_asoc_list, NUM_STA, NUM_STA); + + #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + if (pmlmeext->active_keep_alive_check) { + #ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) { + /* driver doesn't switch channel under MCC */ + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) + switch_channel_by_drv = _FALSE; + } + #endif + + if (!rtw_mi_get_ch_setting_union(padapter, &union_ch, &union_bw, &union_offset) + || pmlmeext->cur_channel != union_ch) + switch_channel_by_drv = _FALSE; + + /* switch to correct channel of current network before issue keep-alive frames */ + if (switch_channel_by_drv == _TRUE && rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) { + backup_ch = rtw_get_oper_ch(padapter); + backup_bw = rtw_get_oper_bw(padapter); + backup_offset = rtw_get_oper_choffset(padapter); + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + } + } + #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */ + + /* check loop */ + for (i = 0; i < chk_alive_num; i++) { + #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + int ret = _FAIL; + #endif + + psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]); + + #ifdef CONFIG_ATMEL_RC_PATCH + if (_rtw_memcmp(pstapriv->atmel_rc_pattern, psta->cmn.mac_addr, ETH_ALEN) == _TRUE) + continue; + if (psta->flag_atmel_rc) + continue; + #endif + + if (!(psta->state & WIFI_ASOC_STATE)) + continue; + + #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + if (pmlmeext->active_keep_alive_check) { + /* issue active keep alive frame to check */ + ret = issue_aka_chk_frame(padapter, psta); + + psta->keep_alive_trycnt++; + if (ret == _SUCCESS) { + RTW_INFO(FUNC_ADPT_FMT" asoc check, "MAC_FMT" is alive\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(psta->cmn.mac_addr)); + psta->expire_to = pstapriv->expire_to; + psta->keep_alive_trycnt = 0; + continue; + } else if (psta->keep_alive_trycnt <= 3) { + RTW_INFO(FUNC_ADPT_FMT" asoc check, "MAC_FMT" keep_alive_trycnt=%d\n" + , FUNC_ADPT_ARG(padapter) , MAC_ARG(psta->cmn.mac_addr), psta->keep_alive_trycnt); + psta->expire_to = 1; + continue; + } + } + #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */ + + psta->keep_alive_trycnt = 0; + del_asoc_list[i] = chk_alive_list[i]; + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&psta->asoc_list) == _FALSE) { + rtw_list_delete(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + STA_SET_MESH_PLINK(psta, NULL); + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + } + + /* delete loop */ + for (i = 0; i < chk_alive_num; i++) { + u8 sta_addr[ETH_ALEN]; + + if (del_asoc_list[i] >= NUM_STA) + continue; + + psta = rtw_get_stainfo_by_offset(pstapriv, del_asoc_list[i]); + _rtw_memcpy(sta_addr, psta->cmn.mac_addr, ETH_ALEN); + + RTW_INFO(FUNC_ADPT_FMT" asoc expire "MAC_FMT", state=0x%x\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(psta->cmn.mac_addr), psta->state); + updated |= ap_free_sta(padapter, psta, _FALSE, WLAN_REASON_DEAUTH_LEAVING, _FALSE); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + rtw_mesh_expire_peer(padapter, sta_addr); + #endif + } + + #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + if (pmlmeext->active_keep_alive_check) { + /* back to the original operation channel */ + if (switch_channel_by_drv == _TRUE && backup_ch > 0) + set_channel_bwmode(padapter, backup_ch, backup_offset, backup_bw); + } + #endif + } + +#ifdef RTW_CONFIG_RFREG18_WA + rtw_check_restore_rf18(padapter); +#endif + associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL); +} + +void rtw_ap_update_sta_ra_info(_adapter *padapter, struct sta_info *psta) +{ + unsigned char sta_band = 0; + u64 tx_ra_bitmap = 0; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network; + + if (!psta) + return; + + if (!(psta->state & WIFI_ASOC_STATE)) + return; + + rtw_hal_update_sta_ra_info(padapter, psta); + tx_ra_bitmap = psta->cmn.ra_info.ramask; + + if (pcur_network->Configuration.DSConfig > 14) { + + if (tx_ra_bitmap & 0xffff000) + sta_band |= WIRELESS_11_5N; + + if (tx_ra_bitmap & 0xff0) + sta_band |= WIRELESS_11A; + + /* 5G band */ +#ifdef CONFIG_80211AC_VHT + if (psta->vhtpriv.vht_option) + sta_band = WIRELESS_11_5AC; +#endif + } else { + if (tx_ra_bitmap & 0xffff000) + sta_band |= WIRELESS_11_24N; + + if (tx_ra_bitmap & 0xff0) + sta_band |= WIRELESS_11G; + + if (tx_ra_bitmap & 0x0f) + sta_band |= WIRELESS_11B; + } + + psta->wireless_mode = sta_band; + rtw_hal_update_sta_wset(padapter, psta); + RTW_INFO("%s=> mac_id:%d , tx_ra_bitmap:0x%016llx, networkType:0x%02x\n", + __FUNCTION__, psta->cmn.mac_id, tx_ra_bitmap, psta->wireless_mode); +} + +#ifdef CONFIG_BMC_TX_RATE_SELECT +u8 rtw_ap_find_mini_tx_rate(_adapter *adapter) +{ + _irqL irqL; + _list *phead, *plist; + u8 miini_tx_rate = ODM_RATEVHTSS4MCS9, sta_tx_rate; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &adapter->stapriv; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + sta_tx_rate = psta->cmn.ra_info.curr_tx_rate & 0x7F; + if (sta_tx_rate < miini_tx_rate) + miini_tx_rate = sta_tx_rate; + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + return miini_tx_rate; +} + +u8 rtw_ap_find_bmc_rate(_adapter *adapter, u8 tx_rate) +{ + PHAL_DATA_TYPE hal_data = GET_HAL_DATA(adapter); + u8 tx_ini_rate = ODM_RATE6M; + + switch (tx_rate) { + case ODM_RATEVHTSS3MCS9: + case ODM_RATEVHTSS3MCS8: + case ODM_RATEVHTSS3MCS7: + case ODM_RATEVHTSS3MCS6: + case ODM_RATEVHTSS3MCS5: + case ODM_RATEVHTSS3MCS4: + case ODM_RATEVHTSS3MCS3: + case ODM_RATEVHTSS2MCS9: + case ODM_RATEVHTSS2MCS8: + case ODM_RATEVHTSS2MCS7: + case ODM_RATEVHTSS2MCS6: + case ODM_RATEVHTSS2MCS5: + case ODM_RATEVHTSS2MCS4: + case ODM_RATEVHTSS2MCS3: + case ODM_RATEVHTSS1MCS9: + case ODM_RATEVHTSS1MCS8: + case ODM_RATEVHTSS1MCS7: + case ODM_RATEVHTSS1MCS6: + case ODM_RATEVHTSS1MCS5: + case ODM_RATEVHTSS1MCS4: + case ODM_RATEVHTSS1MCS3: + case ODM_RATEMCS15: + case ODM_RATEMCS14: + case ODM_RATEMCS13: + case ODM_RATEMCS12: + case ODM_RATEMCS11: + case ODM_RATEMCS7: + case ODM_RATEMCS6: + case ODM_RATEMCS5: + case ODM_RATEMCS4: + case ODM_RATEMCS3: + case ODM_RATE54M: + case ODM_RATE48M: + case ODM_RATE36M: + case ODM_RATE24M: + tx_ini_rate = ODM_RATE24M; + break; + case ODM_RATEVHTSS3MCS2: + case ODM_RATEVHTSS3MCS1: + case ODM_RATEVHTSS2MCS2: + case ODM_RATEVHTSS2MCS1: + case ODM_RATEVHTSS1MCS2: + case ODM_RATEVHTSS1MCS1: + case ODM_RATEMCS10: + case ODM_RATEMCS9: + case ODM_RATEMCS2: + case ODM_RATEMCS1: + case ODM_RATE18M: + case ODM_RATE12M: + tx_ini_rate = ODM_RATE12M; + break; + case ODM_RATEVHTSS3MCS0: + case ODM_RATEVHTSS2MCS0: + case ODM_RATEVHTSS1MCS0: + case ODM_RATEMCS8: + case ODM_RATEMCS0: + case ODM_RATE9M: + case ODM_RATE6M: + tx_ini_rate = ODM_RATE6M; + break; + case ODM_RATE11M: + case ODM_RATE5_5M: + case ODM_RATE2M: + case ODM_RATE1M: + tx_ini_rate = ODM_RATE1M; + break; + default: + tx_ini_rate = ODM_RATE6M; + break; + } + + if (hal_data->current_band_type == BAND_ON_5G) + if (tx_ini_rate < ODM_RATE6M) + tx_ini_rate = ODM_RATE6M; + + return tx_ini_rate; +} + +void rtw_update_bmc_sta_tx_rate(_adapter *adapter) +{ + struct sta_info *psta = NULL; + u8 tx_rate; + + psta = rtw_get_bcmc_stainfo(adapter); + if (psta == NULL) { + RTW_ERR(ADPT_FMT "could not get bmc_sta !!\n", ADPT_ARG(adapter)); + return; + } + + if (adapter->bmc_tx_rate != MGN_UNKNOWN) { + psta->init_rate = adapter->bmc_tx_rate; + goto _exit; + } + + if (adapter->stapriv.asoc_sta_count <= 2) + goto _exit; + + tx_rate = rtw_ap_find_mini_tx_rate(adapter); + #ifdef CONFIG_BMC_TX_LOW_RATE + tx_rate = rtw_ap_find_bmc_rate(adapter, tx_rate); + #endif + + psta->init_rate = hw_rate_to_m_rate(tx_rate); + +_exit: + RTW_INFO(ADPT_FMT" BMC Tx rate - %s\n", ADPT_ARG(adapter), MGN_RATE_STR(psta->init_rate)); +} +#endif + +void rtw_init_bmc_sta_tx_rate(_adapter *padapter, struct sta_info *psta) +{ +#ifdef CONFIG_BMC_TX_LOW_RATE + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); +#endif + u8 rate_idx = 0; + u8 brate_table[] = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M, + MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M}; + + if (!MLME_IS_AP(padapter) && !MLME_IS_MESH(padapter)) + return; + + if (padapter->bmc_tx_rate != MGN_UNKNOWN) + psta->init_rate = padapter->bmc_tx_rate; + else { + #ifdef CONFIG_BMC_TX_LOW_RATE + if (IsEnableHWOFDM(pmlmeext->cur_wireless_mode) && (psta->cmn.ra_info.ramask && 0xFF0)) + rate_idx = get_lowest_rate_idx_ex(psta->cmn.ra_info.ramask, 4); /*from basic rate*/ + else + rate_idx = get_lowest_rate_idx(psta->cmn.ra_info.ramask); /*from basic rate*/ + #else + rate_idx = get_highest_rate_idx(psta->cmn.ra_info.ramask); /*from basic rate*/ + #endif + if (rate_idx < 12) + psta->init_rate = brate_table[rate_idx]; + else + psta->init_rate = MGN_1M; + } + + RTW_INFO(ADPT_FMT" BMC Init Tx rate - %s\n", ADPT_ARG(padapter), MGN_RATE_STR(psta->init_rate)); +} + +void update_bmc_sta(_adapter *padapter) +{ + _irqL irqL; + unsigned char network_type; + int supportRateNum = 0; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network; + struct sta_info *psta = rtw_get_bcmc_stainfo(padapter); + + if (psta) { + psta->cmn.aid = 0;/* default set to 0 */ +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + psta->qos_option = 1; + else +#endif + psta->qos_option = 0; +#ifdef CONFIG_80211N_HT + psta->htpriv.ht_option = _FALSE; +#endif /* CONFIG_80211N_HT */ + + psta->ieee8021x_blocked = 0; + + _rtw_memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + /* psta->dot118021XPrivacy = _NO_PRIVACY_; */ /* !!! remove it, because it has been set before this. */ + + supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->SupportedRates); + network_type = rtw_check_network_type((u8 *)&pcur_network->SupportedRates, supportRateNum, pcur_network->Configuration.DSConfig); + if (IsSupportedTxCCK(network_type)) + network_type = WIRELESS_11B; + else if (network_type == WIRELESS_INVALID) { /* error handling */ + if (pcur_network->Configuration.DSConfig > 14) + network_type = WIRELESS_11A; + else + network_type = WIRELESS_11B; + } + update_sta_basic_rate(psta, network_type); + psta->wireless_mode = network_type; + + rtw_hal_update_sta_ra_info(padapter, psta); + + _enter_critical_bh(&psta->lock, &irqL); + psta->state = WIFI_ASOC_STATE; + _exit_critical_bh(&psta->lock, &irqL); + + rtw_sta_media_status_rpt(padapter, psta, 1); + rtw_init_bmc_sta_tx_rate(padapter, psta); + + } else + RTW_INFO("add_RATid_bmc_sta error!\n"); + +} + +#if defined(CONFIG_80211N_HT) && defined(CONFIG_BEAMFORMING) +void update_sta_info_apmode_ht_bf_cap(_adapter *padapter, struct sta_info *psta) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; + struct ht_priv *phtpriv_sta = &psta->htpriv; + + u8 cur_beamform_cap = 0; + + /*Config Tx beamforming setting*/ + if (TEST_FLAG(phtpriv_ap->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP((u8 *)(&phtpriv_sta->ht_cap))) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + /*Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS((u8 *)(&phtpriv_sta->ht_cap)) << 6); + } + + if (TEST_FLAG(phtpriv_ap->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP((u8 *)(&phtpriv_sta->ht_cap))) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + /*Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS((u8 *)(&phtpriv_sta->ht_cap)) << 4); + } + if (cur_beamform_cap) + RTW_INFO("Client STA(%d) HT Beamforming Cap = 0x%02X\n", psta->cmn.aid, cur_beamform_cap); + + phtpriv_sta->beamform_cap = cur_beamform_cap; + psta->cmn.bf_info.ht_beamform_cap = cur_beamform_cap; + +} +#endif /*CONFIG_80211N_HT && CONFIG_BEAMFORMING*/ + +/* notes: + * AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode */ +void update_sta_info_apmode(_adapter *padapter, struct sta_info *psta) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); +#ifdef CONFIG_80211N_HT + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; + struct ht_priv *phtpriv_sta = &psta->htpriv; +#endif /* CONFIG_80211N_HT */ + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0; + /* set intf_tag to if1 */ + /* psta->intf_tag = 0; */ + + RTW_INFO("%s\n", __FUNCTION__); + + /*alloc macid when call rtw_alloc_stainfo(),release macid when call rtw_free_stainfo()*/ + + if (!MLME_IS_MESH(padapter) && psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + psta->ieee8021x_blocked = _TRUE; + else + psta->ieee8021x_blocked = _FALSE; + + + /* update sta's cap */ + + /* ERP */ + VCS_update(padapter, psta); +#ifdef CONFIG_80211N_HT + /* HT related cap */ + if (phtpriv_sta->ht_option) { + /* check if sta supports rx ampdu */ + phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable; + + phtpriv_sta->rx_ampdu_min_spacing = (phtpriv_sta->ht_cap.ampdu_params_info & IEEE80211_HT_CAP_AMPDU_DENSITY) >> 2; + + /* bwmode */ + if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH)) + psta->cmn.bw_mode = CHANNEL_WIDTH_40; + else + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + + if (phtpriv_sta->op_present + && !GET_HT_OP_ELE_STA_CHL_WIDTH(phtpriv_sta->ht_op)) + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + + if (psta->ht_40mhz_intolerant) + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + + if (pmlmeext->cur_bwmode < psta->cmn.bw_mode) + psta->cmn.bw_mode = pmlmeext->cur_bwmode; + + phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset; + + + /* check if sta support s Short GI 20M */ + if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20)) + phtpriv_sta->sgi_20m = _TRUE; + + /* check if sta support s Short GI 40M */ + if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) { + if (psta->cmn.bw_mode == CHANNEL_WIDTH_40) /* according to psta->bw_mode */ + phtpriv_sta->sgi_40m = _TRUE; + else + phtpriv_sta->sgi_40m = _FALSE; + } + + psta->qos_option = _TRUE; + + /* B0 Config LDPC Coding Capability */ + if (TEST_FLAG(phtpriv_ap->ldpc_cap, LDPC_HT_ENABLE_TX) && + GET_HT_CAP_ELE_LDPC_CAP((u8 *)(&phtpriv_sta->ht_cap))) { + SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX)); + RTW_INFO("Enable HT Tx LDPC for STA(%d)\n", psta->cmn.aid); + } + + /* B7 B8 B9 Config STBC setting */ + if (TEST_FLAG(phtpriv_ap->stbc_cap, STBC_HT_ENABLE_TX) && + GET_HT_CAP_ELE_RX_STBC((u8 *)(&phtpriv_sta->ht_cap))) { + SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX)); + RTW_INFO("Enable HT Tx STBC for STA(%d)\n", psta->cmn.aid); + } + + #ifdef CONFIG_BEAMFORMING + update_sta_info_apmode_ht_bf_cap(padapter, psta); + #endif + } else { + phtpriv_sta->ampdu_enable = _FALSE; + + phtpriv_sta->sgi_20m = _FALSE; + phtpriv_sta->sgi_40m = _FALSE; + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + phtpriv_sta->ldpc_cap = cur_ldpc_cap; + phtpriv_sta->stbc_cap = cur_stbc_cap; + + /* Rx AMPDU */ + send_delba(padapter, 0, psta->cmn.mac_addr);/* recipient */ + + /* TX AMPDU */ + send_delba(padapter, 1, psta->cmn.mac_addr);/* */ /* originator */ + phtpriv_sta->agg_enable_bitmap = 0x0;/* reset */ + phtpriv_sta->candidate_tid_bitmap = 0x0;/* reset */ +#endif /* CONFIG_80211N_HT */ + +#ifdef CONFIG_80211AC_VHT + update_sta_vht_info_apmode(padapter, psta); +#endif + psta->cmn.ra_info.is_support_sgi = query_ra_short_GI(psta, rtw_get_tx_bw_mode(padapter, psta)); + update_ldpc_stbc_cap(psta); + + /* todo: init other variables */ + + _rtw_memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + + /* add ratid */ + /* add_RATid(padapter, psta); */ /* move to ap_sta_info_defer_update() */ + + /* ap mode */ + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE); + + _enter_critical_bh(&psta->lock, &irqL); + + /* Check encryption */ + if (!MLME_IS_MESH(padapter) && psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + psta->state |= WIFI_UNDER_KEY_HANDSHAKE; + + psta->state |= WIFI_ASOC_STATE; + + _exit_critical_bh(&psta->lock, &irqL); +} + +#ifdef CONFIG_RTW_80211K +static void update_rm_cap(u8 *frame_head, _adapter *pa, u32 pktlen, int offset) +{ + u8 *res; + sint len; + + res = rtw_get_ie(frame_head + offset, _EID_RRM_EN_CAP_IE_, &len, + pktlen - offset); + if (res != NULL) + _rtw_memcpy((void *)pa->rmpriv.rm_en_cap_def, (res + 2), len); +} +#endif + +static void update_ap_info(_adapter *padapter, struct sta_info *psta) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); +#ifdef CONFIG_80211N_HT + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; +#endif /* CONFIG_80211N_HT */ + + psta->wireless_mode = pmlmeext->cur_wireless_mode; + + psta->bssratelen = rtw_get_rateset_len(pnetwork->SupportedRates); + _rtw_memcpy(psta->bssrateset, pnetwork->SupportedRates, psta->bssratelen); + +#ifdef CONFIG_80211N_HT + /* HT related cap */ + if (phtpriv_ap->ht_option) { + /* check if sta supports rx ampdu */ + /* phtpriv_ap->ampdu_enable = phtpriv_ap->ampdu_enable; */ + + /* check if sta support s Short GI 20M */ + if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20)) + phtpriv_ap->sgi_20m = _TRUE; + /* check if sta support s Short GI 40M */ + if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) + phtpriv_ap->sgi_40m = _TRUE; + + psta->qos_option = _TRUE; + } else { + phtpriv_ap->ampdu_enable = _FALSE; + + phtpriv_ap->sgi_20m = _FALSE; + phtpriv_ap->sgi_40m = _FALSE; + } + + psta->cmn.bw_mode = pmlmeext->cur_bwmode; + phtpriv_ap->ch_offset = pmlmeext->cur_ch_offset; + + phtpriv_ap->agg_enable_bitmap = 0x0;/* reset */ + phtpriv_ap->candidate_tid_bitmap = 0x0;/* reset */ + + _rtw_memcpy(&psta->htpriv, &pmlmepriv->htpriv, sizeof(struct ht_priv)); + +#ifdef CONFIG_80211AC_VHT + _rtw_memcpy(&psta->vhtpriv, &pmlmepriv->vhtpriv, sizeof(struct vht_priv)); +#endif /* CONFIG_80211AC_VHT */ + +#endif /* CONFIG_80211N_HT */ + + psta->state |= WIFI_AP_STATE; /* Aries, add,fix bug of flush_cam_entry at STOP AP mode , 0724 */ +} + +static void rtw_set_hw_wmm_param(_adapter *padapter) +{ + u8 AIFS, ECWMin, ECWMax, aSifsTime; + u8 acm_mask; + u16 TXOP; + u32 acParm, i; + u32 edca[4], inx[4]; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct registry_priv *pregpriv = &padapter->registrypriv; + + acm_mask = 0; +#ifdef CONFIG_80211N_HT + if (pregpriv->ht_enable && + (is_supported_5g(pmlmeext->cur_wireless_mode) || + (pmlmeext->cur_wireless_mode & WIRELESS_11_24N))) + aSifsTime = 16; + else +#endif /* CONFIG_80211N_HT */ + aSifsTime = 10; + + if (pmlmeinfo->WMM_enable == 0) { + padapter->mlmepriv.acm_mask = 0; + + AIFS = aSifsTime + (2 * pmlmeinfo->slotTime); + + if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11A)) { + ECWMin = 4; + ECWMax = 10; + } else if (pmlmeext->cur_wireless_mode & WIRELESS_11B) { + ECWMin = 5; + ECWMax = 10; + } else { + ECWMin = 4; + ECWMax = 10; + } + + TXOP = 0; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm)); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm)); + + ECWMin = 2; + ECWMax = 3; + TXOP = 0x2f; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + + } else { + edca[0] = edca[1] = edca[2] = edca[3] = 0; + + /*TODO:*/ + acm_mask = 0; + padapter->mlmepriv.acm_mask = acm_mask; + +#if 0 + /* BK */ + /* AIFS = AIFSN * slot time + SIFS - r2t phy delay */ +#endif + AIFS = (7 * pmlmeinfo->slotTime) + aSifsTime; + ECWMin = 4; + ECWMax = 10; + TXOP = 0; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + edca[XMIT_BK_QUEUE] = acParm; + RTW_INFO("WMM(BK): %x\n", acParm); + + /* BE */ + AIFS = (3 * pmlmeinfo->slotTime) + aSifsTime; + ECWMin = 4; + ECWMax = 6; + TXOP = 0; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm)); + edca[XMIT_BE_QUEUE] = acParm; + RTW_INFO("WMM(BE): %x\n", acParm); + + /* VI */ + AIFS = (1 * pmlmeinfo->slotTime) + aSifsTime; + ECWMin = 3; + ECWMax = 4; + TXOP = 94; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm)); + edca[XMIT_VI_QUEUE] = acParm; + RTW_INFO("WMM(VI): %x\n", acParm); + + /* VO */ + AIFS = (1 * pmlmeinfo->slotTime) + aSifsTime; + ECWMin = 2; + ECWMax = 3; + TXOP = 47; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + edca[XMIT_VO_QUEUE] = acParm; + RTW_INFO("WMM(VO): %x\n", acParm); + + + if (padapter->registrypriv.acm_method == 1) + rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask)); + else + padapter->mlmepriv.acm_mask = acm_mask; + + inx[0] = 0; + inx[1] = 1; + inx[2] = 2; + inx[3] = 3; + + if (pregpriv->wifi_spec == 1) { + u32 j, tmp, change_inx = _FALSE; + + /* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */ + for (i = 0 ; i < 4 ; i++) { + for (j = i + 1 ; j < 4 ; j++) { + /* compare CW and AIFS */ + if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) + change_inx = _TRUE; + else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) { + /* compare TXOP */ + if ((edca[j] >> 16) > (edca[i] >> 16)) + change_inx = _TRUE; + } + + if (change_inx) { + tmp = edca[i]; + edca[i] = edca[j]; + edca[j] = tmp; + + tmp = inx[i]; + inx[i] = inx[j]; + inx[j] = tmp; + + change_inx = _FALSE; + } + } + } + } + + for (i = 0 ; i < 4 ; i++) { + pxmitpriv->wmm_para_seq[i] = inx[i]; + RTW_INFO("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]); + } + + } + +} +#ifdef CONFIG_80211N_HT +static void update_hw_ht_param(_adapter *padapter) +{ + unsigned char max_AMPDU_len; + unsigned char min_MPDU_spacing; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + RTW_INFO("%s\n", __FUNCTION__); + + + /* handle A-MPDU parameter field */ + /* + AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k + AMPDU_para [4:2]:Min MPDU Start Spacing + */ + max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03; + + min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2; + + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing)); + + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len)); + + /* */ + /* Config SM Power Save setting */ + /* */ + pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) { +#if 0 + u8 i; + /* update the MCS rates */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i]; +#endif + RTW_INFO("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __FUNCTION__); + } + + /* */ + /* Config current HT Protection mode. */ + /* */ + /* pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; */ + +} +#endif /* CONFIG_80211N_HT */ +static void rtw_ap_check_scan(_adapter *padapter) +{ + _irqL irqL; + _list *plist, *phead; + u32 delta_time, lifetime; + struct wlan_network *pnetwork = NULL; + WLAN_BSSID_EX *pbss = NULL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + _queue *queue = &(pmlmepriv->scanned_queue); + u8 do_scan = _FALSE; + u8 reason = RTW_AUTO_SCAN_REASON_UNSPECIFIED; + + lifetime = SCANQUEUE_LIFETIME; /* 20 sec */ + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + phead = get_list_head(queue); + if (rtw_end_of_queue_search(phead, get_next(phead)) == _TRUE) + if (padapter->registrypriv.wifi_spec) { + do_scan = _TRUE; + reason |= RTW_AUTO_SCAN_REASON_2040_BSS; + } + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + +#ifdef CONFIG_RTW_ACS + if (padapter->registrypriv.acs_auto_scan) { + do_scan = _TRUE; + reason |= RTW_AUTO_SCAN_REASON_ACS; + rtw_acs_start(padapter); + } +#endif/*CONFIG_RTW_ACS*/ + + if (_TRUE == do_scan) { + RTW_INFO("%s : drv scans by itself and wait_completed\n", __func__); + rtw_drv_scan_by_self(padapter, reason); + rtw_scan_wait_completed(padapter); + } + +#ifdef CONFIG_RTW_ACS + if (padapter->registrypriv.acs_auto_scan) + rtw_acs_stop(padapter); +#endif + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + phead = get_list_head(queue); + plist = get_next(phead); + + while (1) { + + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + if (rtw_chset_search_ch(adapter_to_chset(padapter), pnetwork->network.Configuration.DSConfig) >= 0 + && rtw_mlme_band_check(padapter, pnetwork->network.Configuration.DSConfig) == _TRUE + && _TRUE == rtw_validate_ssid(&(pnetwork->network.Ssid))) { + delta_time = (u32) rtw_get_passing_time_ms(pnetwork->last_scanned); + + if (delta_time < lifetime) { + + uint ie_len = 0; + u8 *pbuf = NULL; + u8 *ie = NULL; + + pbss = &pnetwork->network; + ie = pbss->IEs; + + /*check if HT CAP INFO IE exists or not*/ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pbss->IELength - _BEACON_IE_OFFSET_)); + if (pbuf == NULL) { + /* HT CAP INFO IE don't exist, it is b/g mode bss.*/ + + if (_FALSE == ATOMIC_READ(&pmlmepriv->olbc)) + ATOMIC_SET(&pmlmepriv->olbc, _TRUE); + + if (_FALSE == ATOMIC_READ(&pmlmepriv->olbc_ht)) + ATOMIC_SET(&pmlmepriv->olbc_ht, _TRUE); + + if (padapter->registrypriv.wifi_spec) + RTW_INFO("%s: %s is a/b/g ap\n", __func__, pnetwork->network.Ssid.Ssid); + } + } + } + + plist = get_next(plist); + + } + + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); +#ifdef CONFIG_80211N_HT + pmlmepriv->num_sta_no_ht = 0; /* reset to 0 after ap do scanning*/ +#endif +} + +void rtw_start_bss_hdl_after_chbw_decided(_adapter *adapter) +{ + WLAN_BSSID_EX *pnetwork = &(adapter->mlmepriv.cur_network.network); + struct sta_info *sta = NULL; + + /* update cur_wireless_mode */ + update_wireless_mode(adapter); + + /* update RRSR and RTS_INIT_RATE register after set channel and bandwidth */ + UpdateBrateTbl(adapter, pnetwork->SupportedRates); + rtw_hal_set_hwreg(adapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates); + + /* update capability after cur_wireless_mode updated */ + update_capinfo(adapter, rtw_get_capability(pnetwork)); + + /* update bc/mc sta_info */ + update_bmc_sta(adapter); + + /* update AP's sta info */ + sta = rtw_get_stainfo(&adapter->stapriv, pnetwork->MacAddress); + if (!sta) { + RTW_INFO(FUNC_ADPT_FMT" !sta for macaddr="MAC_FMT"\n", FUNC_ADPT_ARG(adapter), MAC_ARG(pnetwork->MacAddress)); + rtw_warn_on(1); + return; + } + + update_ap_info(adapter, sta); +} + +#ifdef CONFIG_FW_HANDLE_TXBCN +bool rtw_ap_nums_check(_adapter *adapter) +{ + if (rtw_ap_get_nums(adapter) < CONFIG_LIMITED_AP_NUM) + return _TRUE; + return _FALSE; +} +u8 rtw_ap_allocate_vapid(struct dvobj_priv *dvobj) +{ + u8 vap_id; + + for (vap_id = 0; vap_id < CONFIG_LIMITED_AP_NUM; vap_id++) { + if (!(dvobj->vap_map & BIT(vap_id))) + break; + } + + if (vap_id < CONFIG_LIMITED_AP_NUM) + dvobj->vap_map |= BIT(vap_id); + + return vap_id; +} +u8 rtw_ap_release_vapid(struct dvobj_priv *dvobj, u8 vap_id) +{ + if (vap_id >= CONFIG_LIMITED_AP_NUM) { + RTW_ERR("%s - vapid(%d) failed\n", __func__, vap_id); + rtw_warn_on(1); + return _FAIL; + } + dvobj->vap_map &= ~ BIT(vap_id); + return _SUCCESS; +} +#endif +static void _rtw_iface_undersurvey_chk(const char *func, _adapter *adapter) +{ + int i; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct mlme_priv *pmlmepriv; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + pmlmepriv = &iface->mlmepriv; + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY)) + RTW_ERR("%s ("ADPT_FMT") under survey\n", func, ADPT_ARG(iface)); + } + } +} +void start_bss_network(_adapter *padapter, struct createbss_parm *parm) +{ +#define DUMP_ADAPTERS_STATUS 0 + u8 mlme_act = MLME_ACTION_UNKNOWN; + u8 val8; + u16 bcn_interval; + u32 acparm; + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct security_priv *psecuritypriv = &(padapter->securitypriv); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network; /* used as input */ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork_mlmeext = &(pmlmeinfo->network); + struct dvobj_priv *pdvobj = padapter->dvobj; + s16 req_ch = REQ_CH_NONE, req_bw = REQ_BW_NONE, req_offset = REQ_OFFSET_NONE; + u8 u_ch = 0, u_bw, u_offset; + bool set_u_ch; + u8 doiqk = _FALSE; + /* use for check ch bw offset can be allowed or not */ + u8 chbw_allow = _TRUE; + int i; + u8 ifbmp_ch_changed = 0; + + if (parm->req_ch != 0) { + /* bypass other setting, go checking ch, bw, offset */ + mlme_act = MLME_OPCH_SWITCH; + req_ch = parm->req_ch; + req_bw = parm->req_bw; + req_offset = parm->req_offset; + goto chbw_decision; + } else { + /* request comes from upper layer */ + if (MLME_IS_AP(padapter)) + mlme_act = MLME_AP_STARTED; + else if (MLME_IS_MESH(padapter)) + mlme_act = MLME_MESH_STARTED; + else + rtw_warn_on(1); + req_ch = 0; + _rtw_memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length); + } + + bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod; + + /* check if there is wps ie, */ + /* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */ + /* and at first time the security ie ( RSN/WPA IE) will not include in beacon. */ + if (NULL == rtw_get_wps_ie(pnetwork->IEs + _FIXED_IE_LENGTH_, pnetwork->IELength - _FIXED_IE_LENGTH_, NULL, NULL)) + pmlmeext->bstart_bss = _TRUE; + + /* todo: update wmm, ht cap */ + /* pmlmeinfo->WMM_enable; */ + /* pmlmeinfo->HT_enable; */ + if (pmlmepriv->qospriv.qos_option) + pmlmeinfo->WMM_enable = _TRUE; +#ifdef CONFIG_80211N_HT + if (pmlmepriv->htpriv.ht_option) { + pmlmeinfo->WMM_enable = _TRUE; + pmlmeinfo->HT_enable = _TRUE; + /* pmlmeinfo->HT_info_enable = _TRUE; */ + /* pmlmeinfo->HT_caps_enable = _TRUE; */ + + update_hw_ht_param(padapter); + } +#endif /* #CONFIG_80211N_HT */ + +#ifdef CONFIG_80211AC_VHT + if (pmlmepriv->vhtpriv.vht_option) { + pmlmeinfo->VHT_enable = _TRUE; + update_hw_vht_param(padapter); + } +#endif /* CONFIG_80211AC_VHT */ + + if (pmlmepriv->cur_network.join_res != _TRUE) { /* setting only at first time */ + /* WEP Key will be set before this function, do not clear CAM. */ + if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_) + && !MLME_IS_MESH(padapter) /* mesh group key is set before this function */ + ) + flush_all_cam_entry(padapter); /* clear CAM */ + } + + /* set MSR to AP_Mode */ + Set_MSR(padapter, _HW_STATE_AP_); + + /* Set BSSID REG */ + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pnetwork->MacAddress); + + /* Set Security */ + val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + /* Beacon Control related register */ + rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval)); + + rtw_hal_rcr_set_chk_bssid(padapter, mlme_act); + +chbw_decision: + ifbmp_ch_changed = rtw_ap_chbw_decision(padapter, parm->ifbmp, parm->excl_ifbmp + , req_ch, req_bw, req_offset + , &u_ch, &u_bw, &u_offset, &chbw_allow, &set_u_ch); + + for (i = 0; i < pdvobj->iface_nums; i++) { + if (!(parm->ifbmp & BIT(i)) || !pdvobj->padapters[i]) + continue; + + /* let pnetwork_mlme == pnetwork_mlmeext */ + _rtw_memcpy(&(pdvobj->padapters[i]->mlmepriv.cur_network.network) + , &(pdvobj->padapters[i]->mlmeextpriv.mlmext_info.network) + , pdvobj->padapters[i]->mlmeextpriv.mlmext_info.network.Length); + + rtw_start_bss_hdl_after_chbw_decided(pdvobj->padapters[i]); + + /* Set EDCA param reg after update cur_wireless_mode & update_capinfo */ + if (pregpriv->wifi_spec == 1) + rtw_set_hw_wmm_param(pdvobj->padapters[i]); + } + +#if defined(CONFIG_DFS_MASTER) + rtw_dfs_rd_en_decision(padapter, mlme_act, parm->excl_ifbmp); +#endif + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) { + /* + * due to check under rtw_ap_chbw_decision + * if under MCC mode, means req channel setting is the same as current channel setting + * if not under MCC mode, mean req channel setting is not the same as current channel setting + */ + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) { + RTW_INFO(FUNC_ADPT_FMT": req channel setting is the same as current channel setting, go to update BCN\n" + , FUNC_ADPT_ARG(padapter)); + + goto update_beacon; + + } + } + + /* issue null data to AP for all interface connecting to AP before switch channel setting for softap */ + rtw_hal_mcc_issue_null_data(padapter, chbw_allow, 1); +#endif /* CONFIG_MCC_MODE */ + + if (!IS_CH_WAITING(adapter_to_rfctl(padapter))) { + doiqk = _TRUE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + } + + if (set_u_ch) + set_channel_bwmode(padapter, u_ch, u_offset, u_bw); + + doiqk = _FALSE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + +#ifdef CONFIG_MCC_MODE + /* after set_channel_bwmode for backup IQK */ + if (rtw_hal_set_mcc_setting_start_bss_network(padapter, chbw_allow) == _FAIL) { + /* MCC setting fail, update to buddy's channel */ + rtw_mi_get_ch_setting_union_no_self(padapter, &u_ch, &u_bw, &u_offset); + pnetwork->Configuration.DSConfig = u_ch; + padapter->mlmeextpriv.cur_channel = u_ch; + padapter->mlmeextpriv.cur_bwmode = u_bw; + padapter->mlmeextpriv.cur_ch_offset = u_offset; + + if (ifbmp_ch_changed == 0) { + u8 ht_option = 0; + +#ifdef CONFIG_80211N_HT + ht_option = padapter->mlmepriv.htpriv.ht_option; +#endif + + rtw_cfg80211_ch_switch_notify(padapter + , padapter->mlmeextpriv.cur_channel + , padapter->mlmeextpriv.cur_bwmode + , padapter->mlmeextpriv.cur_ch_offset + , ht_option, 0); + } + } +#endif + +#if defined(CONFIG_IOCTL_CFG80211) && (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 5, 0)) + for (i = 0; i < pdvobj->iface_nums; i++) { + if (!(ifbmp_ch_changed & BIT(i)) || !pdvobj->padapters[i]) + continue; + + { + u8 ht_option = 0; + + #ifdef CONFIG_80211N_HT + ht_option = pdvobj->padapters[i]->mlmepriv.htpriv.ht_option; + #endif + + rtw_cfg80211_ch_switch_notify(pdvobj->padapters[i] + , pdvobj->padapters[i]->mlmeextpriv.cur_channel + , pdvobj->padapters[i]->mlmeextpriv.cur_bwmode + , pdvobj->padapters[i]->mlmeextpriv.cur_ch_offset + , ht_option, 0); + } + } +#endif /* defined(CONFIG_IOCTL_CFG80211) && (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 5, 0)) */ + + rtw_rfctl_update_op_mode(adapter_to_rfctl(padapter), parm->ifbmp, 1); + + if (DUMP_ADAPTERS_STATUS) { + RTW_INFO(FUNC_ADPT_FMT" done\n", FUNC_ADPT_ARG(padapter)); + dump_adapters_status(RTW_DBGDUMP , adapter_to_dvobj(padapter)); + } + +#ifdef CONFIG_MCC_MODE +update_beacon: +#endif + + for (i = 0; i < pdvobj->iface_nums; i++) { + struct mlme_priv *mlme; + + if (!(parm->ifbmp & BIT(i)) || !pdvobj->padapters[i]) + continue; + + /* update beacon content only if bstart_bss is _TRUE */ + if (pdvobj->padapters[i]->mlmeextpriv.bstart_bss != _TRUE) + continue; + + mlme = &(pdvobj->padapters[i]->mlmepriv); + + #ifdef CONFIG_80211N_HT + if ((ATOMIC_READ(&mlme->olbc) == _TRUE) || (ATOMIC_READ(&mlme->olbc_ht) == _TRUE)) { + /* AP is not starting a 40 MHz BSS in presence of an 802.11g BSS. */ + mlme->ht_op_mode &= (~HT_INFO_OPERATION_MODE_OP_MODE_MASK); + mlme->ht_op_mode |= OP_MODE_MAY_BE_LEGACY_STAS; + update_beacon(pdvobj->padapters[i], _HT_ADD_INFO_IE_, NULL, _FALSE, 0); + } + #endif + + update_beacon(pdvobj->padapters[i], _TIM_IE_, NULL, _FALSE, 0); + } + + if (mlme_act != MLME_OPCH_SWITCH + && pmlmeext->bstart_bss == _TRUE + ) { +#ifdef CONFIG_SUPPORT_MULTI_BCN + _irqL irqL; + + _enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + if (rtw_is_list_empty(&padapter->list)) { + #ifdef CONFIG_FW_HANDLE_TXBCN + padapter->vap_id = rtw_ap_allocate_vapid(pdvobj); + #endif + rtw_list_insert_tail(&padapter->list, get_list_head(&pdvobj->ap_if_q)); + pdvobj->nr_ap_if++; + pdvobj->inter_bcn_space = DEFAULT_BCN_INTERVAL / pdvobj->nr_ap_if; + } + _exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + + #ifdef CONFIG_SWTIMER_BASED_TXBCN + rtw_ap_set_mbid_num(padapter, pdvobj->nr_ap_if); + rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pdvobj->inter_bcn_space)); + #endif /*CONFIG_SWTIMER_BASED_TXBCN*/ + +#endif /*CONFIG_SUPPORT_MULTI_BCN*/ + + #ifdef CONFIG_HW_P0_TSF_SYNC + correct_TSF(padapter, mlme_act); + #endif + } + + rtw_scan_wait_completed(padapter); + + _rtw_iface_undersurvey_chk(__func__, padapter); + /* send beacon */ + ResumeTxBeacon(padapter); + { +#if !defined(CONFIG_INTERRUPT_BASED_TXBCN) +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) || defined(CONFIG_PCI_BCN_POLLING) +#ifdef CONFIG_SWTIMER_BASED_TXBCN + if (pdvobj->nr_ap_if == 1 + && mlme_act != MLME_OPCH_SWITCH + ) { + RTW_INFO("start SW BCN TIMER!\n"); + _set_timer(&pdvobj->txbcn_timer, bcn_interval); + } +#else + for (i = 0; i < pdvobj->iface_nums; i++) { + if (!(parm->ifbmp & BIT(i)) || !pdvobj->padapters[i]) + continue; + + if (send_beacon(pdvobj->padapters[i]) == _FAIL) + RTW_INFO(ADPT_FMT" issue_beacon, fail!\n", ADPT_ARG(pdvobj->padapters[i])); + } +#endif +#endif +#endif /* !defined(CONFIG_INTERRUPT_BASED_TXBCN) */ + +#ifdef CONFIG_FW_HANDLE_TXBCN + if (mlme_act != MLME_OPCH_SWITCH + && pmlmeext->bstart_bss == _TRUE) + rtw_ap_mbid_bcn_en(padapter, padapter->vap_id); +#endif + } +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (MLME_IS_AP(padapter) && padapter->tbtx_capability == _TRUE) { + _set_timer(&pmlmeext->tbtx_token_dispatch_timer, 1); + RTW_INFO("Start token dispatch\n"); + } +#endif +} + +int rtw_check_beacon_data(_adapter *padapter, u8 *pbuf, int len) +{ + int ret = _SUCCESS; + u8 *p; + u8 *pHT_caps_ie = NULL; + u8 *pHT_info_ie = NULL; + u16 cap, ht_cap = _FALSE; + uint ie_len = 0; + int group_cipher, pairwise_cipher, gmcs; + u32 akm; + u8 mfp_opt = MFP_NO; + u8 channel, network_type; + u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01}; + u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + HT_CAP_AMPDU_DENSITY best_ampdu_density; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + WLAN_BSSID_EX *pbss_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network; + u8 *ie = pbss_network->IEs; + u8 vht_cap = _FALSE; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + u8 rf_num = 0; + int ret_rm; + /* SSID */ + /* Supported rates */ + /* DS Params */ + /* WLAN_EID_COUNTRY */ + /* ERP Information element */ + /* Extended supported rates */ + /* WPA/WPA2 */ + /* Radio Resource Management */ + /* Wi-Fi Wireless Multimedia Extensions */ + /* ht_capab, ht_oper */ + /* WPS IE */ + + RTW_INFO("%s, len=%d\n", __FUNCTION__, len); + + if (!MLME_IS_AP(padapter) && !MLME_IS_MESH(padapter)) + return _FAIL; + + + if (len > MAX_IE_SZ) + return _FAIL; + + pbss_network->IELength = len; + + _rtw_memset(ie, 0, MAX_IE_SZ); + + _rtw_memcpy(ie, pbuf, pbss_network->IELength); + + + if (pbss_network->InfrastructureMode != Ndis802_11APMode + && pbss_network->InfrastructureMode != Ndis802_11_mesh + ) { + rtw_warn_on(1); + return _FAIL; + } + + + rtw_ap_check_scan(padapter); + + + pbss_network->Rssi = 0; + + _rtw_memcpy(pbss_network->MacAddress, adapter_mac_addr(padapter), ETH_ALEN); + + /* beacon interval */ + p = rtw_get_beacon_interval_from_ie(ie);/* ie + 8; */ /* 8: TimeStamp, 2: Beacon Interval 2:Capability */ + /* pbss_network->Configuration.BeaconPeriod = le16_to_cpu(*(unsigned short*)p); */ + pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p); + + /* capability */ + /* cap = *(unsigned short *)rtw_get_capability_from_ie(ie); */ + /* cap = le16_to_cpu(cap); */ + cap = RTW_GET_LE16(ie); + + /* SSID */ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + _rtw_memset(&pbss_network->Ssid, 0, sizeof(NDIS_802_11_SSID)); + _rtw_memcpy(pbss_network->Ssid.Ssid, (p + 2), ie_len); + pbss_network->Ssid.SsidLength = ie_len; +#ifdef CONFIG_P2P + _rtw_memcpy(padapter->wdinfo.p2p_group_ssid, pbss_network->Ssid.Ssid, pbss_network->Ssid.SsidLength); + padapter->wdinfo.p2p_group_ssid_len = pbss_network->Ssid.SsidLength; +#endif + } + +#ifdef CONFIG_RTW_MESH + /* Mesh ID */ + if (MLME_IS_MESH(padapter)) { + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, WLAN_EID_MESH_ID, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + _rtw_memset(&pbss_network->mesh_id, 0, sizeof(NDIS_802_11_SSID)); + _rtw_memcpy(pbss_network->mesh_id.Ssid, (p + 2), ie_len); + pbss_network->mesh_id.SsidLength = ie_len; + } + } +#endif + + /* chnnel */ + channel = 0; + pbss_network->Configuration.Length = 0; + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) + channel = *(p + 2); + + pbss_network->Configuration.DSConfig = channel; + + /* support rate ie & ext support ie & IElen & SupportedRates */ + network_type = rtw_update_rate_bymode(pbss_network, pregistrypriv->wireless_mode); + + /* parsing ERP_IE */ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + if(padapter->registrypriv.wireless_mode == WIRELESS_11B ) { + + pbss_network->IELength = pbss_network->IELength - *(p+1) - 2; + ret_rm = rtw_ies_remove_ie(ie , &len, _BEACON_IE_OFFSET_, _ERPINFO_IE_,NULL,0); + RTW_DBG("%s, remove_ie of ERP_IE=%d\n", __FUNCTION__, ret_rm); + } else + ERP_IE_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)p); + + } + + /* update privacy/security */ + if (cap & BIT(4)) + pbss_network->Privacy = 1; + else + pbss_network->Privacy = 0; + + psecuritypriv->wpa_psk = 0; + + /* wpa2 */ + akm = 0; + gmcs = 0; + group_cipher = 0; + pairwise_cipher = 0; + psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_; + psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_; + psecuritypriv->akmp = 0; + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + if (rtw_parse_wpa2_ie(p, ie_len + 2, &group_cipher, &pairwise_cipher, &gmcs, &akm, &mfp_opt, NULL) == _SUCCESS) { + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + psecuritypriv->ndisauthtype = Ndis802_11AuthModeWPA2PSK; + psecuritypriv->dot8021xalg = 1;/* psk, todo:802.1x */ + psecuritypriv->wpa_psk |= BIT(1); + + psecuritypriv->wpa2_group_cipher = group_cipher; + psecuritypriv->wpa2_pairwise_cipher = pairwise_cipher; + psecuritypriv->akmp = akm; + +#ifdef CONFIG_IOCTL_CFG80211 + /** + * Kernel < v5.x, the auth_type set as + * NL80211_AUTHTYPE_AUTOMATIC in + * cfg80211_rtw_start_ap(). if the AKM SAE in the RSN + * IE, we have to update the auth_type for SAE in + * rtw_check_beacon_data() + */ + if (CHECK_BIT(WLAN_AKM_TYPE_SAE, akm)) { + RTW_INFO("%s: Auth type as SAE\n", __func__); + psecuritypriv->auth_type = MLME_AUTHTYPE_SAE; + psecuritypriv->auth_alg = WLAN_AUTH_SAE; + } +#endif /* CONFIG_IOCTL_CFG80211 */ +#if 0 + switch (group_cipher) { + case WPA_CIPHER_NONE: + psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_; + break; + case WPA_CIPHER_WEP40: + psecuritypriv->wpa2_group_cipher = _WEP40_; + break; + case WPA_CIPHER_TKIP: + psecuritypriv->wpa2_group_cipher = _TKIP_; + break; + case WPA_CIPHER_CCMP: + psecuritypriv->wpa2_group_cipher = _AES_; + break; + case WPA_CIPHER_WEP104: + psecuritypriv->wpa2_group_cipher = _WEP104_; + break; + } + + switch (pairwise_cipher) { + case WPA_CIPHER_NONE: + psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_; + break; + case WPA_CIPHER_WEP40: + psecuritypriv->wpa2_pairwise_cipher = _WEP40_; + break; + case WPA_CIPHER_TKIP: + psecuritypriv->wpa2_pairwise_cipher = _TKIP_; + break; + case WPA_CIPHER_CCMP: + psecuritypriv->wpa2_pairwise_cipher = _AES_; + break; + case WPA_CIPHER_WEP104: + psecuritypriv->wpa2_pairwise_cipher = _WEP104_; + break; + } +#endif + } + + } + + /* wpa */ + ie_len = 0; + group_cipher = 0; + pairwise_cipher = 0; + psecuritypriv->wpa_group_cipher = _NO_PRIVACY_; + psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_; + for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) { + p = rtw_get_ie(p, _SSN_IE_1_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2))); + if ((p) && (_rtw_memcmp(p + 2, OUI1, 4))) { + if (rtw_parse_wpa_ie(p, ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) { + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + psecuritypriv->ndisauthtype = Ndis802_11AuthModeWPAPSK; + psecuritypriv->dot8021xalg = 1;/* psk, todo:802.1x */ + + psecuritypriv->wpa_psk |= BIT(0); + + psecuritypriv->wpa_group_cipher = group_cipher; + psecuritypriv->wpa_pairwise_cipher = pairwise_cipher; + +#if 0 + switch (group_cipher) { + case WPA_CIPHER_NONE: + psecuritypriv->wpa_group_cipher = _NO_PRIVACY_; + break; + case WPA_CIPHER_WEP40: + psecuritypriv->wpa_group_cipher = _WEP40_; + break; + case WPA_CIPHER_TKIP: + psecuritypriv->wpa_group_cipher = _TKIP_; + break; + case WPA_CIPHER_CCMP: + psecuritypriv->wpa_group_cipher = _AES_; + break; + case WPA_CIPHER_WEP104: + psecuritypriv->wpa_group_cipher = _WEP104_; + break; + } + + switch (pairwise_cipher) { + case WPA_CIPHER_NONE: + psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_; + break; + case WPA_CIPHER_WEP40: + psecuritypriv->wpa_pairwise_cipher = _WEP40_; + break; + case WPA_CIPHER_TKIP: + psecuritypriv->wpa_pairwise_cipher = _TKIP_; + break; + case WPA_CIPHER_CCMP: + psecuritypriv->wpa_pairwise_cipher = _AES_; + break; + case WPA_CIPHER_WEP104: + psecuritypriv->wpa_pairwise_cipher = _WEP104_; + break; + } +#endif + } + + break; + + } + + if ((p == NULL) || (ie_len == 0)) + break; + + } + + if (mfp_opt == MFP_INVALID) { + RTW_INFO(FUNC_ADPT_FMT" invalid MFP setting\n", FUNC_ADPT_ARG(padapter)); + return _FAIL; + } + psecuritypriv->mfp_opt = mfp_opt; + +#ifdef CONFIG_RTW_80211K + /* RRM */ + update_rm_cap(pbuf, padapter, len, _BEACON_IE_OFFSET_); + +#endif /* CONFIG_RTW_80211K */ + + /* wmm */ + ie_len = 0; + pmlmepriv->qospriv.qos_option = 0; +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + pmlmepriv->qospriv.qos_option = 1; +#endif + if (pregistrypriv->wmm_enable) { + for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) { + p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2))); + if ((p) && _rtw_memcmp(p + 2, WMM_PARA_IE, 6)) { + pmlmepriv->qospriv.qos_option = 1; + + *(p + 8) |= BIT(7); /* QoS Info, support U-APSD */ + + /* disable all ACM bits since the WMM admission control is not supported */ + *(p + 10) &= ~BIT(4); /* BE */ + *(p + 14) &= ~BIT(4); /* BK */ + *(p + 18) &= ~BIT(4); /* VI */ + *(p + 22) &= ~BIT(4); /* VO */ + + WMM_param_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)p); + + break; + } + + if ((p == NULL) || (ie_len == 0)) + break; + } + } +#ifdef CONFIG_80211N_HT + if(padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode)) { + /* parsing HT_CAP_IE */ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor = MAX_AMPDU_FACTOR_64K; + struct rtw_ieee80211_ht_cap *pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2); + +#ifdef CONFIG_RTW_DEBUG + if (0) { + RTW_INFO(FUNC_ADPT_FMT" HT_CAP_IE from upper layer:\n", FUNC_ADPT_ARG(padapter)); + dump_ht_cap_ie_content(RTW_DBGDUMP, p + 2, ie_len); + } +#endif /* CONFIG_RTW_DEBUG */ + pHT_caps_ie = p; + + ht_cap = _TRUE; + network_type |= WIRELESS_11_24N; + + rtw_ht_use_default_setting(padapter); + + /* Update HT Capabilities Info field */ + if (pmlmepriv->htpriv.sgi_20m == _FALSE) + pht_cap->cap_info &= ~(IEEE80211_HT_CAP_SGI_20); + + if (pmlmepriv->htpriv.sgi_40m == _FALSE) + pht_cap->cap_info &= ~(IEEE80211_HT_CAP_SGI_40); + + if (!TEST_FLAG(pmlmepriv->htpriv.ldpc_cap, LDPC_HT_ENABLE_RX)) + pht_cap->cap_info &= ~(IEEE80211_HT_CAP_LDPC_CODING); + + if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_TX)) + pht_cap->cap_info &= ~(IEEE80211_HT_CAP_TX_STBC); + + if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_RX)) + pht_cap->cap_info &= ~(IEEE80211_HT_CAP_RX_STBC_3R); + + /* Update A-MPDU Parameters field */ + pht_cap->ampdu_params_info &= ~(IEEE80211_HT_CAP_AMPDU_FACTOR | IEEE80211_HT_CAP_AMPDU_DENSITY); + + if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) || + (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP)) { + rtw_hal_get_def_var(padapter, HW_VAR_BEST_AMPDU_DENSITY, &best_ampdu_density); + pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & (best_ampdu_density << 2)); + } else + pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & 0x00); + + rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); + pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_FACTOR & max_rx_ampdu_factor); /* set Max Rx AMPDU size to 64K */ + + _rtw_memcpy(&(pmlmeinfo->HT_caps), pht_cap, sizeof(struct HT_caps_element)); + + /* Update Supported MCS Set field */ + { + u8 rx_nss = 0; + int i; + + rx_nss = GET_HAL_RX_NSS(padapter); + + /* RX MCS Bitmask */ + switch (rx_nss) { + case 1: + set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_1R); + break; + case 2: + set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_2R); + break; + case 3: + set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_3R); + break; + case 4: + set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_4R); + break; + default: + RTW_WARN("rf_type:%d or rx_nss:%u is not expected\n", GET_HAL_RFPATH(padapter), rx_nss); + } + for (i = 0; i < 10; i++) + *(HT_CAP_ELE_RX_MCS_MAP(pht_cap) + i) &= padapter->mlmeextpriv.default_supported_mcs_set[i]; + } + +#ifdef CONFIG_BEAMFORMING + /* Use registry value to enable HT Beamforming. */ + /* ToDo: use configure file to set these capability. */ + pht_cap->tx_BF_cap_info = 0; + + /* HT Beamformer */ + if (TEST_FLAG(pmlmepriv->htpriv.beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) { + /* Transmit NDP Capable */ + SET_HT_CAP_TXBF_TRANSMIT_NDP_CAP(pht_cap, 1); + /* Explicit Compressed Steering Capable */ + SET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pht_cap, 1); + /* Compressed Steering Number Antennas */ + SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pht_cap, 1); + rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMER_CAP, (u8 *)&rf_num); + SET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pht_cap, rf_num); + } + + /* HT Beamformee */ + if (TEST_FLAG(pmlmepriv->htpriv.beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) { + /* Receive NDP Capable */ + SET_HT_CAP_TXBF_RECEIVE_NDP_CAP(pht_cap, 1); + /* Explicit Compressed Beamforming Feedback Capable */ + SET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pht_cap, 2); + rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMEE_CAP, (u8 *)&rf_num); + SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pht_cap, rf_num); + } +#endif /* CONFIG_BEAMFORMING */ + + _rtw_memcpy(&pmlmepriv->htpriv.ht_cap, p + 2, ie_len); +#ifdef CONFIG_RTW_DEBUG + if (0) { + RTW_INFO(FUNC_ADPT_FMT" HT_CAP_IE driver masked:\n", FUNC_ADPT_ARG(padapter)); + dump_ht_cap_ie_content(RTW_DBGDUMP, p + 2, ie_len); + } +#endif /* CONFIG_RTW_DEBUG */ + } + + /* parsing HT_INFO_IE */ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) { + pHT_info_ie = p; + if (channel == 0) + pbss_network->Configuration.DSConfig = GET_HT_OP_ELE_PRI_CHL(pHT_info_ie + 2); + else if (channel != GET_HT_OP_ELE_PRI_CHL(pHT_info_ie + 2)) { + RTW_INFO(FUNC_ADPT_FMT" ch inconsistent, DSSS:%u, HT primary:%u\n" + , FUNC_ADPT_ARG(padapter), channel, GET_HT_OP_ELE_PRI_CHL(pHT_info_ie + 2)); + } + } + } +#endif /* CONFIG_80211N_HT */ + pmlmepriv->cur_network.network_type = network_type; + +#ifdef CONFIG_80211N_HT + pmlmepriv->htpriv.ht_option = _FALSE; + + if ((psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_TKIP) || + (psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) { + /* todo: */ + /* ht_cap = _FALSE; */ + } + + /* ht_cap */ + if (padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode) && ht_cap == _TRUE) { + + pmlmepriv->htpriv.ht_option = _TRUE; + pmlmepriv->qospriv.qos_option = 1; + + pmlmepriv->htpriv.ampdu_enable = pregistrypriv->ampdu_enable ? _TRUE : _FALSE; + + HT_caps_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)pHT_caps_ie); + + HT_info_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)pHT_info_ie); + } +#endif + +#ifdef CONFIG_80211AC_VHT + pmlmepriv->ori_vht_en = 0; + pmlmepriv->vhtpriv.vht_option = _FALSE; + + if (pmlmepriv->htpriv.ht_option == _TRUE + && pbss_network->Configuration.DSConfig > 14 + && REGSTY_IS_11AC_ENABLE(pregistrypriv) + && is_supported_vht(pregistrypriv->wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + /* Parsing VHT CAP IE */ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTCapability, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) + vht_cap = _TRUE; + + /* Parsing VHT OPERATION IE */ + + if (vht_cap == _TRUE + && MLME_IS_MESH(padapter) /* allow only mesh temporarily before VHT IE checking is ready */ + ) { + rtw_check_for_vht20(padapter, ie + _BEACON_IE_OFFSET_, pbss_network->IELength - _BEACON_IE_OFFSET_); + pmlmepriv->ori_vht_en = 1; + pmlmepriv->vhtpriv.vht_option = _TRUE; + } else if (REGSTY_IS_11AC_AUTO(pregistrypriv)) { + rtw_vht_ies_detach(padapter, pbss_network); + rtw_vht_ies_attach(padapter, pbss_network); + } + } + + if (pmlmepriv->vhtpriv.vht_option == _FALSE) + rtw_vht_ies_detach(padapter, pbss_network); +#endif /* CONFIG_80211AC_VHT */ + +#ifdef CONFIG_80211N_HT + if(padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode) && + pbss_network->Configuration.DSConfig <= 14 && padapter->registrypriv.wifi_spec == 1 && + pbss_network->IELength + 10 <= MAX_IE_SZ) { + uint len = 0; + + SET_EXT_CAPABILITY_ELE_BSS_COEXIST(pmlmepriv->ext_capab_ie_data, 1); + pmlmepriv->ext_capab_ie_len = 10; + rtw_set_ie(pbss_network->IEs + pbss_network->IELength, EID_EXTCapability, 8, pmlmepriv->ext_capab_ie_data, &len); + pbss_network->IELength += pmlmepriv->ext_capab_ie_len; + } +#endif /* CONFIG_80211N_HT */ + + pbss_network->Length = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX *)pbss_network); + + rtw_ies_get_chbw(pbss_network->IEs + _BEACON_IE_OFFSET_, pbss_network->IELength - _BEACON_IE_OFFSET_ + , &pmlmepriv->ori_ch, &pmlmepriv->ori_bw, &pmlmepriv->ori_offset, 1, 1); + rtw_warn_on(pmlmepriv->ori_ch == 0); + + { + /* alloc sta_info for ap itself */ + + struct sta_info *sta; + + sta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress); + if (!sta) { + sta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress); + if (sta == NULL) + return _FAIL; + } + } + + rtw_startbss_cmd(padapter, RTW_CMDF_WAIT_ACK); + { + int sk_band = RTW_GET_SCAN_BAND_SKIP(padapter); + + if (sk_band) + RTW_CLR_SCAN_BAND_SKIP(padapter, sk_band); + } + + rtw_indicate_connect(padapter); + + pmlmepriv->cur_network.join_res = _TRUE;/* for check if already set beacon */ + + /* update bc/mc sta_info */ + /* update_bmc_sta(padapter); */ + + return ret; + +} + +#if CONFIG_RTW_MACADDR_ACL +void rtw_macaddr_acl_init(_adapter *adapter, u8 period) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + _queue *acl_node_q; + int i; + _irqL irqL; + + if (period >= RTW_ACL_PERIOD_NUM) { + rtw_warn_on(1); + return; + } + + acl = &stapriv->acl_list[period]; + acl_node_q = &acl->acl_node_q; + + _rtw_spinlock_init(&(acl_node_q->lock)); + + _enter_critical_bh(&(acl_node_q->lock), &irqL); + _rtw_init_listhead(&(acl_node_q->queue)); + acl->num = 0; + acl->mode = RTW_ACL_MODE_DISABLED; + for (i = 0; i < NUM_ACL; i++) { + _rtw_init_listhead(&acl->aclnode[i].list); + acl->aclnode[i].valid = _FALSE; + } + _exit_critical_bh(&(acl_node_q->lock), &irqL); +} + +static void _rtw_macaddr_acl_deinit(_adapter *adapter, u8 period, bool clear_only) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + _queue *acl_node_q; + _irqL irqL; + _list *head, *list; + struct rtw_wlan_acl_node *acl_node; + + if (period >= RTW_ACL_PERIOD_NUM) { + rtw_warn_on(1); + return; + } + + acl = &stapriv->acl_list[period]; + acl_node_q = &acl->acl_node_q; + + _enter_critical_bh(&(acl_node_q->lock), &irqL); + head = get_list_head(acl_node_q); + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list); + list = get_next(list); + + if (acl_node->valid == _TRUE) { + acl_node->valid = _FALSE; + rtw_list_delete(&acl_node->list); + acl->num--; + } + } + _exit_critical_bh(&(acl_node_q->lock), &irqL); + + if (!clear_only) + _rtw_spinlock_free(&(acl_node_q->lock)); + + rtw_warn_on(acl->num); + acl->mode = RTW_ACL_MODE_DISABLED; +} + +void rtw_macaddr_acl_deinit(_adapter *adapter, u8 period) +{ + _rtw_macaddr_acl_deinit(adapter, period, 0); +} + +void rtw_macaddr_acl_clear(_adapter *adapter, u8 period) +{ + _rtw_macaddr_acl_deinit(adapter, period, 1); +} + +void rtw_set_macaddr_acl(_adapter *adapter, u8 period, int mode) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + + if (period >= RTW_ACL_PERIOD_NUM) { + rtw_warn_on(1); + return; + } + + acl = &stapriv->acl_list[period]; + + RTW_INFO(FUNC_ADPT_FMT" p=%u, mode=%d\n" + , FUNC_ADPT_ARG(adapter), period, mode); + + acl->mode = mode; +} + +int rtw_acl_add_sta(_adapter *adapter, u8 period, const u8 *addr) +{ + _irqL irqL; + _list *list, *head; + u8 existed = 0; + int i = -1, ret = 0; + struct rtw_wlan_acl_node *acl_node; + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + _queue *acl_node_q; + + if (period >= RTW_ACL_PERIOD_NUM) { + rtw_warn_on(1); + ret = -1; + goto exit; + } + + acl = &stapriv->acl_list[period]; + acl_node_q = &acl->acl_node_q; + + _enter_critical_bh(&(acl_node_q->lock), &irqL); + + head = get_list_head(acl_node_q); + list = get_next(head); + + /* search for existed entry */ + while (rtw_end_of_queue_search(head, list) == _FALSE) { + acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list); + list = get_next(list); + + if (_rtw_memcmp(acl_node->addr, addr, ETH_ALEN)) { + if (acl_node->valid == _TRUE) { + existed = 1; + break; + } + } + } + if (existed) + goto release_lock; + + if (acl->num >= NUM_ACL) + goto release_lock; + + /* find empty one and use */ + for (i = 0; i < NUM_ACL; i++) { + + acl_node = &acl->aclnode[i]; + if (acl_node->valid == _FALSE) { + + _rtw_init_listhead(&acl_node->list); + _rtw_memcpy(acl_node->addr, addr, ETH_ALEN); + acl_node->valid = _TRUE; + + rtw_list_insert_tail(&acl_node->list, get_list_head(acl_node_q)); + acl->num++; + break; + } + } + +release_lock: + _exit_critical_bh(&(acl_node_q->lock), &irqL); + + if (!existed && (i < 0 || i >= NUM_ACL)) + ret = -1; + + RTW_INFO(FUNC_ADPT_FMT" p=%u "MAC_FMT" %s (acl_num=%d)\n" + , FUNC_ADPT_ARG(adapter), period, MAC_ARG(addr) + , (existed ? "existed" : ((i < 0 || i >= NUM_ACL) ? "no room" : "added")) + , acl->num); +exit: + return ret; +} + +int rtw_acl_remove_sta(_adapter *adapter, u8 period, const u8 *addr) +{ + _irqL irqL; + _list *list, *head; + int ret = 0; + struct rtw_wlan_acl_node *acl_node; + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + _queue *acl_node_q; + u8 is_baddr = is_broadcast_mac_addr(addr); + u8 match = 0; + + if (period >= RTW_ACL_PERIOD_NUM) { + rtw_warn_on(1); + goto exit; + } + + acl = &stapriv->acl_list[period]; + acl_node_q = &acl->acl_node_q; + + _enter_critical_bh(&(acl_node_q->lock), &irqL); + + head = get_list_head(acl_node_q); + list = get_next(head); + + while (rtw_end_of_queue_search(head, list) == _FALSE) { + acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list); + list = get_next(list); + + if (is_baddr || _rtw_memcmp(acl_node->addr, addr, ETH_ALEN)) { + if (acl_node->valid == _TRUE) { + acl_node->valid = _FALSE; + rtw_list_delete(&acl_node->list); + acl->num--; + match = 1; + } + } + } + + _exit_critical_bh(&(acl_node_q->lock), &irqL); + + RTW_INFO(FUNC_ADPT_FMT" p=%u "MAC_FMT" %s (acl_num=%d)\n" + , FUNC_ADPT_ARG(adapter), period, MAC_ARG(addr) + , is_baddr ? "clear all" : (match ? "match" : "no found") + , acl->num); + +exit: + return ret; +} +#endif /* CONFIG_RTW_MACADDR_ACL */ + +u8 rtw_ap_set_sta_key(_adapter *adapter, const u8 *addr, u8 alg, const u8 *key, u8 keyid, u8 gk) +{ + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + struct cmd_obj *cmd; + struct set_stakey_parm *param; + u8 res = _SUCCESS; + + cmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmd == NULL) { + res = _FAIL; + goto exit; + } + + param = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm)); + if (param == NULL) { + rtw_mfree((u8 *) cmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmd, param, CMD_SET_STAKEY); + + _rtw_memcpy(param->addr, addr, ETH_ALEN); + param->algorithm = alg; + param->keyid = keyid; + if (!!(alg & _SEC_TYPE_256_)) + _rtw_memcpy(param->key, key, 32); + else + _rtw_memcpy(param->key, key, 16); + param->gk = gk; + + res = rtw_enqueue_cmd(cmdpriv, cmd); + +exit: + return res; +} + +u8 rtw_ap_set_pairwise_key(_adapter *padapter, struct sta_info *psta) +{ + return rtw_ap_set_sta_key(padapter + , psta->cmn.mac_addr + , psta->dot118021XPrivacy + , psta->dot118021x_UncstKey.skey + , 0 + , 0 + ); +} + +static int rtw_ap_set_key(_adapter *padapter, u8 *key, u8 alg, int keyid, u8 set_tx) +{ + u8 keylen; + struct cmd_obj *pcmd; + struct setkey_parm *psetkeyparm; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + int res = _SUCCESS; + + /* RTW_INFO("%s\n", __FUNCTION__); */ + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } + psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm)); + if (psetkeyparm == NULL) { + rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + _rtw_memset(psetkeyparm, 0, sizeof(struct setkey_parm)); + + psetkeyparm->keyid = (u8)keyid; + if (is_wep_enc(alg)) + padapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid); + + psetkeyparm->algorithm = alg; + + psetkeyparm->set_tx = set_tx; + + switch (alg) { + case _WEP40_: + keylen = 5; + break; + case _WEP104_: + keylen = 13; + break; + case _GCMP_256_: + case _CCMP_256_: + keylen = 32; + break; + case _TKIP_: + case _TKIP_WTMIC_: + case _AES_: + case _GCMP_: + default: + keylen = 16; + } + + _rtw_memcpy(&(psetkeyparm->key[0]), key, keylen); + + pcmd->cmdcode = CMD_SET_KEY; /*_SetKey_CMD_;*/ + pcmd->parmbuf = (u8 *)psetkeyparm; + pcmd->cmdsz = (sizeof(struct setkey_parm)); + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + + _rtw_init_listhead(&pcmd->list); + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + + return res; +} + +int rtw_ap_set_group_key(_adapter *padapter, u8 *key, u8 alg, int keyid) +{ + RTW_INFO("%s\n", __FUNCTION__); + + return rtw_ap_set_key(padapter, key, alg, keyid, 1); +} + +int rtw_ap_set_wep_key(_adapter *padapter, u8 *key, u8 keylen, int keyid, u8 set_tx) +{ + u8 alg; + + switch (keylen) { + case 5: + alg = _WEP40_; + break; + case 13: + alg = _WEP104_; + break; + default: + alg = _NO_PRIVACY_; + } + + RTW_INFO("%s\n", __FUNCTION__); + + return rtw_ap_set_key(padapter, key, alg, keyid, set_tx); +} + +u8 rtw_ap_bmc_frames_hdl(_adapter *padapter) +{ +#define HIQ_XMIT_COUNTS (6) + _irqL irqL; + struct sta_info *psta_bmc; + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + bool update_tim = _FALSE; + + + if (padapter->registrypriv.wifi_spec != 1) + return H2C_SUCCESS; + + + psta_bmc = rtw_get_bcmc_stainfo(padapter); + if (!psta_bmc) + return H2C_SUCCESS; + + + _enter_critical_bh(&pxmitpriv->lock, &irqL); + + if ((rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, 0)) && (psta_bmc->sleepq_len > 0)) { + int tx_counts = 0; + + _update_beacon(padapter, _TIM_IE_, NULL, _FALSE, 0, "update TIM with TIB=1"); + + RTW_INFO("sleepq_len of bmc_sta = %d\n", psta_bmc->sleepq_len); + + xmitframe_phead = get_list_head(&psta_bmc->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + rtw_list_delete(&pxmitframe->list); + + psta_bmc->sleepq_len--; + tx_counts++; + + if (psta_bmc->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + if (tx_counts == HIQ_XMIT_COUNTS) + pxmitframe->attrib.mdata = 0; + + pxmitframe->attrib.triggered = 1; + + if (xmitframe_hiq_filter(pxmitframe) == _TRUE) + pxmitframe->attrib.qsel = QSLT_HIGH;/*HIQ*/ + + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + if (tx_counts == HIQ_XMIT_COUNTS) + break; + + } + + } else { + if (psta_bmc->sleepq_len == 0) { + + /*RTW_INFO("sleepq_len of bmc_sta = %d\n", psta_bmc->sleepq_len);*/ + + if (rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, 0)) + update_tim = _TRUE; + + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, 0); + rtw_tim_map_clear(padapter, pstapriv->sta_dz_bitmap, 0); + + if (update_tim == _TRUE) { + RTW_INFO("clear TIB\n"); + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "bmc sleepq and HIQ empty"); + } + } + } + + _exit_critical_bh(&pxmitpriv->lock, &irqL); + +#if 0 + /* HIQ Check */ + rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty); + + while (_FALSE == empty && rtw_get_passing_time_ms(start) < 3000) { + rtw_msleep_os(100); + rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty); + } + + + printk("check if hiq empty=%d\n", empty); +#endif + + return H2C_SUCCESS; +} + +#ifdef CONFIG_NATIVEAP_MLME + +static void associated_stainfo_update(_adapter *padapter, struct sta_info *psta, u32 sta_info_type) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + RTW_INFO("%s: "MAC_FMT", updated_type=0x%x\n", __func__, MAC_ARG(psta->cmn.mac_addr), sta_info_type); +#ifdef CONFIG_80211N_HT + if (sta_info_type & STA_INFO_UPDATE_BW) { + + if ((psta->flags & WLAN_STA_HT) && !psta->ht_20mhz_set) { + if (pmlmepriv->sw_to_20mhz) { + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + /*psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;*/ + psta->htpriv.sgi_40m = _FALSE; + } else { + /*TODO: Switch back to 40MHZ?80MHZ*/ + } + } + } +#endif /* CONFIG_80211N_HT */ + /* + if (sta_info_type & STA_INFO_UPDATE_RATE) { + + } + */ + + if (sta_info_type & STA_INFO_UPDATE_PROTECTION_MODE) + VCS_update(padapter, psta); + + /* + if (sta_info_type & STA_INFO_UPDATE_CAP) { + + } + + if (sta_info_type & STA_INFO_UPDATE_HT_CAP) { + + } + + if (sta_info_type & STA_INFO_UPDATE_VHT_CAP) { + + } + */ + +} + +static void update_bcn_ext_capab_ie(_adapter *padapter) +{ + sint ie_len = 0; + unsigned char *pbuf; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network); + u8 *ie = pnetwork->IEs; + u8 null_extcap_data[8] = {0}; + + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_CAP_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) + rtw_remove_bcn_ie(padapter, pnetwork, _EXT_CAP_IE_); + + if ((pmlmepriv->ext_capab_ie_len > 0) && + (_rtw_memcmp(pmlmepriv->ext_capab_ie_data, null_extcap_data, sizeof(null_extcap_data)) == _FALSE)) + rtw_add_bcn_ie(padapter, pnetwork, _EXT_CAP_IE_, pmlmepriv->ext_capab_ie_data, pmlmepriv->ext_capab_ie_len); + +} + +static void update_bcn_erpinfo_ie(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network); + unsigned char *p, *ie = pnetwork->IEs; + u32 len = 0; + + RTW_INFO("%s, ERP_enable=%d\n", __FUNCTION__, pmlmeinfo->ERP_enable); + + if (!pmlmeinfo->ERP_enable) + return; + + /* parsing ERP_IE */ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (p && len > 0) { + PNDIS_802_11_VARIABLE_IEs pIE = (PNDIS_802_11_VARIABLE_IEs)p; + + if (pmlmepriv->num_sta_non_erp == 1) + pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION; + else + pIE->data[0] &= ~(RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION); + + if (pmlmepriv->num_sta_no_short_preamble > 0) + pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE; + else + pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE); + + ERP_IE_handler(padapter, pIE); + } + +} + +static void update_bcn_htcap_ie(_adapter *padapter) +{ + RTW_INFO("%s\n", __FUNCTION__); + +} + +static void update_bcn_htinfo_ie(_adapter *padapter) +{ +#ifdef CONFIG_80211N_HT + /* + u8 beacon_updated = _FALSE; + u32 sta_info_update_type = STA_INFO_UPDATE_NONE; + */ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network); + unsigned char *p, *ie = pnetwork->IEs; + u32 len = 0; + + if (pmlmepriv->htpriv.ht_option == _FALSE) + return; + + if (pmlmeinfo->HT_info_enable != 1) + return; + + + RTW_INFO("%s current operation mode=0x%X\n", + __FUNCTION__, pmlmepriv->ht_op_mode); + + RTW_INFO("num_sta_40mhz_intolerant(%d), 20mhz_width_req(%d), intolerant_ch_rpt(%d), olbc(%d)\n", + pmlmepriv->num_sta_40mhz_intolerant, pmlmepriv->ht_20mhz_width_req, pmlmepriv->ht_intolerant_ch_reported, ATOMIC_READ(&pmlmepriv->olbc)); + + /*parsing HT_INFO_IE, currently only update ht_op_mode - pht_info->infos[1] & pht_info->infos[2] for wifi logo test*/ + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (p && len > 0) { + struct HT_info_element *pht_info = NULL; + + pht_info = (struct HT_info_element *)(p + 2); + + /* for STA Channel Width/Secondary Channel Offset*/ + if ((pmlmepriv->sw_to_20mhz == 0) && (pmlmeext->cur_channel <= 14)) { + if ((pmlmepriv->num_sta_40mhz_intolerant > 0) || (pmlmepriv->ht_20mhz_width_req == _TRUE) + || (pmlmepriv->ht_intolerant_ch_reported == _TRUE) || (ATOMIC_READ(&pmlmepriv->olbc) == _TRUE)) { + SET_HT_OP_ELE_2ND_CHL_OFFSET(pht_info, 0); + SET_HT_OP_ELE_STA_CHL_WIDTH(pht_info, 0); + + pmlmepriv->sw_to_20mhz = 1; + /* + sta_info_update_type |= STA_INFO_UPDATE_BW; + beacon_updated = _TRUE; + */ + + RTW_INFO("%s:switching to 20Mhz\n", __FUNCTION__); + + /*TODO : cur_bwmode/cur_ch_offset switches to 20Mhz*/ + } + } else { + + if ((pmlmepriv->num_sta_40mhz_intolerant == 0) && (pmlmepriv->ht_20mhz_width_req == _FALSE) + && (pmlmepriv->ht_intolerant_ch_reported == _FALSE) && (ATOMIC_READ(&pmlmepriv->olbc) == _FALSE)) { + + if (pmlmeext->cur_bwmode >= CHANNEL_WIDTH_40) { + + SET_HT_OP_ELE_STA_CHL_WIDTH(pht_info, 1); + + SET_HT_OP_ELE_2ND_CHL_OFFSET(pht_info, + (pmlmeext->cur_ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER) ? + HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE : HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW); + + pmlmepriv->sw_to_20mhz = 0; + /* + sta_info_update_type |= STA_INFO_UPDATE_BW; + beacon_updated = _TRUE; + */ + + RTW_INFO("%s:switching back to 40Mhz\n", __FUNCTION__); + } + } + } + + /* to update ht_op_mode*/ + *(u16 *)(pht_info->infos + 1) = cpu_to_le16(pmlmepriv->ht_op_mode); + + } + + /*associated_clients_update(padapter, beacon_updated, sta_info_update_type);*/ +#endif /* CONFIG_80211N_HT */ +} + +static void update_bcn_rsn_ie(_adapter *padapter) +{ + RTW_INFO("%s\n", __FUNCTION__); + +} + +static void update_bcn_wpa_ie(_adapter *padapter) +{ + RTW_INFO("%s\n", __FUNCTION__); + +} + +static void update_bcn_wmm_ie(_adapter *padapter) +{ + RTW_INFO("%s\n", __FUNCTION__); + +} + +static void update_bcn_wps_ie(_adapter *padapter) +{ + u8 *pwps_ie = NULL, *pwps_ie_src, *premainder_ie, *pbackup_remainder_ie = NULL; + uint wps_ielen = 0, wps_offset, remainder_ielen; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network); + unsigned char *ie = pnetwork->IEs; + u32 ielen = pnetwork->IELength; + + + RTW_INFO("%s\n", __FUNCTION__); + + pwps_ie = rtw_get_wps_ie(ie + _FIXED_IE_LENGTH_, ielen - _FIXED_IE_LENGTH_, NULL, &wps_ielen); + + if (pwps_ie == NULL || wps_ielen == 0) + return; + + pwps_ie_src = pmlmepriv->wps_beacon_ie; + if (pwps_ie_src == NULL) + return; + + wps_offset = (uint)(pwps_ie - ie); + + premainder_ie = pwps_ie + wps_ielen; + + remainder_ielen = ielen - wps_offset - wps_ielen; + + if (remainder_ielen > 0) { + pbackup_remainder_ie = rtw_malloc(remainder_ielen); + if (pbackup_remainder_ie) + _rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen); + } + + wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */ + if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) { + _rtw_memcpy(pwps_ie, pwps_ie_src, wps_ielen + 2); + pwps_ie += (wps_ielen + 2); + + if (pbackup_remainder_ie) + _rtw_memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen); + + /* update IELength */ + pnetwork->IELength = wps_offset + (wps_ielen + 2) + remainder_ielen; + } + + if (pbackup_remainder_ie) + rtw_mfree(pbackup_remainder_ie, remainder_ielen); + + /* deal with the case without set_tx_beacon_cmd() in update_beacon() */ +#if defined(CONFIG_INTERRUPT_BASED_TXBCN) || defined(CONFIG_PCI_HCI) + if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) { + u8 sr = 0; + rtw_get_wps_attr_content(pwps_ie_src, wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL); + + if (sr) { + set_fwstate(pmlmepriv, WIFI_UNDER_WPS); + RTW_INFO("%s, set WIFI_UNDER_WPS\n", __func__); + } else { + clr_fwstate(pmlmepriv, WIFI_UNDER_WPS); + RTW_INFO("%s, clr WIFI_UNDER_WPS\n", __func__); + } + } +#endif +} + +static void update_bcn_p2p_ie(_adapter *padapter) +{ + +} + +static void update_bcn_vendor_spec_ie(_adapter *padapter, u8 *oui) +{ + RTW_INFO("%s\n", __FUNCTION__); + + if (_rtw_memcmp(RTW_WPA_OUI, oui, 4)) + update_bcn_wpa_ie(padapter); + else if (_rtw_memcmp(WMM_OUI, oui, 4)) + update_bcn_wmm_ie(padapter); + else if (_rtw_memcmp(WPS_OUI, oui, 4)) + update_bcn_wps_ie(padapter); + else if (_rtw_memcmp(P2P_OUI, oui, 4)) + update_bcn_p2p_ie(padapter); + else + RTW_INFO("unknown OUI type!\n"); + + +} + +void _update_beacon(_adapter *padapter, u8 ie_id, u8 *oui, u8 tx, u8 flags, const char *tag) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv; + struct mlme_ext_priv *pmlmeext; + bool updated = 1; /* treat as upadated by default */ + + if (!padapter) + return; + + pmlmepriv = &(padapter->mlmepriv); + pmlmeext = &(padapter->mlmeextpriv); + + if (pmlmeext->bstart_bss == _FALSE) + return; + + _enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL); + + switch (ie_id) { + case _TIM_IE_: + update_BCNTIM(padapter); + break; + + case _ERPINFO_IE_: + update_bcn_erpinfo_ie(padapter); + break; + + case _HT_CAPABILITY_IE_: + update_bcn_htcap_ie(padapter); + break; + + case _RSN_IE_2_: + update_bcn_rsn_ie(padapter); + break; + + case _HT_ADD_INFO_IE_: + update_bcn_htinfo_ie(padapter); + break; + + case _EXT_CAP_IE_: + update_bcn_ext_capab_ie(padapter); + break; + +#ifdef CONFIG_RTW_MESH + case WLAN_EID_MESH_CONFIG: + updated = rtw_mesh_update_bss_peering_status(padapter, &(pmlmeext->mlmext_info.network)); + updated |= rtw_mesh_update_bss_formation_info(padapter, &(pmlmeext->mlmext_info.network)); + updated |= rtw_mesh_update_bss_forwarding_state(padapter, &(pmlmeext->mlmext_info.network)); + break; +#endif + + case _VENDOR_SPECIFIC_IE_: + update_bcn_vendor_spec_ie(padapter, oui); + break; + + case 0xFF: + default: + break; + } + + if (updated) + pmlmepriv->update_bcn = _TRUE; + + _exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL); + +#ifndef CONFIG_INTERRUPT_BASED_TXBCN +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) || defined(CONFIG_PCI_BCN_POLLING) + if (tx && updated) { + /* send_beacon(padapter); */ /* send_beacon must execute on TSR level */ + if (0) + RTW_INFO(FUNC_ADPT_FMT" ie_id:%u - %s\n", FUNC_ADPT_ARG(padapter), ie_id, tag); + if(flags == RTW_CMDF_WAIT_ACK) + set_tx_beacon_cmd(padapter, RTW_CMDF_WAIT_ACK); + else + set_tx_beacon_cmd(padapter, 0); + } +#else + { + /* PCI will issue beacon when BCN interrupt occurs. */ + } +#endif +#endif /* !CONFIG_INTERRUPT_BASED_TXBCN */ +} + +#ifdef CONFIG_80211N_HT + +void rtw_process_public_act_bsscoex(_adapter *padapter, u8 *pframe, uint frame_len) +{ + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 beacon_updated = _FALSE; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + uint frame_body_len = frame_len - sizeof(struct rtw_ieee80211_hdr_3addr); + u8 category, action; + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta == NULL) + return; + + + category = frame_body[0]; + action = frame_body[1]; + + if (frame_body_len > 0) { + if ((frame_body[2] == EID_BSSCoexistence) && (frame_body[3] > 0)) { + u8 ie_data = frame_body[4]; + + if (ie_data & RTW_WLAN_20_40_BSS_COEX_40MHZ_INTOL) { + if (psta->ht_40mhz_intolerant == 0) { + psta->ht_40mhz_intolerant = 1; + pmlmepriv->num_sta_40mhz_intolerant++; + beacon_updated = _TRUE; + } + } else if (ie_data & RTW_WLAN_20_40_BSS_COEX_20MHZ_WIDTH_REQ) { + if (pmlmepriv->ht_20mhz_width_req == _FALSE) { + pmlmepriv->ht_20mhz_width_req = _TRUE; + beacon_updated = _TRUE; + } + } else + beacon_updated = _FALSE; + } + } + + if (frame_body_len > 8) { + /* if EID_BSSIntolerantChlReport ie exists */ + if ((frame_body[5] == EID_BSSIntolerantChlReport) && (frame_body[6] > 0)) { + /*todo:*/ + if (pmlmepriv->ht_intolerant_ch_reported == _FALSE) { + pmlmepriv->ht_intolerant_ch_reported = _TRUE; + beacon_updated = _TRUE; + } + } + } + + if (beacon_updated) { + + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _TRUE, 0); + + associated_stainfo_update(padapter, psta, STA_INFO_UPDATE_BW); + } + + + +} + +void rtw_process_ht_action_smps(_adapter *padapter, u8 *ta, u8 ctrl_field) +{ + u8 e_field, m_field; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + psta = rtw_get_stainfo(pstapriv, ta); + if (psta == NULL) + return; + + e_field = (ctrl_field & BIT(0)) ? 1 : 0; /*SM Power Save Enabled*/ + m_field = (ctrl_field & BIT(1)) ? 1 : 0; /*SM Mode, 0:static SMPS, 1:dynamic SMPS*/ + + if (e_field) { + if (m_field) { /*mode*/ + psta->htpriv.smps_cap = WLAN_HT_CAP_SM_PS_DYNAMIC; + RTW_ERR("Don't support dynamic SMPS\n"); + } + else + psta->htpriv.smps_cap = WLAN_HT_CAP_SM_PS_STATIC; + } else { + /*disable*/ + psta->htpriv.smps_cap = WLAN_HT_CAP_SM_PS_DISABLED; + } + + if (psta->htpriv.smps_cap != WLAN_HT_CAP_SM_PS_DYNAMIC) + rtw_ssmps_wk_cmd(padapter, psta, e_field, 1); +} + +/* +op_mode +Set to 0 (HT pure) under the followign conditions + - all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or + - all STAs in the BSS are 20 MHz HT in 20 MHz BSS +Set to 1 (HT non-member protection) if there may be non-HT STAs + in both the primary and the secondary channel +Set to 2 if only HT STAs are associated in BSS, + however and at least one 20 MHz HT STA is associated +Set to 3 (HT mixed mode) when one or more non-HT STAs are associated + (currently non-GF HT station is considered as non-HT STA also) +*/ +int rtw_ht_operation_update(_adapter *padapter) +{ + u16 cur_op_mode, new_op_mode; + int op_mode_changes = 0; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv; + + if (pmlmepriv->htpriv.ht_option == _FALSE) + return 0; + + /*if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed) + return 0;*/ + + RTW_INFO("%s current operation mode=0x%X\n", + __FUNCTION__, pmlmepriv->ht_op_mode); + + if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) + && pmlmepriv->num_sta_ht_no_gf) { + pmlmepriv->ht_op_mode |= + HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT; + op_mode_changes++; + } else if ((pmlmepriv->ht_op_mode & + HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) && + pmlmepriv->num_sta_ht_no_gf == 0) { + pmlmepriv->ht_op_mode &= + ~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT; + op_mode_changes++; + } + + if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) && + (pmlmepriv->num_sta_no_ht || ATOMIC_READ(&pmlmepriv->olbc_ht))) { + pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT; + op_mode_changes++; + } else if ((pmlmepriv->ht_op_mode & + HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) && + (pmlmepriv->num_sta_no_ht == 0 && !ATOMIC_READ(&pmlmepriv->olbc_ht))) { + pmlmepriv->ht_op_mode &= + ~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT; + op_mode_changes++; + } + + /* Note: currently we switch to the MIXED op mode if HT non-greenfield + * station is associated. Probably it's a theoretical case, since + * it looks like all known HT STAs support greenfield. + */ + new_op_mode = 0; + if (pmlmepriv->num_sta_no_ht /*|| + (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)*/) + new_op_mode = OP_MODE_MIXED; + else if ((phtpriv_ap->ht_cap.cap_info & IEEE80211_HT_CAP_SUP_WIDTH) + && pmlmepriv->num_sta_ht_20mhz) + new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED; + else if (ATOMIC_READ(&pmlmepriv->olbc_ht)) + new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS; + else + new_op_mode = OP_MODE_PURE; + + cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK; + if (cur_op_mode != new_op_mode) { + pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK; + pmlmepriv->ht_op_mode |= new_op_mode; + op_mode_changes++; + } + + RTW_INFO("%s new operation mode=0x%X changes=%d\n", + __FUNCTION__, pmlmepriv->ht_op_mode, op_mode_changes); + + return op_mode_changes; + +} + +#endif /* CONFIG_80211N_HT */ + +void associated_clients_update(_adapter *padapter, u8 updated, u32 sta_info_type) +{ + /* update associcated stations cap. */ + if (updated == _TRUE) { + _irqL irqL; + _list *phead, *plist; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* check asoc_queue */ + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + + plist = get_next(plist); + + associated_stainfo_update(padapter, psta, sta_info_type); + } + + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + } + +} + +/* called > TSR LEVEL for USB or SDIO Interface*/ +void bss_cap_update_on_sta_join(_adapter *padapter, struct sta_info *psta) +{ + u8 beacon_updated = _FALSE; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + +#if 0 + if (!(psta->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) && + !psta->no_short_preamble_set) { + psta->no_short_preamble_set = 1; + pmlmepriv->num_sta_no_short_preamble++; + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_preamble == 1)) + ieee802_11_set_beacons(hapd->iface); + } +#endif + + + if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) { + if (!psta->no_short_preamble_set) { + psta->no_short_preamble_set = 1; + + pmlmepriv->num_sta_no_short_preamble++; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_preamble == 1)) + beacon_updated = _TRUE; + } + } else { + if (psta->no_short_preamble_set) { + psta->no_short_preamble_set = 0; + + pmlmepriv->num_sta_no_short_preamble--; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_preamble == 0)) + beacon_updated = _TRUE; + } + } + +#if 0 + if (psta->flags & WLAN_STA_NONERP && !psta->nonerp_set) { + psta->nonerp_set = 1; + pmlmepriv->num_sta_non_erp++; + if (pmlmepriv->num_sta_non_erp == 1) + ieee802_11_set_beacons(hapd->iface); + } +#endif + + if (psta->flags & WLAN_STA_NONERP) { + if (!psta->nonerp_set) { + psta->nonerp_set = 1; + + pmlmepriv->num_sta_non_erp++; + + if (pmlmepriv->num_sta_non_erp == 1) { + beacon_updated = _TRUE; + update_beacon(padapter, _ERPINFO_IE_, NULL, _FALSE, 0); + } + } + + } else { + if (psta->nonerp_set) { + psta->nonerp_set = 0; + + pmlmepriv->num_sta_non_erp--; + + if (pmlmepriv->num_sta_non_erp == 0) { + beacon_updated = _TRUE; + update_beacon(padapter, _ERPINFO_IE_, NULL, _FALSE, 0); + } + } + + } + + +#if 0 + if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT) && + !psta->no_short_slot_time_set) { + psta->no_short_slot_time_set = 1; + pmlmepriv->num_sta_no_short_slot_time++; + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_slot_time == 1)) + ieee802_11_set_beacons(hapd->iface); + } +#endif + + if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT)) { + if (!psta->no_short_slot_time_set) { + psta->no_short_slot_time_set = 1; + + pmlmepriv->num_sta_no_short_slot_time++; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_slot_time == 1)) + beacon_updated = _TRUE; + } + } else { + if (psta->no_short_slot_time_set) { + psta->no_short_slot_time_set = 0; + + pmlmepriv->num_sta_no_short_slot_time--; + + if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && + (pmlmepriv->num_sta_no_short_slot_time == 0)) + beacon_updated = _TRUE; + } + } + +#ifdef CONFIG_80211N_HT + if(padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode)) { + if (psta->flags & WLAN_STA_HT) { + u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info); + + RTW_INFO("HT: STA " MAC_FMT " HT Capabilities Info: 0x%04x\n", + MAC_ARG(psta->cmn.mac_addr), ht_capab); + + if (psta->no_ht_set) { + psta->no_ht_set = 0; + pmlmepriv->num_sta_no_ht--; + } + + if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) { + if (!psta->no_ht_gf_set) { + psta->no_ht_gf_set = 1; + pmlmepriv->num_sta_ht_no_gf++; + } + RTW_INFO("%s STA " MAC_FMT " - no " + "greenfield, num of non-gf stations %d\n", + __FUNCTION__, MAC_ARG(psta->cmn.mac_addr), + pmlmepriv->num_sta_ht_no_gf); + } + + if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH) == 0) { + if (!psta->ht_20mhz_set) { + psta->ht_20mhz_set = 1; + pmlmepriv->num_sta_ht_20mhz++; + } + RTW_INFO("%s STA " MAC_FMT " - 20 MHz HT, " + "num of 20MHz HT STAs %d\n", + __FUNCTION__, MAC_ARG(psta->cmn.mac_addr), + pmlmepriv->num_sta_ht_20mhz); + } + + if (((ht_capab & RTW_IEEE80211_HT_CAP_40MHZ_INTOLERANT) != 0) && + (psta->ht_40mhz_intolerant == 0)) { + psta->ht_40mhz_intolerant = 1; + pmlmepriv->num_sta_40mhz_intolerant++; + RTW_INFO("%s STA " MAC_FMT " - 40MHZ_INTOLERANT, ", + __FUNCTION__, MAC_ARG(psta->cmn.mac_addr)); + } + + } else { + if (!psta->no_ht_set) { + psta->no_ht_set = 1; + pmlmepriv->num_sta_no_ht++; + } + if (pmlmepriv->htpriv.ht_option == _TRUE) { + RTW_INFO("%s STA " MAC_FMT + " - no HT, num of non-HT stations %d\n", + __FUNCTION__, MAC_ARG(psta->cmn.mac_addr), + pmlmepriv->num_sta_no_ht); + } + } + + if (rtw_ht_operation_update(padapter) > 0) { + update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, _FALSE, 0); + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _FALSE, 0); + beacon_updated = _TRUE; + } + } +#endif /* CONFIG_80211N_HT */ + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + struct sta_priv *pstapriv = &padapter->stapriv; + + update_beacon(padapter, WLAN_EID_MESH_CONFIG, NULL, _FALSE, 0); + if (pstapriv->asoc_list_cnt == 1) + _set_timer(&padapter->mesh_atlm_param_req_timer, 0); + beacon_updated = _TRUE; + } +#endif + + if (beacon_updated) + update_beacon(padapter, 0xFF, NULL, _TRUE, 0); + + /* update associcated stations cap. */ + associated_clients_update(padapter, beacon_updated, STA_INFO_UPDATE_ALL); + + RTW_INFO("%s, updated=%d\n", __func__, beacon_updated); + +} + +u8 bss_cap_update_on_sta_leave(_adapter *padapter, struct sta_info *psta) +{ + u8 beacon_updated = _FALSE; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + if (!psta) + return beacon_updated; + + if (rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid)) { + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + beacon_updated = _TRUE; + update_beacon(padapter, _TIM_IE_, NULL, _FALSE, 0); + } + + if (psta->no_short_preamble_set) { + psta->no_short_preamble_set = 0; + pmlmepriv->num_sta_no_short_preamble--; + if (pmlmeext->cur_wireless_mode > WIRELESS_11B + && pmlmepriv->num_sta_no_short_preamble == 0) + beacon_updated = _TRUE; + } + + if (psta->nonerp_set) { + psta->nonerp_set = 0; + pmlmepriv->num_sta_non_erp--; + if (pmlmepriv->num_sta_non_erp == 0) { + beacon_updated = _TRUE; + update_beacon(padapter, _ERPINFO_IE_, NULL, _FALSE, 0); + } + } + + if (psta->no_short_slot_time_set) { + psta->no_short_slot_time_set = 0; + pmlmepriv->num_sta_no_short_slot_time--; + if (pmlmeext->cur_wireless_mode > WIRELESS_11B + && pmlmepriv->num_sta_no_short_slot_time == 0) + beacon_updated = _TRUE; + } + +#ifdef CONFIG_80211N_HT + if (psta->no_ht_gf_set) { + psta->no_ht_gf_set = 0; + pmlmepriv->num_sta_ht_no_gf--; + } + + if (psta->no_ht_set) { + psta->no_ht_set = 0; + pmlmepriv->num_sta_no_ht--; + } + + if (psta->ht_20mhz_set) { + psta->ht_20mhz_set = 0; + pmlmepriv->num_sta_ht_20mhz--; + } + + if (psta->ht_40mhz_intolerant) { + psta->ht_40mhz_intolerant = 0; + if (pmlmepriv->num_sta_40mhz_intolerant > 0) + pmlmepriv->num_sta_40mhz_intolerant--; + else + rtw_warn_on(1); + } + + if (rtw_ht_operation_update(padapter) > 0) { + update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, _FALSE, 0); + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _FALSE, 0); + } +#endif /* CONFIG_80211N_HT */ + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + update_beacon(padapter, WLAN_EID_MESH_CONFIG, NULL, _FALSE, 0); + if (pstapriv->asoc_list_cnt == 0) + _cancel_timer_ex(&padapter->mesh_atlm_param_req_timer); + beacon_updated = _TRUE; + } +#endif + + if (beacon_updated == _TRUE) + update_beacon(padapter, 0xFF, NULL, _TRUE, 0); + +#if 0 + /* update associated stations cap. */ + associated_clients_update(padapter, beacon_updated, STA_INFO_UPDATE_ALL); /* move it to avoid deadlock */ +#endif + + RTW_INFO("%s, updated=%d\n", __func__, beacon_updated); + + return beacon_updated; + +} + +u8 ap_free_sta(_adapter *padapter, struct sta_info *psta, bool active, u16 reason, bool enqueue) +{ + _irqL irqL; + u8 beacon_updated = _FALSE; + + if (!psta) + return beacon_updated; + + if (active == _TRUE) { +#ifdef CONFIG_80211N_HT + /* tear down Rx AMPDU */ + send_delba(padapter, 0, psta->cmn.mac_addr);/* recipient */ + + /* tear down TX AMPDU */ + send_delba(padapter, 1, psta->cmn.mac_addr);/* */ /* originator */ + +#endif /* CONFIG_80211N_HT */ + + if (!MLME_IS_MESH(padapter)) + issue_deauth(padapter, psta->cmn.mac_addr, reason); + } + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + rtw_mesh_path_flush_by_nexthop(psta); +#endif + +#ifdef CONFIG_BEAMFORMING + beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_LEAVE, psta->cmn.mac_addr, ETH_ALEN, 1); +#endif + +#ifdef CONFIG_80211N_HT + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ +#endif + + + + _enter_critical_bh(&psta->lock, &irqL); + psta->state &= ~(WIFI_ASOC_STATE | WIFI_UNDER_KEY_HANDSHAKE); + +#ifdef CONFIG_IOCTL_CFG80211 + if ((psta->auth_len != 0) && (psta->pauth_frame != NULL)) { + rtw_mfree(psta->pauth_frame, psta->auth_len); + psta->pauth_frame = NULL; + psta->auth_len = 0; + } + if (psta->passoc_req && psta->assoc_req_len > 0) { + rtw_mfree(psta->passoc_req , psta->assoc_req_len); + psta->passoc_req = NULL; + psta->assoc_req_len = 0; + } +#endif /* CONFIG_IOCTL_CFG80211 */ + _exit_critical_bh(&psta->lock, &irqL); + + if (!MLME_IS_MESH(padapter)) { + #ifdef CONFIG_RTW_WDS + rtw_wds_path_flush_by_nexthop(psta); + #endif + +#ifdef CONFIG_IOCTL_CFG80211 + #ifdef COMPAT_KERNEL_RELEASE + rtw_cfg80211_indicate_sta_disassoc(padapter, psta->cmn.mac_addr, reason); + #elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) + rtw_cfg80211_indicate_sta_disassoc(padapter, psta->cmn.mac_addr, reason); + #else /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */ + /* will call rtw_cfg80211_indicate_sta_disassoc() in cmd_thread for old API context */ + #endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */ +#else + rtw_indicate_sta_disassoc_event(padapter, psta); +#endif + } + + beacon_updated = bss_cap_update_on_sta_leave(padapter, psta); + + report_del_sta_event(padapter, psta->cmn.mac_addr, reason, enqueue, _FALSE); + + /* clear cam entry / key */ + rtw_clearstakey_cmd(padapter, psta, enqueue); + + return beacon_updated; + +} + +int rtw_ap_inform_ch_switch(_adapter *padapter, u8 new_ch, u8 ch_offset) +{ + _irqL irqL; + _list *phead, *plist; + int ret = 0; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE) + return ret; + + RTW_INFO(FUNC_NDEV_FMT" with ch:%u, offset:%u\n", + FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset); + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* for each sta in asoc_queue */ + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + issue_action_spct_ch_switch(padapter, psta->cmn.mac_addr, new_ch, ch_offset); + psta->expire_to = ((pstapriv->expire_to * 2) > 5) ? 5 : (pstapriv->expire_to * 2); + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + issue_action_spct_ch_switch(padapter, bc_addr, new_ch, ch_offset); + + return ret; +} + +int rtw_sta_flush(_adapter *padapter, bool enqueue) +{ + _irqL irqL; + _list *phead, *plist; + int ret = 0; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 flush_num = 0; + char flush_list[NUM_STA]; + int i; + + if (!MLME_IS_AP(padapter) && !MLME_IS_MESH(padapter)) + return ret; + + RTW_INFO(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev)); + + /* pick sta from sta asoc_queue */ + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + int stainfo_offset; + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + rtw_list_delete(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + STA_SET_MESH_PLINK(psta, NULL); + + stainfo_offset = rtw_stainfo_offset(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) + flush_list[flush_num++] = stainfo_offset; + else + rtw_warn_on(1); + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + /* call ap_free_sta() for each sta picked */ + for (i = 0; i < flush_num; i++) { + u8 sta_addr[ETH_ALEN]; + + psta = rtw_get_stainfo_by_offset(pstapriv, flush_list[i]); + _rtw_memcpy(sta_addr, psta->cmn.mac_addr, ETH_ALEN); + + ap_free_sta(padapter, psta, _TRUE, WLAN_REASON_DEAUTH_LEAVING, enqueue); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + rtw_mesh_expire_peer(padapter, sta_addr); + #endif + } + + if (!MLME_IS_MESH(padapter)) + issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING); + + associated_clients_update(padapter, _TRUE, STA_INFO_UPDATE_ALL); + + return ret; +} + +/* called > TSR LEVEL for USB or SDIO Interface*/ +void sta_info_update(_adapter *padapter, struct sta_info *psta) +{ + int flags = psta->flags; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + + /* update wmm cap. */ + if (WLAN_STA_WME & flags) + psta->qos_option = 1; + else + psta->qos_option = 0; + + if (pmlmepriv->qospriv.qos_option == 0) + psta->qos_option = 0; + + +#ifdef CONFIG_80211N_HT + /* update 802.11n ht cap. */ + if (WLAN_STA_HT & flags) { + psta->htpriv.ht_option = _TRUE; + psta->qos_option = 1; + + psta->htpriv.smps_cap = (psta->htpriv.ht_cap.cap_info & IEEE80211_HT_CAP_SM_PS) >> 2; + } else + psta->htpriv.ht_option = _FALSE; + + if (pmlmepriv->htpriv.ht_option == _FALSE) + psta->htpriv.ht_option = _FALSE; +#endif + +#ifdef CONFIG_80211AC_VHT + /* update 802.11AC vht cap. */ + if (WLAN_STA_VHT & flags) + psta->vhtpriv.vht_option = _TRUE; + else + psta->vhtpriv.vht_option = _FALSE; + + if (pmlmepriv->vhtpriv.vht_option == _FALSE) + psta->vhtpriv.vht_option = _FALSE; +#endif + + update_sta_info_apmode(padapter, psta); +} + +/* called >= TSR LEVEL for USB or SDIO Interface*/ +void ap_sta_info_defer_update(_adapter *padapter, struct sta_info *psta) +{ + if (psta->state & WIFI_ASOC_STATE) + rtw_hal_update_ra_mask(psta); /* DM_RATR_STA_INIT */ +} +/* restore hw setting from sw data structures */ +void rtw_ap_restore_network(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + struct security_priv *psecuritypriv = &(padapter->securitypriv); + _irqL irqL; + _list *phead, *plist; + u8 chk_alive_num = 0; + char chk_alive_list[NUM_STA]; + int i; + + rtw_setopmode_cmd(padapter + , MLME_IS_AP(padapter) ? Ndis802_11APMode : Ndis802_11_mesh + , RTW_CMDF_DIRECTLY + ); + + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + rtw_startbss_cmd(padapter, RTW_CMDF_DIRECTLY); + + if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) || + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) { + /* restore group key, WEP keys is restored in ips_leave() */ + rtw_set_key(padapter, psecuritypriv, psecuritypriv->dot118021XGrpKeyid, 0, _FALSE); + } + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + int stainfo_offset; + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + stainfo_offset = rtw_stainfo_offset(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) + chk_alive_list[chk_alive_num++] = stainfo_offset; + } + + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + for (i = 0; i < chk_alive_num; i++) { + psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]); + + if (psta == NULL) + RTW_INFO(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter)); + else if (psta->state & WIFI_ASOC_STATE) { + rtw_sta_media_status_rpt(padapter, psta, 1); + Update_RA_Entry(padapter, psta); + /* pairwise key */ + /* per sta pairwise key and settings */ + if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) || + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) + rtw_setstakey_cmd(padapter, psta, UNICAST_KEY, _FALSE); + } + } + +} + +void start_ap_mode(_adapter *padapter) +{ + int i; + struct sta_info *psta = NULL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct security_priv *psecuritypriv = &padapter->securitypriv; + + pmlmepriv->update_bcn = _FALSE; + + /*init_mlme_ap_info(padapter);*/ + + pmlmeext->bstart_bss = _FALSE; + + pmlmepriv->num_sta_non_erp = 0; + + pmlmepriv->num_sta_no_short_slot_time = 0; + + pmlmepriv->num_sta_no_short_preamble = 0; + + pmlmepriv->num_sta_ht_no_gf = 0; +#ifdef CONFIG_80211N_HT + pmlmepriv->num_sta_no_ht = 0; +#endif /* CONFIG_80211N_HT */ + pmlmeinfo->HT_info_enable = 0; + pmlmeinfo->HT_caps_enable = 0; + pmlmeinfo->HT_enable = 0; + + pmlmepriv->num_sta_ht_20mhz = 0; + pmlmepriv->num_sta_40mhz_intolerant = 0; + ATOMIC_SET(&pmlmepriv->olbc, _FALSE); + ATOMIC_SET(&pmlmepriv->olbc_ht, _FALSE); + +#ifdef CONFIG_80211N_HT + pmlmepriv->ht_20mhz_width_req = _FALSE; + pmlmepriv->ht_intolerant_ch_reported = _FALSE; + pmlmepriv->ht_op_mode = 0; + pmlmepriv->sw_to_20mhz = 0; +#endif + + _rtw_memset(pmlmepriv->ext_capab_ie_data, 0, sizeof(pmlmepriv->ext_capab_ie_data)); + pmlmepriv->ext_capab_ie_len = 0; + + psecuritypriv->dot118021x_bmc_cam_id = INVALID_SEC_MAC_CAM_ID; + + for (i = 0 ; i < pstapriv->max_aid; i++) + pstapriv->sta_aid[i] = NULL; + +#ifdef CONFIG_RTW_WDS + if (MLME_IS_AP(padapter)) + rtw_wds_pathtbl_init(padapter); +#endif + + psta = rtw_get_bcmc_stainfo(padapter); + /*_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);*/ + if (psta) + rtw_free_stainfo(padapter, psta); + /*_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);*/ + + rtw_init_bcmc_stainfo(padapter); + + if (rtw_mi_get_ap_num(padapter)) + RTW_SET_SCAN_BAND_SKIP(padapter, BAND_5G); + +} + +void stop_ap_mode(_adapter *padapter) +{ + u8 self_action = MLME_ACTION_UNKNOWN; + struct sta_info *psta = NULL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#ifdef CONFIG_SUPPORT_MULTI_BCN + struct dvobj_priv *pdvobj = padapter->dvobj; + _irqL irqL; +#endif + + RTW_INFO("%s -"ADPT_FMT"\n", __func__, ADPT_ARG(padapter)); + + if (MLME_IS_AP(padapter)) + self_action = MLME_AP_STOPPED; + else if (MLME_IS_MESH(padapter)) + self_action = MLME_MESH_STOPPED; + else + rtw_warn_on(1); + + pmlmepriv->update_bcn = _FALSE; + /*pmlmeext->bstart_bss = _FALSE;*/ + padapter->netif_up = _FALSE; + /* _rtw_spinlock_free(&pmlmepriv->bcn_update_lock); */ + + /* reset and init security priv , this can refine with rtw_reset_securitypriv */ + _rtw_memset((unsigned char *)&padapter->securitypriv, 0, sizeof(struct security_priv)); + padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen; + padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled; + +#ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(padapter, self_action, 0); +#endif + + rtw_rfctl_update_op_mode(adapter_to_rfctl(padapter), BIT(padapter->iface_id), 0); + + /* free scan queue */ + rtw_free_network_queue(padapter, _TRUE); + +#if CONFIG_RTW_MACADDR_ACL + rtw_macaddr_acl_clear(padapter, RTW_ACL_PERIOD_BSS); +#endif + + rtw_sta_flush(padapter, _TRUE); + + /* free_assoc_sta_resources */ + rtw_free_all_stainfo(padapter); + + psta = rtw_get_bcmc_stainfo(padapter); + if (psta) { + rtw_sta_mstatus_disc_rpt(padapter, psta->cmn.mac_id); + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + rtw_free_stainfo(padapter, psta); + /*_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);*/ + } + + pmlmepriv->ap_isolate = 0; +#ifdef CONFIG_RTW_WDS + adapter_set_use_wds(padapter, 0); +#endif +#ifdef CONFIG_RTW_MULTI_AP + padapter->multi_ap = 0; +#endif + rtw_free_mlme_priv_ie_data(pmlmepriv); + +#ifdef CONFIG_SUPPORT_MULTI_BCN + if (pmlmeext->bstart_bss == _TRUE) { + #ifdef CONFIG_FW_HANDLE_TXBCN + u8 free_apid = CONFIG_LIMITED_AP_NUM; + #endif + + _enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + pdvobj->nr_ap_if--; + if (pdvobj->nr_ap_if > 0) + pdvobj->inter_bcn_space = DEFAULT_BCN_INTERVAL / pdvobj->nr_ap_if; + else + pdvobj->inter_bcn_space = DEFAULT_BCN_INTERVAL; + #ifdef CONFIG_FW_HANDLE_TXBCN + rtw_ap_release_vapid(pdvobj, padapter->vap_id); + free_apid = padapter->vap_id; + padapter->vap_id = CONFIG_LIMITED_AP_NUM; + #endif + rtw_list_delete(&padapter->list); + _exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + #ifdef CONFIG_FW_HANDLE_TXBCN + rtw_ap_mbid_bcn_dis(padapter, free_apid); + #endif + + #ifdef CONFIG_SWTIMER_BASED_TXBCN + rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pdvobj->inter_bcn_space)); + + if (pdvobj->nr_ap_if == 0) + _cancel_timer_ex(&pdvobj->txbcn_timer); + #endif + } +#endif + + pmlmeext->bstart_bss = _FALSE; + + rtw_hal_rcr_set_chk_bssid(padapter, self_action); + +#ifdef CONFIG_HW_P0_TSF_SYNC + correct_TSF(padapter, self_action); +#endif + +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_MediaStatusNotify(padapter, 0); /* disconnect */ +#endif + +#ifdef CONFIG_RTW_WDS + if (MLME_IS_AP(padapter)) + rtw_wds_pathtbl_unregister(padapter); +#endif +} + +#endif /* CONFIG_NATIVEAP_MLME */ + +void rtw_ap_update_bss_chbw(_adapter *adapter, WLAN_BSSID_EX *bss, u8 ch, u8 bw, u8 offset) +{ +#define UPDATE_VHT_CAP 1 +#define UPDATE_HT_CAP 1 +#ifdef CONFIG_80211AC_VHT + struct vht_priv *vhtpriv = &adapter->mlmepriv.vhtpriv; +#endif + { + u8 *p; + int ie_len; + u8 old_ch = bss->Configuration.DSConfig; + bool change_band = _FALSE; + + if ((ch <= 14 && old_ch >= 36) || (ch >= 36 && old_ch <= 14)) + change_band = _TRUE; + + /* update channel in IE */ + p = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs))); + if (p && ie_len > 0) + *(p + 2) = ch; + + bss->Configuration.DSConfig = ch; + + /* band is changed, update ERP, support rate, ext support rate IE */ + if (change_band == _TRUE) + change_band_update_ie(adapter, bss, ch); + } + +#ifdef CONFIG_80211AC_VHT + if (vhtpriv->vht_option == _TRUE) { + u8 *vht_cap_ie, *vht_op_ie; + int vht_cap_ielen, vht_op_ielen; + u8 center_freq; + + vht_cap_ie = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_VHTCapability, &vht_cap_ielen, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs))); + vht_op_ie = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_VHTOperation, &vht_op_ielen, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs))); + center_freq = rtw_get_center_ch(ch, bw, offset); + + /* update vht cap ie */ + if (vht_cap_ie && vht_cap_ielen) { + #if UPDATE_VHT_CAP + /* if ((bw == CHANNEL_WIDTH_160 || bw == CHANNEL_WIDTH_80_80) && pvhtpriv->sgi_160m) + SET_VHT_CAPABILITY_ELE_SHORT_GI160M(pvht_cap_ie + 2, 1); + else */ + SET_VHT_CAPABILITY_ELE_SHORT_GI160M(vht_cap_ie + 2, 0); + + if (bw >= CHANNEL_WIDTH_80 && vhtpriv->sgi_80m) + SET_VHT_CAPABILITY_ELE_SHORT_GI80M(vht_cap_ie + 2, 1); + else + SET_VHT_CAPABILITY_ELE_SHORT_GI80M(vht_cap_ie + 2, 0); + #endif + } + + /* update vht op ie */ + if (vht_op_ie && vht_op_ielen) { + if (bw < CHANNEL_WIDTH_80) { + SET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2, 0); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(vht_op_ie + 2, 0); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(vht_op_ie + 2, 0); + } else if (bw == CHANNEL_WIDTH_80) { + SET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2, 1); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(vht_op_ie + 2, center_freq); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(vht_op_ie + 2, 0); + } else { + RTW_ERR(FUNC_ADPT_FMT" unsupported BW:%u\n", FUNC_ADPT_ARG(adapter), bw); + rtw_warn_on(1); + } + } + } +#endif /* CONFIG_80211AC_VHT */ +#ifdef CONFIG_80211N_HT + { + struct ht_priv *htpriv = &adapter->mlmepriv.htpriv; + u8 *ht_cap_ie, *ht_op_ie; + int ht_cap_ielen, ht_op_ielen; + + ht_cap_ie = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_HTCapability, &ht_cap_ielen, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs))); + ht_op_ie = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_HTInfo, &ht_op_ielen, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs))); + + /* update ht cap ie */ + if (ht_cap_ie && ht_cap_ielen) { + #if UPDATE_HT_CAP + if (bw >= CHANNEL_WIDTH_40) + SET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2, 1); + else + SET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2, 0); + + if (bw >= CHANNEL_WIDTH_40 && htpriv->sgi_40m) + SET_HT_CAP_ELE_SHORT_GI40M(ht_cap_ie + 2, 1); + else + SET_HT_CAP_ELE_SHORT_GI40M(ht_cap_ie + 2, 0); + + if (htpriv->sgi_20m) + SET_HT_CAP_ELE_SHORT_GI20M(ht_cap_ie + 2, 1); + else + SET_HT_CAP_ELE_SHORT_GI20M(ht_cap_ie + 2, 0); + #endif + } + + /* update ht op ie */ + if (ht_op_ie && ht_op_ielen) { + SET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2, ch); + switch (offset) { + case HAL_PRIME_CHNL_OFFSET_LOWER: + SET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2, SCA); + break; + case HAL_PRIME_CHNL_OFFSET_UPPER: + SET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2, SCB); + break; + case HAL_PRIME_CHNL_OFFSET_DONT_CARE: + default: + SET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2, SCN); + break; + } + + if (bw >= CHANNEL_WIDTH_40) + SET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2, 1); + else + SET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2, 0); + } + } +#endif /* CONFIG_80211N_HT */ +} + +static u8 rtw_ap_update_chbw_by_ifbmp(struct dvobj_priv *dvobj, u8 ifbmp + , u8 cur_ie_ch[], u8 cur_ie_bw[], u8 cur_ie_offset[] + , u8 dec_ch[], u8 dec_bw[], u8 dec_offset[] + , const char *caller) +{ + _adapter *iface; + struct mlme_ext_priv *mlmeext; + WLAN_BSSID_EX *network; + u8 ifbmp_ch_changed = 0; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + + iface = dvobj->padapters[i]; + mlmeext = &(iface->mlmeextpriv); + + if (MLME_IS_ASOC(iface)) { + RTW_INFO(FUNC_ADPT_FMT" %u,%u,%u => %u,%u,%u%s\n", caller, ADPT_ARG(iface) + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset + , dec_ch[i], dec_bw[i], dec_offset[i] + , MLME_IS_OPCH_SW(iface) ? " OPCH_SW" : ""); + } else { + RTW_INFO(FUNC_ADPT_FMT" %u,%u,%u => %u,%u,%u%s\n", caller, ADPT_ARG(iface) + , cur_ie_ch[i], cur_ie_bw[i], cur_ie_offset[i] + , dec_ch[i], dec_bw[i], dec_offset[i] + , MLME_IS_OPCH_SW(iface) ? " OPCH_SW" : ""); + } + } + + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + + iface = dvobj->padapters[i]; + mlmeext = &(iface->mlmeextpriv); + network = &(mlmeext->mlmext_info.network); + + /* ch setting differs from mlmeext.network IE */ + if (cur_ie_ch[i] != dec_ch[i] + || cur_ie_bw[i] != dec_bw[i] + || cur_ie_offset[i] != dec_offset[i]) + ifbmp_ch_changed |= BIT(i); + + /* ch setting differs from existing one */ + if (MLME_IS_ASOC(iface) + && (mlmeext->cur_channel != dec_ch[i] + || mlmeext->cur_bwmode != dec_bw[i] + || mlmeext->cur_ch_offset != dec_offset[i]) + ) { + if (rtw_linked_check(iface) == _TRUE) { + #ifdef CONFIG_SPCT_CH_SWITCH + if (1) + rtw_ap_inform_ch_switch(iface, dec_ch[i], dec_offset[i]); + else + #endif + rtw_sta_flush(iface, _FALSE); + } + } + + mlmeext->cur_channel = dec_ch[i]; + mlmeext->cur_bwmode = dec_bw[i]; + mlmeext->cur_ch_offset = dec_offset[i]; + + rtw_ap_update_bss_chbw(iface, network, dec_ch[i], dec_bw[i], dec_offset[i]); + } + + return ifbmp_ch_changed; +} + +static u8 rtw_ap_ch_specific_chk(_adapter *adapter, u8 ch, u8 *bw, u8 *offset, const char *caller) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + int ch_idx; + u8 ret = _SUCCESS; + + ch_idx = rtw_chset_search_ch(chset, ch); + if (ch_idx < 0) { + RTW_WARN("%s ch:%u doesn't fit in chplan\n", caller, ch); + ret = _FAIL; + goto exit; + } + if (chset[ch_idx].flags & RTW_CHF_NO_IR) { + RTW_WARN("%s ch:%u is passive\n", caller, ch); + ret = _FAIL; + goto exit; + } + + rtw_adjust_chbw(adapter, ch, bw, offset); + + if (!rtw_get_offset_by_chbw(ch, *bw, offset)) { + RTW_WARN("%s %u,%u has no valid offset\n", caller, ch, *bw); + ret = _FAIL; + goto exit; + } + + while (!rtw_chset_is_chbw_valid(chset, ch, *bw, *offset, 0, 0) + || (rtw_rfctl_dfs_domain_unknown(rfctl) && rtw_chset_is_dfs_chbw(chset, ch, *bw, *offset)) + ) { + if (*bw > CHANNEL_WIDTH_20) + (*bw)--; + if (*bw == CHANNEL_WIDTH_20) { + *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + } + + if (rtw_rfctl_dfs_domain_unknown(rfctl) && rtw_chset_is_dfs_chbw(chset, ch, *bw, *offset)) { + RTW_WARN("%s DFS channel %u can't be used\n", caller, ch); + ret = _FAIL; + goto exit; + } + +exit: + return ret; +} + +static bool rtw_ap_choose_chbw(_adapter *adapter, u8 sel_ch, u8 max_bw, u8 cur_ch + , u8 *ch, u8 *bw, u8 *offset, bool by_int_info, u8 mesh_only, const char *caller) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + bool ch_avail = _FALSE; + +#if defined(CONFIG_DFS_MASTER) + if (!rtw_rfctl_dfs_domain_unknown(rfctl)) { + if (rfctl->radar_detected + && rfctl->dbg_dfs_choose_dfs_ch_first + ) { + ch_avail = rtw_choose_shortest_waiting_ch(rfctl, sel_ch, max_bw + , ch, bw, offset + , RTW_CHF_DFS, 0 + , cur_ch, by_int_info, mesh_only); + if (ch_avail == _TRUE) { + RTW_INFO("%s choose 5G DFS channel for debug\n", caller); + goto exit; + } + } + + if (rfctl->radar_detected + && (rfctl->dfs_ch_sel_e_flags || rfctl->dfs_ch_sel_d_flags) + ) { + ch_avail = rtw_choose_shortest_waiting_ch(rfctl, sel_ch, max_bw + , ch, bw, offset + , rfctl->dfs_ch_sel_e_flags, rfctl->dfs_ch_sel_d_flags + , cur_ch, by_int_info, mesh_only); + if (ch_avail == _TRUE) { + RTW_INFO("%s choose with dfs_ch_sel_ e_flags:0x%02x d_flags:0x%02x for debug\n" + , caller, rfctl->dfs_ch_sel_e_flags, rfctl->dfs_ch_sel_d_flags); + goto exit; + } + } + + ch_avail = rtw_choose_shortest_waiting_ch(rfctl, sel_ch, max_bw + , ch, bw, offset + , 0, 0 + , cur_ch, by_int_info, mesh_only); + } else +#endif /* defined(CONFIG_DFS_MASTER) */ + { + ch_avail = rtw_choose_shortest_waiting_ch(rfctl, sel_ch, max_bw + , ch, bw, offset + , 0, RTW_CHF_DFS + , cur_ch, by_int_info, mesh_only); + } +#if defined(CONFIG_DFS_MASTER) +exit: +#endif + if (ch_avail == _FALSE) + RTW_WARN("%s no available channel\n", caller); + + return ch_avail; +} + +u8 rtw_ap_chbw_decision(_adapter *adapter, u8 ifbmp, u8 excl_ifbmp + , s16 req_ch, s8 req_bw, s8 req_offset + , u8 *ch, u8 *bw, u8 *offset, u8 *chbw_allow, bool *set_u_ch) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + RT_CHANNEL_INFO *chset = adapter_to_chset(adapter); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + bool ch_avail = _FALSE; + u8 cur_ie_ch[CONFIG_IFACE_NUMBER] = {0}; + u8 cur_ie_bw[CONFIG_IFACE_NUMBER] = {0}; + u8 cur_ie_offset[CONFIG_IFACE_NUMBER] = {0}; + u8 dec_ch[CONFIG_IFACE_NUMBER] = {0}; + u8 dec_bw[CONFIG_IFACE_NUMBER] = {0}; + u8 dec_offset[CONFIG_IFACE_NUMBER] = {0}; + u8 u_ch = 0, u_bw = 0, u_offset = 0; + struct mlme_ext_priv *mlmeext; + WLAN_BSSID_EX *network; + struct mi_state mstate; + struct mi_state mstate_others; + u8 ifbmp_others = 0xFF & ~ifbmp & ~excl_ifbmp; + u8 ifbmp_ch_changed = 0; + bool ifbmp_all_mesh = 0; + _adapter *iface; + int i; + + *set_u_ch = _FALSE; + +#ifdef CONFIG_RTW_MESH + for (i = 0; i < dvobj->iface_nums; i++) + if ((ifbmp & BIT(i)) && dvobj->padapters) + if (!MLME_IS_MESH(dvobj->padapters[i])) + break; + ifbmp_all_mesh = i >= dvobj->iface_nums ? 1 : 0; +#endif + + RTW_INFO("%s ifbmp:0x%02x excl_ifbmp:0x%02x req:%d,%d,%d\n", __func__ + , ifbmp, excl_ifbmp, req_ch, req_bw, req_offset); + rtw_mi_status_by_ifbmp(dvobj, ifbmp, &mstate); + rtw_mi_status_by_ifbmp(dvobj, ifbmp_others, &mstate_others); + RTW_INFO("%s others ld_sta_num:%u, lg_sta_num:%u, ap_num:%u, mesh_num:%u\n" + , __func__, MSTATE_STA_LD_NUM(&mstate_others), MSTATE_STA_LG_NUM(&mstate_others) + , MSTATE_AP_NUM(&mstate_others), MSTATE_MESH_NUM(&mstate_others)); + + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + iface = dvobj->padapters[i]; + mlmeext = &(iface->mlmeextpriv); + network = &(mlmeext->mlmext_info.network); + + /* get current IE channel settings */ + rtw_ies_get_chbw(BSS_EX_TLV_IES(network), BSS_EX_TLV_IES_LEN(network) + , &cur_ie_ch[i], &cur_ie_bw[i], &cur_ie_offset[i], 1, 1); + + /* prepare temporary channel setting decision */ + if (req_ch == 0) { + /* request comes from upper layer, use cur_ie values */ + dec_ch[i] = cur_ie_ch[i]; + dec_bw[i] = cur_ie_bw[i]; + dec_offset[i] = cur_ie_offset[i]; + } else { + /* use chbw of cur_ie updated with specifying req as temporary decision */ + dec_ch[i] = (req_ch <= REQ_CH_NONE) ? cur_ie_ch[i] : req_ch; + if (req_bw <= REQ_BW_NONE) { + if (req_bw == REQ_BW_ORI) + dec_bw[i] = iface->mlmepriv.ori_bw; + else + dec_bw[i] = cur_ie_bw[i]; + } else + dec_bw[i] = req_bw; + dec_offset[i] = (req_offset <= REQ_OFFSET_NONE) ? cur_ie_offset[i] : req_offset; + } + } + + if (MSTATE_STA_LD_NUM(&mstate_others) || MSTATE_STA_LG_NUM(&mstate_others) + || MSTATE_AP_NUM(&mstate_others) || MSTATE_MESH_NUM(&mstate_others) + ) { + /* has linked/linking STA or has AP/Mesh mode */ + rtw_warn_on(!rtw_mi_get_ch_setting_union_by_ifbmp(dvobj, ifbmp_others, &u_ch, &u_bw, &u_offset)); + RTW_INFO("%s others union:%u,%u,%u\n", __func__, u_ch, u_bw, u_offset); + } + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(adapter) && req_ch == 0) { + if (rtw_hal_check_mcc_status(adapter, MCC_STATUS_DOING_MCC)) { + u8 if_id = adapter->iface_id; + + mlmeext = &(adapter->mlmeextpriv); + + /* check channel settings are the same */ + if (cur_ie_ch[if_id] == mlmeext->cur_channel + && cur_ie_bw[if_id] == mlmeext->cur_bwmode + && cur_ie_offset[if_id] == mlmeext->cur_ch_offset) { + + RTW_INFO(FUNC_ADPT_FMT"req ch settings are the same as current ch setting, go to exit\n" + , FUNC_ADPT_ARG(adapter)); + + *chbw_allow = _FALSE; + goto exit; + } else { + RTW_INFO(FUNC_ADPT_FMT"request channel settings are not the same as current channel setting(%d,%d,%d,%d,%d,%d), restart MCC\n" + , FUNC_ADPT_ARG(adapter) + , cur_ie_ch[if_id], cur_ie_bw[if_id], cur_ie_offset[if_id] + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset); + + rtw_hal_set_mcc_setting_disconnect(adapter); + } + } + } +#endif /* CONFIG_MCC_MODE */ + + if (MSTATE_STA_LG_NUM(&mstate_others) && !MSTATE_STA_LD_NUM(&mstate_others)) { + /* has linking STA but no linked STA */ + + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + iface = dvobj->padapters[i]; + + rtw_adjust_chbw(iface, dec_ch[i], &dec_bw[i], &dec_offset[i]); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(iface)) + rtw_mesh_adjust_chbw(dec_ch[i], &dec_bw[i], &dec_offset[i]); + #endif + + if (rtw_is_chbw_grouped(u_ch, u_bw, u_offset, dec_ch[i], dec_bw[i], dec_offset[i])) { + rtw_chset_sync_chbw(chset + , &dec_ch[i], &dec_bw[i], &dec_offset[i] + , &u_ch, &u_bw, &u_offset, 1, 0); + *set_u_ch = _TRUE; + + /* channel bw offset can be allowed, not need MCC */ + *chbw_allow = _TRUE; + } else { + #ifdef CONFIG_MCC_MODE + if (MCC_EN(iface)) { + mlmeext = &(iface->mlmeextpriv); + mlmeext->cur_channel = *ch = dec_ch[i]; + mlmeext->cur_bwmode = *bw = dec_bw[i]; + mlmeext->cur_ch_offset = *offset = dec_offset[i]; + + /* channel bw offset can not be allowed, need MCC */ + *chbw_allow = _FALSE; + RTW_INFO(FUNC_ADPT_FMT" enable mcc: %u,%u,%u\n", FUNC_ADPT_ARG(iface) + , *ch, *bw, *offset); + goto exit; + } + #endif /* CONFIG_MCC_MODE */ + + /* set this for possible ch change when join down*/ + set_fwstate(&iface->mlmepriv, WIFI_OP_CH_SWITCHING); + } + } + + } else if (MSTATE_STA_LD_NUM(&mstate_others) + || MSTATE_AP_NUM(&mstate_others) || MSTATE_MESH_NUM(&mstate_others) + ) { + /* has linked STA mode or AP/Mesh mode */ + + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + iface = dvobj->padapters[i]; + + rtw_adjust_chbw(iface, u_ch, &dec_bw[i], &dec_offset[i]); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(iface)) + rtw_mesh_adjust_chbw(u_ch, &dec_bw[i], &dec_offset[i]); + #endif + + #ifdef CONFIG_MCC_MODE + if (MCC_EN(iface)) { + if (!rtw_is_chbw_grouped(u_ch, u_bw, u_offset, dec_ch[i], dec_bw[i], dec_offset[i])) { + mlmeext = &(iface->mlmeextpriv); + mlmeext->cur_channel = *ch = dec_ch[i] = cur_ie_ch[i]; + mlmeext->cur_bwmode = *bw = dec_bw[i] = cur_ie_bw[i]; + mlmeext->cur_ch_offset = *offset = dec_offset[i] = cur_ie_offset[i]; + /* channel bw offset can not be allowed, need MCC */ + *chbw_allow = _FALSE; + RTW_INFO(FUNC_ADPT_FMT" enable mcc: %u,%u,%u\n", FUNC_ADPT_ARG(iface) + , *ch, *bw, *offset); + goto exit; + } else + /* channel bw offset can be allowed, not need MCC */ + *chbw_allow = _TRUE; + } + #endif /* CONFIG_MCC_MODE */ + + if (req_ch == 0 && dec_bw[i] > u_bw + && rtw_chset_is_dfs_chbw(chset, u_ch, u_bw, u_offset) + ) { + /* request comes from upper layer, prevent from additional channel waiting */ + dec_bw[i] = u_bw; + if (dec_bw[i] == CHANNEL_WIDTH_20) + dec_offset[i] = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + /* follow */ + rtw_chset_sync_chbw(chset + , &dec_ch[i], &dec_bw[i], &dec_offset[i] + , &u_ch, &u_bw, &u_offset, 1, 0); + } + + *set_u_ch = _TRUE; + + } else { + /* autonomous decision */ + u8 ori_ch = 0; + u8 max_bw; + bool by_int_info; + + /* autonomous decision, not need MCC */ + *chbw_allow = _TRUE; + + if (req_ch <= REQ_CH_NONE) /* channel is not specified */ + goto choose_chbw; + + /* get tmp dec union of ifbmp */ + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + if (u_ch == 0) { + u_ch = dec_ch[i]; + u_bw = dec_bw[i]; + u_offset = dec_offset[i]; + rtw_adjust_chbw(adapter, u_ch, &u_bw, &u_offset); + rtw_get_offset_by_chbw(u_ch, u_bw, &u_offset); + } else { + u8 tmp_ch = dec_ch[i]; + u8 tmp_bw = dec_bw[i]; + u8 tmp_offset = dec_offset[i]; + + rtw_adjust_chbw(adapter, tmp_ch, &tmp_bw, &tmp_offset); + rtw_get_offset_by_chbw(tmp_ch, tmp_bw, &tmp_offset); + + rtw_warn_on(!rtw_is_chbw_grouped(u_ch, u_bw, u_offset, tmp_ch, tmp_bw, tmp_offset)); + rtw_sync_chbw(&tmp_ch, &tmp_bw, &tmp_offset, &u_ch, &u_bw, &u_offset); + } + } + + #ifdef CONFIG_RTW_MESH + /* if ifbmp are all mesh, apply bw restriction */ + if (ifbmp_all_mesh) + rtw_mesh_adjust_chbw(u_ch, &u_bw, &u_offset); + #endif + + RTW_INFO("%s ifbmp:0x%02x tmp union:%u,%u,%u\n", __func__, ifbmp, u_ch, u_bw, u_offset); + + /* check if tmp dec union is usable */ + if (rtw_ap_ch_specific_chk(adapter, u_ch, &u_bw, &u_offset, __func__) == _FAIL) { + /* channel can't be used */ + if (req_ch > 0) { + /* specific channel and not from IE => don't change channel setting */ + goto exit; + } + goto choose_chbw; + } else if (rtw_chset_is_chbw_non_ocp(chset, u_ch, u_bw, u_offset)) { + RTW_WARN("%s DFS channel %u,%u under non ocp\n", __func__, u_ch, u_bw); + if (req_ch > 0 && req_bw > REQ_BW_NONE) { + /* change_chbw with specific channel and specific bw, goto update_bss_chbw directly */ + goto update_bss_chbw; + } + } else + goto update_bss_chbw; + +choose_chbw: + by_int_info = req_ch == REQ_CH_INT_INFO ? 1 : 0; + req_ch = req_ch > 0 ? req_ch : 0; + max_bw = req_bw > REQ_BW_NONE ? req_bw : CHANNEL_WIDTH_20; + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + iface = dvobj->padapters[i]; + mlmeext = &(iface->mlmeextpriv); + + if (req_bw <= REQ_BW_NONE) { + if (req_bw == REQ_BW_ORI) { + if (max_bw < iface->mlmepriv.ori_bw) + max_bw = iface->mlmepriv.ori_bw; + } else { + if (max_bw < cur_ie_bw[i]) + max_bw = cur_ie_bw[i]; + } + } + + if (MSTATE_AP_NUM(&mstate) || MSTATE_MESH_NUM(&mstate)) { + if (ori_ch == 0) + ori_ch = mlmeext->cur_channel; + else if (ori_ch != mlmeext->cur_channel) + rtw_warn_on(1); + } else { + if (ori_ch == 0) + ori_ch = cur_ie_ch[i]; + else if (ori_ch != cur_ie_ch[i]) + rtw_warn_on(1); + } + } + + ch_avail = rtw_ap_choose_chbw(adapter, req_ch, max_bw + , ori_ch, &u_ch, &u_bw, &u_offset, by_int_info, ifbmp_all_mesh, __func__); + if (ch_avail == _FALSE) + goto exit; + +update_bss_chbw: + for (i = 0; i < dvobj->iface_nums; i++) { + if (!(ifbmp & BIT(i)) || !dvobj->padapters[i]) + continue; + iface = dvobj->padapters[i]; + + dec_ch[i] = u_ch; + if (dec_bw[i] > u_bw) + dec_bw[i] = u_bw; + if (dec_bw[i] == CHANNEL_WIDTH_20) + dec_offset[i] = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + else + dec_offset[i] = u_offset; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(iface)) + rtw_mesh_adjust_chbw(dec_ch[i], &dec_bw[i], &dec_offset[i]); + #endif + } + + *set_u_ch = _TRUE; + } + + ifbmp_ch_changed = rtw_ap_update_chbw_by_ifbmp(dvobj, ifbmp + , cur_ie_ch, cur_ie_bw, cur_ie_offset + , dec_ch, dec_bw, dec_offset + , __func__); + + if (u_ch != 0) + RTW_INFO("%s union:%u,%u,%u\n", __func__, u_ch, u_bw, u_offset); + + if (*set_u_ch == _TRUE) { + rtw_mi_update_union_chan_inf(adapter, u_ch, u_offset, u_bw); + *ch = u_ch; + *bw = u_bw; + *offset = u_offset; + } + + if (rtw_mi_check_fwstate(adapter, WIFI_UNDER_SURVEY)) { + /* scanning, leave ch setting to scan state machine */ + *set_u_ch = _FALSE; + } + +exit: + return ifbmp_ch_changed; +} + +u8 rtw_ap_sta_states_check(_adapter *adapter) +{ + struct sta_info *psta; + struct sta_priv *pstapriv = &adapter->stapriv; + _list *plist, *phead; + _irqL irqL; + u8 rst = _FALSE; + + if (!MLME_IS_AP(adapter) && !MLME_IS_MESH(adapter)) + return _FALSE; + + if (pstapriv->auth_list_cnt !=0) + return _TRUE; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + if (!(psta->state & WIFI_ASOC_STATE)) { + RTW_INFO(ADPT_FMT"- SoftAP/Mesh - sta under linking, its state = 0x%x\n", ADPT_ARG(adapter), psta->state); + rst = _TRUE; + break; + } else if (psta->state & WIFI_UNDER_KEY_HANDSHAKE) { + RTW_INFO(ADPT_FMT"- SoftAP/Mesh - sta under key handshaking, its state = 0x%x\n", ADPT_ARG(adapter), psta->state); + rst = _TRUE; + break; + } + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + return rst; +} + +/*#define DBG_SWTIMER_BASED_TXBCN*/ +#ifdef CONFIG_SWTIMER_BASED_TXBCN +void tx_beacon_handlder(struct dvobj_priv *pdvobj) +{ +#define BEACON_EARLY_TIME 20 /* unit:TU*/ + _irqL irqL; + _list *plist, *phead; + u32 timestamp[2]; + u32 bcn_interval_us; /* unit : usec */ + u64 time; + u32 cur_tick, time_offset; /* unit : usec */ + u32 inter_bcn_space_us; /* unit : usec */ + u32 txbcn_timer_ms; /* unit : ms */ + int nr_vap, idx, bcn_idx; + int i; + u8 val8, late = 0; + _adapter *padapter = NULL; + + i = 0; + + /* get first ap mode interface */ + _enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + if (rtw_is_list_empty(&pdvobj->ap_if_q.queue) || (pdvobj->nr_ap_if == 0)) { + RTW_INFO("[%s] ERROR: ap_if_q is empty!or nr_ap = %d\n", __func__, pdvobj->nr_ap_if); + _exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + return; + } else + padapter = LIST_CONTAINOR(get_next(&(pdvobj->ap_if_q.queue)), struct _ADAPTER, list); + _exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + + if (NULL == padapter) { + RTW_INFO("[%s] ERROR: no any ap interface!\n", __func__); + return; + } + + + bcn_interval_us = DEFAULT_BCN_INTERVAL * NET80211_TU_TO_US; + if (0 == bcn_interval_us) { + RTW_INFO("[%s] ERROR: beacon interval = 0\n", __func__); + return; + } + + /* read TSF */ + timestamp[1] = rtw_read32(padapter, 0x560 + 4); + timestamp[0] = rtw_read32(padapter, 0x560); + while (timestamp[1]) { + time = (0xFFFFFFFF % bcn_interval_us + 1) * timestamp[1] + timestamp[0]; + timestamp[0] = (u32)time; + timestamp[1] = (u32)(time >> 32); + } + cur_tick = timestamp[0] % bcn_interval_us; + + + _enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + + nr_vap = (pdvobj->nr_ap_if - 1); + if (nr_vap > 0) { + inter_bcn_space_us = pdvobj->inter_bcn_space * NET80211_TU_TO_US; /* beacon_interval / (nr_vap+1); */ + idx = cur_tick / inter_bcn_space_us; + if (idx < nr_vap) /* if (idx < (nr_vap+1))*/ + bcn_idx = idx + 1; /* bcn_idx = (idx + 1) % (nr_vap+1);*/ + else + bcn_idx = 0; + + /* to get padapter based on bcn_idx */ + padapter = NULL; + phead = get_list_head(&pdvobj->ap_if_q); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + padapter = LIST_CONTAINOR(plist, struct _ADAPTER, list); + + plist = get_next(plist); + + if (i == bcn_idx) + break; + + i++; + } + if ((NULL == padapter) || (i > pdvobj->nr_ap_if)) { + RTW_INFO("[%s] ERROR: nr_ap_if = %d, padapter=%p, bcn_idx=%d, index=%d\n", + __func__, pdvobj->nr_ap_if, padapter, bcn_idx, i); + _exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + return; + } +#ifdef DBG_SWTIMER_BASED_TXBCN + RTW_INFO("BCN_IDX=%d, cur_tick=%d, padapter=%p\n", bcn_idx, cur_tick, padapter); +#endif + if (((idx + 2 == nr_vap + 1) && (idx < nr_vap + 1)) || (0 == bcn_idx)) { + time_offset = bcn_interval_us - cur_tick - BEACON_EARLY_TIME * NET80211_TU_TO_US; + if ((s32)time_offset < 0) + time_offset += inter_bcn_space_us; + + } else { + time_offset = (idx + 2) * inter_bcn_space_us - cur_tick - BEACON_EARLY_TIME * NET80211_TU_TO_US; + if (time_offset > (inter_bcn_space_us + (inter_bcn_space_us >> 1))) { + time_offset -= inter_bcn_space_us; + late = 1; + } + } + } else + /*#endif*/ { /* MBSSID */ + time_offset = 2 * bcn_interval_us - cur_tick - BEACON_EARLY_TIME * NET80211_TU_TO_US; + if (time_offset > (bcn_interval_us + (bcn_interval_us >> 1))) { + time_offset -= bcn_interval_us; + late = 1; + } + } + _exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL); + +#ifdef DBG_SWTIMER_BASED_TXBCN + RTW_INFO("set sw bcn timer %d us\n", time_offset); +#endif + txbcn_timer_ms = time_offset / NET80211_TU_TO_US; + _set_timer(&pdvobj->txbcn_timer, txbcn_timer_ms); + + if (padapter) { +#ifdef CONFIG_BCN_RECOVERY + rtw_ap_bcn_recovery(padapter); +#endif /*CONFIG_BCN_RECOVERY*/ + +#ifdef CONFIG_BCN_XMIT_PROTECT + rtw_ap_bcn_queue_empty_check(padapter, txbcn_timer_ms); +#endif /*CONFIG_BCN_XMIT_PROTECT*/ + +#ifdef DBG_SWTIMER_BASED_TXBCN + RTW_INFO("padapter=%p, PORT=%d\n", padapter, padapter->hw_port); +#endif + /* bypass TX BCN queue if op ch is switching/waiting */ + if (!check_fwstate(&padapter->mlmepriv, WIFI_OP_CH_SWITCHING) + && !IS_CH_WAITING(adapter_to_rfctl(padapter)) + ) { + /*update_beacon(padapter, _TIM_IE_, NULL, _FALSE, 0);*/ + /*issue_beacon(padapter, 0);*/ + send_beacon(padapter); + } + } + +#if 0 + /* handle any buffered BC/MC frames*/ + /* Don't dynamically change DIS_ATIM due to HW will auto send ACQ after HIQ empty.*/ + val8 = *((unsigned char *)priv->beaconbuf + priv->timoffset + 4); + if (val8 & 0x01) { + process_mcast_dzqueue(priv); + priv->pkt_in_dtimQ = 0; + } +#endif + +} + +void tx_beacon_timer_handlder(void *ctx) +{ + struct dvobj_priv *pdvobj = (struct dvobj_priv *)ctx; + _adapter *padapter = pdvobj->padapters[0]; + + if (padapter) + set_tx_beacon_cmd(padapter, 0); +} +#endif + +void rtw_ap_parse_sta_capability(_adapter *adapter, struct sta_info *sta, u8 *cap) +{ + sta->capability = RTW_GET_LE16(cap); + if (sta->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) + sta->flags |= WLAN_STA_SHORT_PREAMBLE; + else + sta->flags &= ~WLAN_STA_SHORT_PREAMBLE; +} + +u16 rtw_ap_parse_sta_supported_rates(_adapter *adapter, struct sta_info *sta, u8 *tlv_ies, u16 tlv_ies_len) +{ + u8 rate_set[12]; + u8 rate_num; + int i; + u16 status = _STATS_SUCCESSFUL_; + + rtw_ies_get_supported_rate(tlv_ies, tlv_ies_len, rate_set, &rate_num); + if (rate_num == 0) { + RTW_INFO(FUNC_ADPT_FMT" sta "MAC_FMT" with no supported rate\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + status = _STATS_FAILURE_; + goto exit; + } + + _rtw_memcpy(sta->bssrateset, rate_set, rate_num); + sta->bssratelen = rate_num; + + if (MLME_IS_AP(adapter)) { + /* this function force only CCK rates to be bassic rate... */ + UpdateBrateTblForSoftAP(sta->bssrateset, sta->bssratelen); + } + + /* if (hapd->iface->current_mode->mode == HOSTAPD_MODE_IEEE80211G) */ /* ? */ + sta->flags |= WLAN_STA_NONERP; + for (i = 0; i < sta->bssratelen; i++) { + if ((sta->bssrateset[i] & 0x7f) > 22) { + sta->flags &= ~WLAN_STA_NONERP; + break; + } + } + +exit: + return status; +} + +u16 rtw_ap_parse_sta_security_ie(_adapter *adapter, struct sta_info *sta, struct rtw_ieee802_11_elems *elems) +{ + struct security_priv *sec = &adapter->securitypriv; + u8 *wpa_ie; + int wpa_ie_len; + int group_cipher = 0, pairwise_cipher = 0, gmcs = 0; + u32 akm = 0; + u8 mfp_opt = MFP_NO; + u8 spp_opt = 0; + u16 status = _STATS_SUCCESSFUL_; + + sta->dot8021xalg = 0; + sta->wpa_psk = 0; + sta->wpa_group_cipher = 0; + sta->wpa2_group_cipher = 0; + sta->wpa_pairwise_cipher = 0; + sta->wpa2_pairwise_cipher = 0; + _rtw_memset(sta->wpa_ie, 0, sizeof(sta->wpa_ie)); + + if ((sec->wpa_psk & BIT(1)) && elems->rsn_ie) { + wpa_ie = elems->rsn_ie; + wpa_ie_len = elems->rsn_ie_len; + + if (rtw_parse_wpa2_ie(wpa_ie - 2, wpa_ie_len + 2, &group_cipher, &pairwise_cipher, &gmcs, &akm, &mfp_opt, &spp_opt) == _SUCCESS) { + sta->dot8021xalg = 1;/* psk, todo:802.1x */ + sta->wpa_psk |= BIT(1); + + sta->wpa2_group_cipher = group_cipher & sec->wpa2_group_cipher; + sta->wpa2_pairwise_cipher = pairwise_cipher & sec->wpa2_pairwise_cipher; + + sta->akm_suite_type = akm; + if (MLME_IS_AP(adapter) && (CHECK_BIT(WLAN_AKM_TYPE_SAE, akm)) && (MFP_NO == mfp_opt)) { + status = WLAN_STATUS_ROBUST_MGMT_FRAME_POLICY_VIOLATION; + goto exit; + } + + if (MLME_IS_AP(adapter) && (!CHECK_BIT(sec->akmp, akm))) { + status = WLAN_STATUS_AKMP_NOT_VALID; + goto exit; + } + + if (!sta->wpa2_group_cipher) { + status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID; + goto exit; + } + + if (!sta->wpa2_pairwise_cipher) { + status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID; + goto exit; + } + + } else { + status = WLAN_STATUS_INVALID_IE; + goto exit; + } + + } + else if ((sec->wpa_psk & BIT(0)) && elems->wpa_ie) { + wpa_ie = elems->wpa_ie; + wpa_ie_len = elems->wpa_ie_len; + + if (rtw_parse_wpa_ie(wpa_ie - 2, wpa_ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) { + sta->dot8021xalg = 1;/* psk, todo:802.1x */ + sta->wpa_psk |= BIT(0); + + sta->wpa_group_cipher = group_cipher & sec->wpa_group_cipher; + sta->wpa_pairwise_cipher = pairwise_cipher & sec->wpa_pairwise_cipher; + + if (!sta->wpa_group_cipher) { + status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID; + goto exit; + } + + if (!sta->wpa_pairwise_cipher) { + status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID; + goto exit; + } + } else { + status = WLAN_STATUS_INVALID_IE; + goto exit; + } + + } else { + wpa_ie = NULL; + wpa_ie_len = 0; + } + if (sec->dot11PrivacyAlgrthm != _NO_PRIVACY_) { + /*check if amsdu is allowed */ + if (rtw_check_amsdu_disable(adapter->registrypriv.amsdu_mode, spp_opt) == _TRUE) + sta->flags |= WLAN_STA_AMSDU_DISABLE; + } + + if ((sec->mfp_opt == MFP_REQUIRED && mfp_opt < MFP_OPTIONAL) + || (mfp_opt == MFP_REQUIRED && sec->mfp_opt < MFP_OPTIONAL) + ) { + status = WLAN_STATUS_ROBUST_MGMT_FRAME_POLICY_VIOLATION; + goto exit; + } + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + /* MFP is mandatory for secure mesh */ + if (adapter->mesh_info.mesh_auth_id) + sta->flags |= WLAN_STA_MFP; + } else +#endif + if (sec->mfp_opt >= MFP_OPTIONAL && mfp_opt >= MFP_OPTIONAL) + sta->flags |= WLAN_STA_MFP; + +#ifdef CONFIG_IEEE80211W + if ((sta->flags & WLAN_STA_MFP) + && (sec->mfp_opt >= MFP_OPTIONAL && mfp_opt >= MFP_OPTIONAL) + && security_type_bip_to_gmcs(sec->dot11wCipher) != gmcs + ) { + status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; + goto exit; + } +#endif + +#ifdef CONFIG_IOCTL_CFG80211 + if (MLME_IS_AP(adapter) && + (sec->auth_type == MLME_AUTHTYPE_SAE) && + (CHECK_BIT(WLAN_AKM_TYPE_SAE, sta->akm_suite_type)) && + (WLAN_AUTH_OPEN == sta->authalg)) { + /* WPA3-SAE, PMK caching */ + if (rtw_cached_pmkid(adapter, sta->cmn.mac_addr) == -1) { + RTW_INFO("SAE: No PMKSA cache entry found\n"); + status = WLAN_STATUS_INVALID_PMKID; + goto exit; + } else { + RTW_INFO("SAE: PMKSA cache entry found\n"); + } + } +#endif /* CONFIG_IOCTL_CFG80211 */ + + if (!MLME_IS_AP(adapter)) + goto exit; + + sta->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS); + /* if (hapd->conf->wps_state && wpa_ie == NULL) { */ /* todo: to check ap if supporting WPS */ + if (wpa_ie == NULL) { + if (elems->wps_ie) { + RTW_INFO("STA included WPS IE in " + "(Re)Association Request - assume WPS is " + "used\n"); + sta->flags |= WLAN_STA_WPS; + /* wpabuf_free(sta->wps_ie); */ + /* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */ + /* elems.wps_ie_len - 4); */ + } else { + RTW_INFO("STA did not include WPA/RSN IE " + "in (Re)Association Request - possible WPS " + "use\n"); + sta->flags |= WLAN_STA_MAYBE_WPS; + } + + /* AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */ + /* that the selected registrar of AP is _FLASE */ + if ((sec->wpa_psk > 0) + && (sta->flags & (WLAN_STA_WPS | WLAN_STA_MAYBE_WPS)) + ) { + struct mlme_priv *mlme = &adapter->mlmepriv; + + if (mlme->wps_beacon_ie) { + u8 selected_registrar = 0; + + rtw_get_wps_attr_content(mlme->wps_beacon_ie, mlme->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL); + + if (!selected_registrar) { + RTW_INFO("selected_registrar is _FALSE , or AP is not ready to do WPS\n"); + status = _STATS_UNABLE_HANDLE_STA_; + goto exit; + } + } + } + + } else { + int copy_len; + + if (sec->wpa_psk == 0) { + RTW_INFO("STA " MAC_FMT + ": WPA/RSN IE in association request, but AP don't support WPA/RSN\n", + MAC_ARG(sta->cmn.mac_addr)); + status = WLAN_STATUS_INVALID_IE; + goto exit; + } + + if (elems->wps_ie) { + RTW_INFO("STA included WPS IE in " + "(Re)Association Request - WPS is " + "used\n"); + sta->flags |= WLAN_STA_WPS; + copy_len = 0; + } else + copy_len = ((wpa_ie_len + 2) > sizeof(sta->wpa_ie)) ? (sizeof(sta->wpa_ie)) : (wpa_ie_len + 2); + + if (copy_len > 0) + _rtw_memcpy(sta->wpa_ie, wpa_ie - 2, copy_len); + } + +exit: + return status; +} + +void rtw_ap_parse_sta_wmm_ie(_adapter *adapter, struct sta_info *sta, u8 *tlv_ies, u16 tlv_ies_len) +{ + struct mlme_priv *mlme = &adapter->mlmepriv; + unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01}; + u8 *p; + + sta->flags &= ~WLAN_STA_WME; + sta->qos_option = 0; + sta->qos_info = 0; + sta->has_legacy_ac = _TRUE; + sta->uapsd_vo = 0; + sta->uapsd_vi = 0; + sta->uapsd_be = 0; + sta->uapsd_bk = 0; + + if (!mlme->qospriv.qos_option) + goto exit; + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + /* QoS is mandatory in mesh */ + sta->flags |= WLAN_STA_WME; + } +#endif + + p = rtw_get_ie_ex(tlv_ies, tlv_ies_len, WLAN_EID_VENDOR_SPECIFIC, WMM_IE, 6, NULL, NULL); + if (!p) + goto exit; + + sta->flags |= WLAN_STA_WME; + sta->qos_option = 1; + sta->qos_info = *(p + 8); + sta->max_sp_len = (sta->qos_info >> 5) & 0x3; + + if ((sta->qos_info & 0xf) != 0xf) + sta->has_legacy_ac = _TRUE; + else + sta->has_legacy_ac = _FALSE; + + if (sta->qos_info & 0xf) { + if (sta->qos_info & BIT(0)) + sta->uapsd_vo = BIT(0) | BIT(1); + else + sta->uapsd_vo = 0; + + if (sta->qos_info & BIT(1)) + sta->uapsd_vi = BIT(0) | BIT(1); + else + sta->uapsd_vi = 0; + + if (sta->qos_info & BIT(2)) + sta->uapsd_bk = BIT(0) | BIT(1); + else + sta->uapsd_bk = 0; + + if (sta->qos_info & BIT(3)) + sta->uapsd_be = BIT(0) | BIT(1); + else + sta->uapsd_be = 0; + } + +exit: + return; +} + +void rtw_ap_parse_sta_ht_ie(_adapter *adapter, struct sta_info *sta, struct rtw_ieee802_11_elems *elems) +{ + struct mlme_priv *mlme = &adapter->mlmepriv; + + sta->flags &= ~WLAN_STA_HT; + +#ifdef CONFIG_80211N_HT + if (mlme->htpriv.ht_option == _FALSE) + goto exit; + + /* save HT capabilities in the sta object */ + _rtw_memset(&sta->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap)); + if (elems->ht_capabilities && elems->ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) { + sta->flags |= WLAN_STA_HT; + sta->flags |= WLAN_STA_WME; + _rtw_memcpy(&sta->htpriv.ht_cap, elems->ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap)); + + if (elems->ht_operation && elems->ht_operation_len == HT_OP_IE_LEN) { + _rtw_memcpy(sta->htpriv.ht_op, elems->ht_operation, HT_OP_IE_LEN); + sta->htpriv.op_present = 1; + } + } +exit: +#endif + + return; +} + +void rtw_ap_parse_sta_vht_ie(_adapter *adapter, struct sta_info *sta, struct rtw_ieee802_11_elems *elems) +{ + struct mlme_priv *mlme = &adapter->mlmepriv; + + sta->flags &= ~WLAN_STA_VHT; + +#ifdef CONFIG_80211AC_VHT + if (mlme->vhtpriv.vht_option == _FALSE) + goto exit; + + _rtw_memset(&sta->vhtpriv, 0, sizeof(struct vht_priv)); + if (elems->vht_capabilities && elems->vht_capabilities_len == VHT_CAP_IE_LEN) { + sta->flags |= WLAN_STA_VHT; + _rtw_memcpy(sta->vhtpriv.vht_cap, elems->vht_capabilities, VHT_CAP_IE_LEN); + + if (elems->vht_operation && elems->vht_operation_len== VHT_OP_IE_LEN) { + _rtw_memcpy(sta->vhtpriv.vht_op, elems->vht_operation, VHT_OP_IE_LEN); + sta->vhtpriv.op_present = 1; + } + + if (elems->vht_op_mode_notify && elems->vht_op_mode_notify_len == 1) { + _rtw_memcpy(&sta->vhtpriv.vht_op_mode_notify, elems->vht_op_mode_notify, 1); + sta->vhtpriv.notify_present = 1; + } + } +exit: +#endif + + return; +} + +void rtw_ap_parse_sta_multi_ap_ie(_adapter *adapter, struct sta_info *sta, u8 *ies, int ies_len) +{ + sta->flags &= ~WLAN_STA_MULTI_AP; + +#ifdef CONFIG_RTW_MULTI_AP + if (adapter->multi_ap + && (rtw_get_multi_ap_ie_ext(ies, ies_len) & MULTI_AP_BACKHAUL_STA) + ) { + if (adapter->multi_ap & MULTI_AP_BACKHAUL_BSS) /* with backhaul bss, enable WDS */ + sta->flags |= WLAN_STA_MULTI_AP | WLAN_STA_WDS; + else if (adapter->multi_ap & MULTI_AP_FRONTHAUL_BSS) /* fronthaul bss only */ + sta->flags |= WLAN_STA_MULTI_AP; + } +#endif +} + +#if CONFIG_RTW_AP_DATA_BMC_TO_UC +static bool rtw_ap_data_bmc_to_uc(_adapter *adapter + , const u8 *da, const u8 *sa, const u8 *ori_ta + , u16 os_qid, _list *b2u_list) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct xmit_priv *xmitpriv = &adapter->xmitpriv; + _irqL irqL; + _list *head, *list; + struct sta_info *sta; + char b2u_sta_id[NUM_STA]; + u8 b2u_sta_num = 0; + bool bmc_need = _FALSE; + int i; + + _enter_critical_bh(&stapriv->asoc_list_lock, &irqL); + head = &stapriv->asoc_list; + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + int stainfo_offset; + + sta = LIST_CONTAINOR(list, struct sta_info, asoc_list); + list = get_next(list); + + stainfo_offset = rtw_stainfo_offset(stapriv, sta); + if (stainfo_offset_valid(stainfo_offset)) + b2u_sta_id[b2u_sta_num++] = stainfo_offset; + } + _exit_critical_bh(&stapriv->asoc_list_lock, &irqL); + + if (!b2u_sta_num) + goto exit; + + for (i = 0; i < b2u_sta_num; i++) { + struct xmit_frame *b2uframe; + struct pkt_attrib *attrib; + + sta = rtw_get_stainfo_by_offset(stapriv, b2u_sta_id[i]); + if (!(sta->state & WIFI_ASOC_STATE) + || _rtw_memcmp(sta->cmn.mac_addr, sa, ETH_ALEN) == _TRUE + || (ori_ta && _rtw_memcmp(sta->cmn.mac_addr, ori_ta, ETH_ALEN) == _TRUE) + || is_broadcast_mac_addr(sta->cmn.mac_addr) + || is_zero_mac_addr(sta->cmn.mac_addr)) + continue; + + b2uframe = rtw_alloc_xmitframe(xmitpriv, os_qid); + if (!b2uframe) { + bmc_need = _TRUE; + break; + } + + attrib = &b2uframe->attrib; + + _rtw_memcpy(attrib->ra, sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(attrib->ta, adapter_mac_addr(adapter), ETH_ALEN); + #ifdef CONFIG_RTW_WDS + if (adapter_use_wds(adapter) && (sta->flags & WLAN_STA_WDS)) { + _rtw_memcpy(attrib->dst, da, ETH_ALEN); + attrib->wds = 1; + } else + #endif + _rtw_memcpy(attrib->dst, attrib->ra, ETH_ALEN); + _rtw_memcpy(attrib->src, sa, ETH_ALEN); + + rtw_list_insert_tail(&b2uframe->list, b2u_list); + } + +exit: + return bmc_need; +} + +void dump_ap_b2u_flags(void *sel, _adapter *adapter) +{ + RTW_PRINT_SEL(sel, "%4s %4s\n", "src", "fwd"); + RTW_PRINT_SEL(sel, "0x%02x 0x%02x\n", adapter->b2u_flags_ap_src, adapter->b2u_flags_ap_fwd); +} +#endif /* CONFIG_RTW_AP_DATA_BMC_TO_UC */ + +static int rtw_ap_nexthop_resolve(_adapter *adapter, struct xmit_frame *xframe) +{ + struct pkt_attrib *attrib = &xframe->attrib; + int ret = _SUCCESS; + +#ifdef CONFIG_RTW_WDS + if (adapter_use_wds(adapter) + && rtw_wds_nexthop_lookup(adapter, attrib->dst, attrib->ra) == 0 + ) { + if (_rtw_memcmp(attrib->dst, attrib->ra, ETH_ALEN) == _FALSE) + attrib->wds = 1; + } else +#endif + _rtw_memcpy(attrib->ra, attrib->dst, ETH_ALEN); + + return ret; +} + +int rtw_ap_addr_resolve(_adapter *adapter, u16 os_qid, struct xmit_frame *xframe, _pkt *pkt, _list *b2u_list) +{ + struct pkt_file pktfile; + struct ethhdr etherhdr; + struct pkt_attrib *attrib; + struct rtw_mesh_path *mpath = NULL, *mppath = NULL; + u8 is_da_mcast; + u8 addr4_need; +#if CONFIG_RTW_AP_DATA_BMC_TO_UC + bool bmc_need = _TRUE; +#endif + int res = _SUCCESS; + + _rtw_open_pktfile(pkt, &pktfile); + if (_rtw_pktfile_read(&pktfile, (u8 *)ðerhdr, ETH_HLEN) != ETH_HLEN) { + res = _FAIL; + goto exit; + } + + xframe->pkt = pkt; +#if CONFIG_RTW_AP_DATA_BMC_TO_UC + _rtw_init_listhead(b2u_list); +#endif + + is_da_mcast = IS_MCAST(etherhdr.h_dest); + if (is_da_mcast) { + #if CONFIG_RTW_AP_DATA_BMC_TO_UC + if (rtw_ap_src_b2u_policy_chk(adapter->b2u_flags_ap_src, etherhdr.h_dest) + && adapter->registrypriv.wifi_spec == 0 + && adapter->xmitpriv.free_xmitframe_cnt > (NR_XMITFRAME / 4) + ) { + bmc_need = rtw_ap_data_bmc_to_uc(adapter + , etherhdr.h_dest, etherhdr.h_source, NULL, os_qid, b2u_list); + if (bmc_need == _FALSE) { + res = RTW_BMC_NO_NEED; + goto exit; + } + } + #endif + } + + attrib = &xframe->attrib; + + _rtw_memcpy(attrib->dst, etherhdr.h_dest, ETH_ALEN); + _rtw_memcpy(attrib->src, etherhdr.h_source, ETH_ALEN); + _rtw_memcpy(attrib->ta, adapter_mac_addr(adapter), ETH_ALEN); + + if (is_da_mcast) + _rtw_memcpy(attrib->ra, attrib->dst, ETH_ALEN); + else + res = rtw_ap_nexthop_resolve(adapter, xframe); + +exit: + return res; +} + +int rtw_ap_rx_data_validate_hdr(_adapter *adapter, union recv_frame *rframe, struct sta_info **sta) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + u8 *whdr = get_recvframe_data(rframe); + u8 is_ra_bmc = 0; + sint ret = _FAIL; + + if (!(MLME_STATE(adapter) & WIFI_ASOC_STATE)) + goto exit; + + switch (rattrib->to_fr_ds) { + case 1: + if (IS_MCAST(GetAddr1Ptr(whdr))) + goto exit; + _rtw_memcpy(rattrib->ra, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->ta, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->dst, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->src, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->bssid, GetAddr1Ptr(whdr), ETH_ALEN); + break; + case 3: + is_ra_bmc = IS_MCAST(GetAddr1Ptr(whdr)) ? 1 : 0; + _rtw_memcpy(rattrib->ra, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->ta, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->dst, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->src, GetAddr4Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + if (!is_ra_bmc) + _rtw_memcpy(rattrib->bssid, GetAddr1Ptr(whdr), ETH_ALEN); + break; + default: + ret = RTW_RX_HANDLED; /* don't count for drop */ + goto exit; + } + + *sta = rtw_get_stainfo(stapriv, rattrib->ta); + if (*sta == NULL) { + if (!is_ra_bmc && !IS_RADAR_DETECTED(adapter_to_rfctl(adapter))) { + #ifndef CONFIG_CUSTOMER_ALIBABA_GENERAL + RTW_INFO(FUNC_ADPT_FMT" issue_deauth to "MAC_FMT" with reason(7), unknown TA\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(rattrib->ta)); + issue_deauth(adapter, rattrib->ta, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + #endif + } + ret = RTW_RX_HANDLED; + goto exit; + } + +#ifdef CONFIG_RTW_WDS_AUTO_EN + if (rattrib->to_fr_ds == 3 && !(sta->flags & WLAN_STA_WDS)) + sta->flags |= WLAN_STA_WDS; +#endif + + process_pwrbit_data(adapter, rframe, *sta); + + if ((get_frame_sub_type(whdr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) + process_wmmps_data(adapter, rframe, *sta); + + if (get_frame_sub_type(whdr) & BIT(6)) { + /* No data, will not indicate to upper layer, temporily count it here */ + count_rx_stats(adapter, rframe, *sta); + ret = RTW_RX_HANDLED; + goto exit; + } + + ret = _SUCCESS; + +exit: + return ret; +} + +int rtw_ap_rx_msdu_act_check(union recv_frame *rframe + , const u8 *da, const u8 *sa + , u8 *msdu, enum rtw_rx_llc_hdl llc_hdl + , struct xmit_frame **fwd_frame, _list *b2u_list) +{ + _adapter *adapter = rframe->u.hdr.adapter; + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + struct rtw_wds_path *wpath; + u8 is_da_bmc = IS_MCAST(da); + u8 is_da_self = !is_da_bmc && _rtw_memcmp(da, adapter_mac_addr(adapter), ETH_ALEN); + u8 is_da_peer = 0; + int in_wds_tbl = 0; + u16 os_qid; + struct xmit_frame *xframe; + struct pkt_attrib *xattrib; + u8 fwd_ra[ETH_ALEN] = {0}; + int act = 0; +#if CONFIG_RTW_AP_DATA_BMC_TO_UC + bool bmc_need = _TRUE; +#endif + +#ifdef CONFIG_RTW_WDS + /* update/create wds info for SA, RA */ + if (adapter_use_wds(adapter) + && (rframe->u.hdr.psta->state & WIFI_ASOC_STATE) + && _rtw_memcmp(sa, rframe->u.hdr.psta->cmn.mac_addr, ETH_ALEN) == _FALSE + ) { + rtw_rcu_read_lock(); + wpath = rtw_wds_path_lookup(adapter, sa); + if (!wpath) + rtw_wds_path_add(adapter, sa, rframe->u.hdr.psta); + else { + rtw_wds_path_assign_nexthop(wpath, rframe->u.hdr.psta); + wpath->last_update = rtw_get_current_time(); + } + rtw_rcu_read_unlock(); + } +#endif + + /* SA is self, need no further process */ + if (_rtw_memcmp(sa, adapter_mac_addr(adapter), ETH_ALEN) == _TRUE) + goto exit; + + if (is_da_bmc) { + /* DA is bmc addr */ + act |= RTW_RX_MSDU_ACT_INDICATE; + if (adapter->mlmepriv.ap_isolate) + goto exit; + goto fwd_chk; + + } + + if (is_da_self) { + /* DA is self, indicate */ + act |= RTW_RX_MSDU_ACT_INDICATE; + goto exit; + } + + /* DA is not self */ +#ifdef CONFIG_RTW_WDS + if (adapter_use_wds(adapter)) + in_wds_tbl = rtw_wds_nexthop_lookup(adapter, da, fwd_ra) == 0; +#endif + if (!in_wds_tbl) + is_da_peer = rtw_get_stainfo(&adapter->stapriv, da) ? 1 : 0; + + if (in_wds_tbl || is_da_peer) { + /* DA is known (peer or can be forwarded by peer) */ + if (adapter->mlmepriv.ap_isolate) { + #if defined(DBG_RX_DROP_FRAME) + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" DA("MAC_FMT") through peer, ap_isolate\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(da)); + #endif + goto exit; + } + goto fwd_chk; + } + + /* DA is unknown*/ + act |= RTW_RX_MSDU_ACT_INDICATE; + goto exit; + +fwd_chk: + + if (adapter->stapriv.asoc_list_cnt <= 1) + goto exit; + + os_qid = rtw_os_recv_select_queue(msdu, llc_hdl); + +#if CONFIG_RTW_AP_DATA_BMC_TO_UC + _rtw_init_listhead(b2u_list); + + if (is_da_bmc + && rtw_ap_fwd_b2u_policy_chk(adapter->b2u_flags_ap_fwd, da, rattrib->to_fr_ds == 3 && !IS_MCAST(rattrib->ra)) + && adapter->registrypriv.wifi_spec == 0 + && adapter->xmitpriv.free_xmitframe_cnt > (NR_XMITFRAME / 4) + ) { + bmc_need = rtw_ap_data_bmc_to_uc(adapter + , da, sa, rframe->u.hdr.psta->cmn.mac_addr + , os_qid, b2u_list); + } + + if (bmc_need == _TRUE) +#endif + { + xframe = rtw_alloc_xmitframe(&adapter->xmitpriv, os_qid); + if (!xframe) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME "FUNC_ADPT_FMT" rtw_alloc_xmitframe fail\n" + , FUNC_ADPT_ARG(adapter)); + #endif + goto exit; + } + + xattrib = &xframe->attrib; + + _rtw_memcpy(xattrib->dst, da, ETH_ALEN); + _rtw_memcpy(xattrib->src, sa, ETH_ALEN); + _rtw_memcpy(xattrib->ta, adapter_mac_addr(adapter), ETH_ALEN); + + #ifdef CONFIG_RTW_WDS + if (in_wds_tbl && _rtw_memcmp(da, fwd_ra, ETH_ALEN) == _FALSE) { + _rtw_memcpy(xattrib->ra, fwd_ra, ETH_ALEN); + xattrib->wds = 1; + } else + #endif + _rtw_memcpy(xattrib->ra, da, ETH_ALEN); + + *fwd_frame = xframe; + } + + act |= RTW_RX_MSDU_ACT_FORWARD; + +exit: + return act; +} + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +void rtw_issue_action_token_req(_adapter *padapter, struct sta_info *pstat) +{ + /* Token Request Format + Category code : 1 Byte + Action code : 1 Byte + Element field: 4 Bytes, the duration of data transmission requested for the station. + */ + + u8 val = 0x0; + u8 category = RTW_WLAN_CATEGORY_TBTX; + u32 tbtx_duration = TBTX_TX_DURATION*1000; + u8 *pframe; + unsigned short *fctrl; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct rtw_ieee80211_hdr *pwlanhdr; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network); + + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + RTW_DBG("%s: %6ph\n", __FUNCTION__, pstat->cmn.mac_addr); + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->rate = MGN_24M; /* issue action request using OFDM rate? 20190716 Bruce add */ + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy((void *)get_addr2_ptr(pwlanhdr), adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(val), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(tbtx_duration), &(pattrib->pktlen)); + + pattrib->last_txcmdsz = pattrib->pktlen; + padapter->stapriv.last_token_holder = pstat; + dump_mgntframe(padapter, pmgntframe); + +} +#endif /* CONFIG_RTW_TOKEN_BASED_XMIT */ +#endif /* CONFIG_AP_MODE */ + diff --git a/core/rtw_beamforming.c b/core/rtw_beamforming.c new file mode 100644 index 0000000..8eda985 --- /dev/null +++ b/core/rtw_beamforming.c @@ -0,0 +1,2194 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_BEAMFORMING_C_ + +#include +#include + +#ifdef CONFIG_BEAMFORMING + +#ifdef RTW_BEAMFORMING_VERSION_2 + +struct ndpa_sta_info { + u16 aid:12; + u16 feedback_type:1; + u16 nc_index:3; +}; + +static void _get_txvector_parameter(PADAPTER adapter, struct sta_info *sta, u8 *g_id, u16 *p_aid) +{ + struct mlme_priv *mlme; + u16 aid; + u8 *bssid; + u16 val16; + u8 i; + + + mlme = &adapter->mlmepriv; + + if (check_fwstate(mlme, WIFI_AP_STATE)) { + /* + * Sent by an AP and addressed to a STA associated with that AP + * or sent by a DLS or TDLS STA in a direct path to + * a DLS or TDLS peer STA + */ + + aid = sta->cmn.aid; + bssid = adapter_mac_addr(adapter); + RTW_INFO("%s: AID=0x%x BSSID=" MAC_FMT "\n", + __FUNCTION__, sta->cmn.aid, MAC_ARG(bssid)); + + /* AID[0:8] */ + aid &= 0x1FF; + /* BSSID[44:47] xor BSSID[40:43] */ + val16 = ((bssid[5] & 0xF0) >> 4) ^ (bssid[5] & 0xF); + /* (dec(AID[0:8]) + dec(BSSID)*2^5) mod 2^9 */ + *p_aid = (aid + (val16 << 5)) & 0x1FF; + *g_id = 63; + } else if ((check_fwstate(mlme, WIFI_ADHOC_STATE) == _TRUE) + || (check_fwstate(mlme, WIFI_ADHOC_MASTER_STATE) == _TRUE)) { + /* + * Otherwise, includes + * 1. Sent to an IBSS STA + * 2. Sent by an AP to a non associated STA + * 3. Sent to a STA for which it is not known + * which condition is applicable + */ + *p_aid = 0; + *g_id = 63; + } else { + /* Addressed to AP */ + bssid = sta->cmn.mac_addr; + RTW_INFO("%s: BSSID=" MAC_FMT "\n", __FUNCTION__, MAC_ARG(bssid)); + + /* BSSID[39:47] */ + *p_aid = (bssid[5] << 1) | (bssid[4] >> 7); + *g_id = 0; + } + + RTW_INFO("%s: GROUP_ID=0x%02x PARTIAL_AID=0x%04x\n", + __FUNCTION__, *g_id, *p_aid); +} + +/* + * Parameters + * adapter struct _adapter* + * sta struct sta_info* + * sta_bf_cap beamforming capabe of sta + * sounding_dim Number of Sounding Dimensions + * comp_steering Compressed Steering Number of Beamformer Antennas Supported + */ +static void _get_sta_beamform_cap(PADAPTER adapter, struct sta_info *sta, + u8 *sta_bf_cap, u8 *sounding_dim, u8 *comp_steering) +{ + struct beamforming_info *info; + struct mlme_priv *mlme; + struct ht_priv *ht; + u16 ht_bf_cap; +#ifdef CONFIG_80211AC_VHT + struct vht_priv *vht; + u16 vht_bf_cap; +#endif /* CONFIG_80211AC_VHT */ + + + *sta_bf_cap = 0; + *sounding_dim = 0; + *comp_steering = 0; + + info = GET_BEAMFORM_INFO(adapter); + ht = &adapter->mlmepriv.htpriv; +#ifdef CONFIG_80211AC_VHT + vht = &adapter->mlmepriv.vhtpriv; +#endif /* CONFIG_80211AC_VHT */ + mlme = &adapter->mlmepriv; + + if (is_supported_ht(sta->wireless_mode) == _FALSE) + return; + + /* HT */ + if (check_fwstate(mlme, WIFI_AP_STATE)) { + /* Get peer clinet's BF cap: the cap. is intersected with associated AP.*/ + ht_bf_cap = sta->htpriv.beamform_cap; + RTW_INFO("At AP state, peer sta's ht_bf_cap=0x%x\n", ht_bf_cap); + + if (TEST_FLAG(ht_bf_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) { + info->beamforming_cap |= BEAMFORMER_CAP_HT_EXPLICIT; + *sta_bf_cap |= BEAMFORMEE_CAP_HT_EXPLICIT; + *comp_steering = (ht_bf_cap & BEAMFORMING_HT_BEAMFORMER_STEER_NUM) >> 4; + RTW_INFO("%s: we support BEAMFORMER_CAP_HT_EXPLICIT\n", __func__); + } + if (TEST_FLAG(ht_bf_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) { + info->beamforming_cap |= BEAMFORMEE_CAP_HT_EXPLICIT; + *sta_bf_cap |= BEAMFORMER_CAP_HT_EXPLICIT; + *sounding_dim = (ht_bf_cap & BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP) >> 6; + RTW_INFO("%s: we support BEAMFORMEE_CAP_HT_EXPLICIT\n", __func__); + } + } else { + /* Get adapter's BF Cap: the cap. is intersected with associated AP.*/ + ht_bf_cap = ht->beamform_cap; + RTW_INFO("At non-AP state, adapter's ht_bf_cap=0x%x\n", ht_bf_cap); + + if (TEST_FLAG(ht_bf_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) { + info->beamforming_cap |= BEAMFORMEE_CAP_HT_EXPLICIT; + *sta_bf_cap |= BEAMFORMER_CAP_HT_EXPLICIT; + *sounding_dim = (ht_bf_cap & BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP) >> 6; + RTW_INFO("%s: we support BEAMFORMEE_CAP_HT_EXPLICIT\n", __func__); + } + if (TEST_FLAG(ht_bf_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) { + info->beamforming_cap |= BEAMFORMER_CAP_HT_EXPLICIT; + *sta_bf_cap |= BEAMFORMEE_CAP_HT_EXPLICIT; + *comp_steering = (ht_bf_cap & BEAMFORMING_HT_BEAMFORMER_STEER_NUM) >> 4; + RTW_INFO("%s: we support BEAMFORMER_CAP_HT_EXPLICIT\n", __func__); + } + } + +#ifdef CONFIG_80211AC_VHT + + if (is_supported_vht(sta->wireless_mode) == _FALSE) + return; + + /* VHT */ + if (check_fwstate(mlme, WIFI_AP_STATE)) { + /* Get peer clinet's BF cap: the cap. is intersected with associated AP.*/ + vht_bf_cap = sta->vhtpriv.beamform_cap; + RTW_INFO("At AP state, peer sta's vht_bf_cap=0x%x\n", vht_bf_cap); + + /* We are SU Beamformer because the STA is SU Beamformee */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)) { + info->beamforming_cap |= BEAMFORMER_CAP_VHT_SU; + *sta_bf_cap |= BEAMFORMEE_CAP_VHT_SU; + RTW_INFO("%s: we support BEAMFORMER_CAP_VHT_SU\n", __func__); + + /* We are MU Beamformer because the STA is MU Beamformee */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE)) { + info->beamforming_cap |= BEAMFORMER_CAP_VHT_MU; + *sta_bf_cap |= BEAMFORMEE_CAP_VHT_MU; + RTW_INFO("%s: we support BEAMFORMER_CAP_VHT_MU\n", __func__); + } + + *comp_steering = (vht_bf_cap & BEAMFORMING_VHT_BEAMFORMER_STS_CAP) >> 8; + } + /* We are SU Beamformee because the STA is SU Beamformer */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)) { + info->beamforming_cap |= BEAMFORMEE_CAP_VHT_SU; + *sta_bf_cap |= BEAMFORMER_CAP_VHT_SU; + RTW_INFO("%s: we support BEAMFORMEE_CAP_VHT_SU\n", __func__); + + /* The STA is MU Beamformer, but we(AP) should not be MU Beamformee */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE)) { + RTW_WARN("%s: Associated STA should not be a MU BFer.\n", __func__); + } + + *sounding_dim = (vht_bf_cap & BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM) >> 12; + } + } else { + /* Get adapter's BF Cap: the cap. is intersected with associated AP.*/ + vht_bf_cap = vht->beamform_cap; + RTW_INFO("At non-AP state, adapter's vht_bf_cap=0x%x\n", vht_bf_cap); + + /* We are SU Beamformee */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)) { + info->beamforming_cap |= BEAMFORMEE_CAP_VHT_SU; + *sta_bf_cap |= BEAMFORMER_CAP_VHT_SU; + RTW_INFO("%s: we support BEAMFORMEE_CAP_VHT_SU\n", __func__); + + /* We are MU Beamformee */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE)) { + info->beamforming_cap |= BEAMFORMEE_CAP_VHT_MU; + *sta_bf_cap |= BEAMFORMER_CAP_VHT_MU; + RTW_INFO("%s: we support BEAMFORMEE_CAP_VHT_MU\n", __func__); + } + + *sounding_dim = (vht_bf_cap & BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM) >> 12; + } + /* We are SU Beamformer */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)) { + info->beamforming_cap |= BEAMFORMER_CAP_VHT_SU; + *sta_bf_cap |= BEAMFORMEE_CAP_VHT_SU; + RTW_INFO("%s: we support BEAMFORMER_CAP_VHT_SU\n", __func__); + + /* We are MU Beamformer, but client should not be a MU Beamformer */ + if (TEST_FLAG(vht_bf_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE)) { + RTW_WARN("%s: non-AP state should not support MU BFer.\n", __func__); + } + + *comp_steering = (vht_bf_cap & BEAMFORMING_VHT_BEAMFORMER_STS_CAP) >> 8; + } + } +#endif /* CONFIG_80211AC_VHT */ + +} + +static u8 _send_ht_ndpa_packet(PADAPTER adapter, u8 *ra, enum channel_width bw) +{ + /* General */ + struct xmit_priv *pxmitpriv; + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + struct xmit_frame *pmgntframe; + /* Beamforming */ + struct beamforming_info *info; + struct beamformee_entry *bfee; + struct ndpa_sta_info sta_info; + u8 ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xE0, 0x4C}; + /* MISC */ + struct pkt_attrib *attrib; + struct rtw_ieee80211_hdr *pwlanhdr; + enum MGN_RATE txrate; + u8 *pframe; + u16 duration = 0; + u8 aSifsTime = 0; + + + RTW_INFO("+%s: Send to " MAC_FMT "\n", __FUNCTION__, MAC_ARG(ra)); + + pxmitpriv = &adapter->xmitpriv; + pmlmeext = &adapter->mlmeextpriv; + pmlmeinfo = &pmlmeext->mlmext_info; + bfee = rtw_bf_bfee_get_entry_by_addr(adapter, ra); + if (!bfee) { + RTW_ERR("%s: Cann't find beamformee entry!\n", __FUNCTION__); + return _FALSE; + } + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (!pmgntframe) { + RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__); + return _FALSE; + } + + txrate = beamforming_get_htndp_tx_rate(GET_PDM_ODM(adapter), bfee->comp_steering_num_of_bfer); + + /* update attribute */ + attrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, attrib); + /*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */ + attrib->subtype = WIFI_ACTION_NOACK; + attrib->bwmode = bw; + /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */ + attrib->order = 1; + attrib->rate = (u8)txrate; + attrib->bf_pkt_type = 0; + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + /* Frame control */ + pwlanhdr->frame_ctl = 0; + set_frame_sub_type(pframe, attrib->subtype); + set_order_bit(pframe); + + /* Duration */ + if (pmlmeext->cur_wireless_mode == WIRELESS_11B) + aSifsTime = 10; + else + aSifsTime = 16; + duration = 2 * aSifsTime + 40; + if (bw == CHANNEL_WIDTH_40) + duration += 87; + else + duration += 180; + set_duration(pframe, duration); + + /* DA */ + _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); + /* SA */ + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + /* BSSID */ + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + + /* HT control field */ + SET_HT_CTRL_CSI_STEERING(pframe + 24, 3); + SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1); + + /* + * Frame Body + * Category field: vender-specific value, 0x7F + * OUI: 0x00E04C + */ + _rtw_memcpy(pframe + 28, ActionHdr, 4); + + attrib->pktlen = 32; + attrib->last_txcmdsz = attrib->pktlen; + + dump_mgntframe(adapter, pmgntframe); + + return _TRUE; +} + +static u8 _send_vht_ndpa_packet(PADAPTER adapter, u8 *ra, u16 aid, enum channel_width bw) +{ + /* General */ + struct xmit_priv *pxmitpriv; + struct mlme_ext_priv *pmlmeext; + struct xmit_frame *pmgntframe; + /* Beamforming */ + struct beamforming_info *info; + struct beamformee_entry *bfee; + struct ndpa_sta_info sta_info; + /* MISC */ + struct pkt_attrib *attrib; + struct rtw_ieee80211_hdr *pwlanhdr; + u8 *pframe; + enum MGN_RATE txrate; + u16 duration = 0; + u8 sequence = 0, aSifsTime = 0; + + + RTW_INFO("+%s: Send to " MAC_FMT "\n", __FUNCTION__, MAC_ARG(ra)); + + pxmitpriv = &adapter->xmitpriv; + pmlmeext = &adapter->mlmeextpriv; + info = GET_BEAMFORM_INFO(adapter); + bfee = rtw_bf_bfee_get_entry_by_addr(adapter, ra); + if (!bfee) { + RTW_ERR("%s: Cann't find beamformee entry!\n", __FUNCTION__); + return _FALSE; + } + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (!pmgntframe) { + RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__); + return _FALSE; + } + + txrate = beamforming_get_vht_ndp_tx_rate(GET_PDM_ODM(adapter), bfee->comp_steering_num_of_bfer); + + /* update attribute */ + attrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, attrib); + /*pattrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */ + attrib->subtype = WIFI_NDPA; + attrib->bwmode = bw; + /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */ + attrib->rate = (u8)txrate; + attrib->bf_pkt_type = 0; + + _rtw_memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET); + pframe = pmgntframe->buf_addr + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + /* Frame control */ + pwlanhdr->frame_ctl = 0; + set_frame_sub_type(pframe, attrib->subtype); + + /* Duration */ + if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode)) + aSifsTime = 16; + else + aSifsTime = 10; + duration = 2 * aSifsTime + 44; + if (bw == CHANNEL_WIDTH_80) + duration += 40; + else if (bw == CHANNEL_WIDTH_40) + duration += 87; + else + duration += 180; + set_duration(pframe, duration); + + /* RA */ + _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); + + /* TA */ + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + + /* Sounding Sequence, bit0~1 is reserved */ + sequence = info->sounding_sequence << 2; + if (info->sounding_sequence >= 0x3f) + info->sounding_sequence = 0; + else + info->sounding_sequence++; + _rtw_memcpy(pframe + 16, &sequence, 1); + + /* STA Info */ + /* + * "AID12" Equal to 0 if the STA is an AP, mesh STA or + * STA that is a member of an IBSS + */ + if (check_fwstate(&adapter->mlmepriv, WIFI_AP_STATE) == _FALSE) + aid = 0; + sta_info.aid = aid; + /* "Feedback Type" set to 0 for SU */ + sta_info.feedback_type = 0; + /* "Nc Index" reserved if the Feedback Type field indicates SU */ + sta_info.nc_index = 0; + _rtw_memcpy(pframe + 17, (u8 *)&sta_info, 2); + + attrib->pktlen = 19; + attrib->last_txcmdsz = attrib->pktlen; + + dump_mgntframe(adapter, pmgntframe); + + return _TRUE; +} + +static u8 _send_vht_mu_ndpa_packet(PADAPTER adapter, enum channel_width bw) +{ + /* General */ + struct xmit_priv *pxmitpriv; + struct mlme_ext_priv *pmlmeext; + struct xmit_frame *pmgntframe; + /* Beamforming */ + struct beamforming_info *info; + struct sounding_info *sounding; + struct beamformee_entry *bfee; + struct ndpa_sta_info sta_info; + /* MISC */ + struct pkt_attrib *attrib; + struct rtw_ieee80211_hdr *pwlanhdr; + enum MGN_RATE txrate; + u8 *pframe; + u8 *ra = NULL; + u16 duration = 0; + u8 sequence = 0, aSifsTime = 0; + u8 i; + + + RTW_INFO("+%s\n", __FUNCTION__); + + pxmitpriv = &adapter->xmitpriv; + pmlmeext = &adapter->mlmeextpriv; + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + txrate = MGN_VHT2SS_MCS0; + + /* + * Fill the first MU BFee entry (STA1) MAC addr to destination address then + * HW will change A1 to broadcast addr. + * 2015.05.28. Suggested by SD1 Chunchu. + */ + bfee = &info->bfee_entry[sounding->mu_sounding_list[0]]; + ra = bfee->mac_addr; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (!pmgntframe) { + RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__); + return _FALSE; + } + + /* update attribute */ + attrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, attrib); + /*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */ + attrib->subtype = WIFI_NDPA; + attrib->bwmode = bw; + /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */ + attrib->rate = (u8)txrate; + /* Set TxBFPktType of Tx desc to unicast type if there is only one MU STA for HW design */ + if (info->sounding_info.candidate_mu_bfee_cnt > 1) + attrib->bf_pkt_type = 1; + else + attrib->bf_pkt_type = 0; + + _rtw_memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET); + pframe = pmgntframe->buf_addr + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + /* Frame control */ + pwlanhdr->frame_ctl = 0; + set_frame_sub_type(pframe, attrib->subtype); + + /* Duration */ + if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode)) + aSifsTime = 16; + else + aSifsTime = 10; + duration = 2 * aSifsTime + 44; + if (bw == CHANNEL_WIDTH_80) + duration += 40; + else if (bw == CHANNEL_WIDTH_40) + duration += 87; + else + duration += 180; + set_duration(pframe, duration); + + /* RA */ + _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); + + /* TA */ + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + + /* Sounding Sequence, bit0~1 is reserved */ + sequence = info->sounding_sequence << 2; + if (info->sounding_sequence >= 0x3f) + info->sounding_sequence = 0; + else + info->sounding_sequence++; + _rtw_memcpy(pframe + 16, &sequence, 1); + + attrib->pktlen = 17; + + /* + * Construct STA info. for multiple STAs + * STA Info1, ..., STA Info n + */ + for (i = 0; i < sounding->candidate_mu_bfee_cnt; i++) { + bfee = &info->bfee_entry[sounding->mu_sounding_list[i]]; + sta_info.aid = bfee->aid; + sta_info.feedback_type = 1; /* 1'b1: MU */ + sta_info.nc_index = 0; + _rtw_memcpy(pframe + attrib->pktlen, (u8 *)&sta_info, 2); + attrib->pktlen += 2; + } + + attrib->last_txcmdsz = attrib->pktlen; + + dump_mgntframe(adapter, pmgntframe); + + return _TRUE; +} + +static u8 _send_bf_report_poll(PADAPTER adapter, u8 *ra, u8 bFinalPoll) +{ + /* General */ + struct xmit_priv *pxmitpriv; + struct xmit_frame *pmgntframe; + /* MISC */ + struct pkt_attrib *attrib; + struct rtw_ieee80211_hdr *pwlanhdr; + u8 *pframe; + + + RTW_INFO("+%s: Send to " MAC_FMT "\n", __FUNCTION__, MAC_ARG(ra)); + + pxmitpriv = &adapter->xmitpriv; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (!pmgntframe) { + RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__); + return _FALSE; + } + + /* update attribute */ + attrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, attrib); + /*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */ + attrib->subtype = WIFI_BF_REPORT_POLL; + attrib->bwmode = CHANNEL_WIDTH_20; + /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */ + attrib->rate = MGN_6M; + if (bFinalPoll) + attrib->bf_pkt_type = 3; + else + attrib->bf_pkt_type = 2; + + _rtw_memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET); + pframe = pmgntframe->buf_addr + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + /* Frame control */ + pwlanhdr->frame_ctl = 0; + set_frame_sub_type(pframe, attrib->subtype); + + /* Duration */ + set_duration(pframe, 100); + + /* RA */ + _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); + + /* TA */ + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + + /* Feedback Segment Retransmission Bitmap */ + pframe[16] = 0xFF; + + attrib->pktlen = 17; + attrib->last_txcmdsz = attrib->pktlen; + + dump_mgntframe(adapter, pmgntframe); + + return _TRUE; +} + +static void _sounding_update_min_period(PADAPTER adapter, u16 period, u8 leave) +{ + struct beamforming_info *info; + struct beamformee_entry *bfee; + u8 i = 0; + u16 min_val = 0xFFFF; + + + info = GET_BEAMFORM_INFO(adapter); + + if (_TRUE == leave) { + /* + * When a BFee left, + * we need to find the latest min sounding period + * from the remaining BFees + */ + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if ((bfee->used == _TRUE) + && (bfee->sound_period < min_val)) + min_val = bfee->sound_period; + } + + if (min_val == 0xFFFF) + info->sounding_info.min_sounding_period = 0; + else + info->sounding_info.min_sounding_period = min_val; + } else { + if ((info->sounding_info.min_sounding_period == 0) + || (period < info->sounding_info.min_sounding_period)) + info->sounding_info.min_sounding_period = period; + } +} + +static void _sounding_init(struct sounding_info *sounding) +{ + _rtw_memset(sounding->su_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_SU); + _rtw_memset(sounding->mu_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_MU); + sounding->state = SOUNDING_STATE_NONE; + sounding->su_bfee_curidx = 0xFF; + sounding->candidate_mu_bfee_cnt = 0; + sounding->min_sounding_period = 0; + sounding->sound_remain_cnt_per_period = 0; +} + +static void _sounding_reset_vars(PADAPTER adapter) +{ + struct beamforming_info *info; + struct sounding_info *sounding; + u8 idx; + + + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + _rtw_memset(sounding->su_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_SU); + _rtw_memset(sounding->mu_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_MU); + sounding->su_bfee_curidx = 0xFF; + sounding->candidate_mu_bfee_cnt = 0; + + /* Clear bSound flag for the new period */ + for (idx = 0; idx < MAX_BEAMFORMEE_ENTRY_NUM; idx++) { + if ((info->bfee_entry[idx].used == _TRUE) + && (info->bfee_entry[idx].sounding == _TRUE)) { + info->bfee_entry[idx].sounding = _FALSE; + info->bfee_entry[idx].bCandidateSoundingPeer = _FALSE; + } + } +} + +/* + * Return + * 0 Prepare sounding list OK + * -1 Fail to prepare sounding list, because no beamformee need to souding + * -2 Fail to prepare sounding list, because beamformee state not ready + * + */ +static int _sounding_get_list(PADAPTER adapter) +{ + struct beamforming_info *info; + struct sounding_info *sounding; + struct beamformee_entry *bfee; + u8 i, mu_idx = 0, su_idx = 0, not_ready = 0; + int ret = 0; + + + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + /* Add MU BFee list first because MU priority is higher than SU */ + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (bfee->used == _FALSE) + continue; + + if (bfee->state != BEAMFORM_ENTRY_HW_STATE_ADDED) { + RTW_ERR("%s: Invalid BFee idx(%d) Hw state=%d\n", __FUNCTION__, i, bfee->state); + not_ready++; + continue; + } + + /* + * Decrease BFee's SoundCnt per period + * If the remain count is 0, + * then it can be sounded at this time + */ + if (bfee->SoundCnt) { + bfee->SoundCnt--; + if (bfee->SoundCnt) + continue; + } + + /* + * + * If the STA supports MU BFee capability then we add it to MUSoundingList directly + * because we can only sound one STA by unicast NDPA with MU cap enabled to get correct channel info. + * Suggested by BB team Luke Lee. 2015.11.25. + */ + if (bfee->cap & BEAMFORMEE_CAP_VHT_MU) { + /* MU BFee */ + if (mu_idx >= MAX_NUM_BEAMFORMEE_MU) { + RTW_ERR("%s: Too much MU bfee entry(Limit:%d)\n", __FUNCTION__, MAX_NUM_BEAMFORMEE_MU); + continue; + } + + if (bfee->bApplySounding == _TRUE) { + bfee->bCandidateSoundingPeer = _TRUE; + bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, sounding->min_sounding_period); + sounding->mu_sounding_list[mu_idx] = i; + mu_idx++; + } + } else if (bfee->cap & (BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) { + /* SU BFee (HT/VHT) */ + if (su_idx >= MAX_NUM_BEAMFORMEE_SU) { + RTW_ERR("%s: Too much SU bfee entry(Limit:%d)\n", __FUNCTION__, MAX_NUM_BEAMFORMEE_SU); + continue; + } + + if (bfee->bDeleteSounding == _TRUE) { + sounding->su_sounding_list[su_idx] = i; + su_idx++; + } else if ((bfee->bApplySounding == _TRUE) + && (bfee->bSuspendSUCap == _FALSE)) { + bfee->bCandidateSoundingPeer = _TRUE; + bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, sounding->min_sounding_period); + sounding->su_sounding_list[su_idx] = i; + su_idx++; + } + } + } + + sounding->candidate_mu_bfee_cnt = mu_idx; + + if (su_idx + mu_idx == 0) { + ret = -1; + if (not_ready) + ret = -2; + } + + RTW_INFO("-%s: There are %d SU and %d MU BFees in this sounding period\n", __FUNCTION__, su_idx, mu_idx); + + return ret; +} + +static void _sounding_handler(PADAPTER adapter) +{ + struct beamforming_info *info; + struct sounding_info *sounding; + struct beamformee_entry *bfee; + u8 su_idx, i; + u32 timeout_period = 0; + u8 set_timer = _FALSE; + int ret = 0; + static u16 wait_cnt = 0; + + + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + RTW_DBG("+%s: state=%d\n", __FUNCTION__, sounding->state); + if ((sounding->state != SOUNDING_STATE_INIT) + && (sounding->state != SOUNDING_STATE_SU_SOUNDDOWN) + && (sounding->state != SOUNDING_STATE_MU_SOUNDDOWN) + && (sounding->state != SOUNDING_STATE_SOUNDING_TIMEOUT)) { + RTW_WARN("%s: Invalid State(%d) and return!\n", __FUNCTION__, sounding->state); + return; + } + + if (sounding->state == SOUNDING_STATE_INIT) { + RTW_INFO("%s: Sounding start\n", __FUNCTION__); + + /* Init Var */ + _sounding_reset_vars(adapter); + + /* Get the sounding list of this sounding period */ + ret = _sounding_get_list(adapter); + if (ret == -1) { + wait_cnt = 0; + sounding->state = SOUNDING_STATE_NONE; + RTW_ERR("%s: No BFees found, set to SOUNDING_STATE_NONE\n", __FUNCTION__); + info->sounding_running--; + return; + } + if (ret == -2) { + RTW_WARN("%s: Temporarily cann't find BFee to sounding\n", __FUNCTION__); + if (wait_cnt < 5) { + wait_cnt++; + } else { + wait_cnt = 0; + sounding->state = SOUNDING_STATE_NONE; + RTW_ERR("%s: Wait changing state timeout!! Set to SOUNDING_STATE_NONE\n", __FUNCTION__); + } + info->sounding_running--; + return; + } + if (ret != 0) { + wait_cnt = 0; + RTW_ERR("%s: Unkown state(%d)!\n", __FUNCTION__, ret); + info->sounding_running--; + return; + + } + + wait_cnt = 0; + + if (check_fwstate(&adapter->mlmepriv, WIFI_UNDER_SURVEY) == _TRUE) { + RTW_INFO("%s: Sounding abort! scanning APs...\n", __FUNCTION__); + info->sounding_running--; + return; + } + + rtw_ps_deny(adapter, PS_DENY_BEAMFORMING); + LeaveAllPowerSaveModeDirect(adapter); + } + + /* Get non-sound SU BFee index */ + for (i = 0; i < MAX_NUM_BEAMFORMEE_SU; i++) { + su_idx = sounding->su_sounding_list[i]; + if (su_idx >= MAX_BEAMFORMEE_ENTRY_NUM) + continue; + bfee = &info->bfee_entry[su_idx]; + if (_FALSE == bfee->sounding) + break; + } + if (i < MAX_NUM_BEAMFORMEE_SU) { + sounding->su_bfee_curidx = su_idx; + /* Set to sounding start state */ + sounding->state = SOUNDING_STATE_SU_START; + RTW_DBG("%s: Set to SOUNDING_STATE_SU_START\n", __FUNCTION__); + + bfee->sounding = _TRUE; + /* Reset sounding timeout flag for the new sounding */ + bfee->bSoundingTimeout = _FALSE; + + if (_TRUE == bfee->bDeleteSounding) { + u8 res = _FALSE; + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 0); + return; + } + + /* Start SU sounding */ + if (bfee->cap & BEAMFORMEE_CAP_VHT_SU) + _send_vht_ndpa_packet(adapter, bfee->mac_addr, bfee->aid, bfee->sound_bw); + else if (bfee->cap & BEAMFORMEE_CAP_HT_EXPLICIT) + _send_ht_ndpa_packet(adapter, bfee->mac_addr, bfee->sound_bw); + + /* Set sounding timeout timer */ + _set_timer(&info->sounding_timeout_timer, SU_SOUNDING_TIMEOUT); + return; + } + + if (sounding->candidate_mu_bfee_cnt > 0) { + /* + * If there is no SU BFee then find MU BFee and perform MU sounding + * + * Need to check the MU starting condition. 2015.12.15. + */ + sounding->state = SOUNDING_STATE_MU_START; + RTW_DBG("%s: Set to SOUNDING_STATE_MU_START\n", __FUNCTION__); + + /* Update MU BFee info */ + for (i = 0; i < sounding->candidate_mu_bfee_cnt; i++) { + bfee = &info->bfee_entry[sounding->mu_sounding_list[i]]; + bfee->sounding = _TRUE; + } + + /* Send MU NDPA */ + bfee = &info->bfee_entry[sounding->mu_sounding_list[0]]; + _send_vht_mu_ndpa_packet(adapter, bfee->sound_bw); + + /* Send BF report poll if more than 1 MU STA */ + for (i = 1; i < sounding->candidate_mu_bfee_cnt; i++) { + bfee = &info->bfee_entry[sounding->mu_sounding_list[i]]; + + if (i == (sounding->candidate_mu_bfee_cnt - 1))/* The last STA*/ + _send_bf_report_poll(adapter, bfee->mac_addr, _TRUE); + else + _send_bf_report_poll(adapter, bfee->mac_addr, _FALSE); + } + + sounding->candidate_mu_bfee_cnt = 0; + + /* Set sounding timeout timer */ + _set_timer(&info->sounding_timeout_timer, MU_SOUNDING_TIMEOUT); + return; + } + + info->sounding_running--; + sounding->state = SOUNDING_STATE_INIT; + RTW_INFO("%s: Sounding finished!\n", __FUNCTION__); + rtw_ps_deny_cancel(adapter, PS_DENY_BEAMFORMING); +} + +static void _sounding_force_stop(PADAPTER adapter) +{ + struct beamforming_info *info; + struct sounding_info *sounding; + + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + if ((sounding->state == SOUNDING_STATE_SU_START) + || (sounding->state == SOUNDING_STATE_MU_START)) { + u8 res = _FALSE; + _cancel_timer_ex(&info->sounding_timeout_timer); + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 1); + return; + } + + info->sounding_running--; + sounding->state = SOUNDING_STATE_INIT; + RTW_INFO("%s: Sounding finished!\n", __FUNCTION__); + rtw_ps_deny_cancel(adapter, PS_DENY_BEAMFORMING); +} + +static void _sounding_timer_handler(void *FunctionContext) +{ + PADAPTER adapter; + struct beamforming_info *info; + struct sounding_info *sounding; + static u8 delay = 0; + + + RTW_DBG("+%s\n", __FUNCTION__); + + adapter = (PADAPTER)FunctionContext; + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + if (SOUNDING_STATE_NONE == sounding->state) { + RTW_INFO("%s: Stop!\n", __FUNCTION__); + if (info->sounding_running) + RTW_WARN("%s: souding_running=%d when thread stop!\n", + __FUNCTION__, info->sounding_running); + return; + } + + _set_timer(&info->sounding_timer, sounding->min_sounding_period); + + if (!info->sounding_running) { + if (SOUNDING_STATE_INIT != sounding->state) { + RTW_WARN("%s: state(%d) != SOUNDING_STATE_INIT!!\n", __FUNCTION__, sounding->state); + sounding->state = SOUNDING_STATE_INIT; + } + delay = 0; + info->sounding_running++; + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 1); + } else { + if (delay != 0xFF) + delay++; + RTW_WARN("%s: souding is still processing...(state:%d, running:%d, delay:%d)\n", + __FUNCTION__, sounding->state, info->sounding_running, delay); + if (delay > 3) { + RTW_WARN("%s: Stop sounding!!\n", __FUNCTION__); + _sounding_force_stop(adapter); + } + } +} + +static void _sounding_timeout_timer_handler(void *FunctionContext) +{ + PADAPTER adapter; + struct beamforming_info *info; + struct sounding_info *sounding; + struct beamformee_entry *bfee; + + + RTW_WARN("+%s\n", __FUNCTION__); + + adapter = (PADAPTER)FunctionContext; + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + if (SOUNDING_STATE_SU_START == sounding->state) { + sounding->state = SOUNDING_STATE_SOUNDING_TIMEOUT; + RTW_ERR("%s: Set to SU SOUNDING_STATE_SOUNDING_TIMEOUT\n", __FUNCTION__); + /* SU BFee */ + bfee = &info->bfee_entry[sounding->su_bfee_curidx]; + bfee->bSoundingTimeout = _TRUE; + RTW_WARN("%s: The BFee entry[%d] is Sounding Timeout!\n", __FUNCTION__, sounding->su_bfee_curidx); + } else if (SOUNDING_STATE_MU_START == sounding->state) { + sounding->state = SOUNDING_STATE_SOUNDING_TIMEOUT; + RTW_ERR("%s: Set to MU SOUNDING_STATE_SOUNDING_TIMEOUT\n", __FUNCTION__); + } else { + RTW_WARN("%s: unexpected sounding state:0x%02x\n", __FUNCTION__, sounding->state); + return; + } + + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 1); +} + +static struct beamformer_entry *_bfer_get_free_entry(PADAPTER adapter) +{ + u8 i = 0; + struct beamforming_info *info; + struct beamformer_entry *bfer; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) { + bfer = &info->bfer_entry[i]; + if (bfer->used == _FALSE) + return bfer; + } + + return NULL; +} + +static struct beamformer_entry *_bfer_get_entry_by_addr(PADAPTER adapter, u8 *ra) +{ + u8 i = 0; + struct beamforming_info *info; + struct beamformer_entry *bfer; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) { + bfer = &info->bfer_entry[i]; + if (bfer->used == _FALSE) + continue; + if (_rtw_memcmp(ra, bfer->mac_addr, ETH_ALEN) == _TRUE) + return bfer; + } + + return NULL; +} + +static struct beamformer_entry *_bfer_add_entry(PADAPTER adapter, + struct sta_info *sta, u8 bf_cap, u8 sounding_dim, u8 comp_steering) +{ + struct mlme_priv *mlme; + struct beamforming_info *info; + struct beamformer_entry *bfer; + u8 *bssid; + u16 val16; + u8 i; + + + mlme = &adapter->mlmepriv; + info = GET_BEAMFORM_INFO(adapter); + + bfer = _bfer_get_entry_by_addr(adapter, sta->cmn.mac_addr); + if (!bfer) { + bfer = _bfer_get_free_entry(adapter); + if (!bfer) + return NULL; + } + + bfer->used = _TRUE; + _get_txvector_parameter(adapter, sta, &bfer->g_id, &bfer->p_aid); + _rtw_memcpy(bfer->mac_addr, sta->cmn.mac_addr, ETH_ALEN); + bfer->cap = bf_cap; + bfer->state = BEAMFORM_ENTRY_HW_STATE_ADD_INIT; + bfer->NumofSoundingDim = sounding_dim; + + if (TEST_FLAG(bf_cap, BEAMFORMER_CAP_VHT_MU)) { + info->beamformer_mu_cnt += 1; + bfer->aid = sta->cmn.aid; + } else if (TEST_FLAG(bf_cap, BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT)) { + info->beamformer_su_cnt += 1; + + /* Record HW idx info */ + for (i = 0; i < MAX_NUM_BEAMFORMER_SU; i++) { + if ((info->beamformer_su_reg_maping & BIT(i)) == 0) { + info->beamformer_su_reg_maping |= BIT(i); + bfer->su_reg_index = i; + break; + } + } + RTW_INFO("%s: Add BFer entry beamformer_su_reg_maping=%#x, su_reg_index=%d\n", + __FUNCTION__, info->beamformer_su_reg_maping, bfer->su_reg_index); + } + + return bfer; +} + +static void _bfer_remove_entry(PADAPTER adapter, struct beamformer_entry *entry) +{ + struct beamforming_info *info; + + + info = GET_BEAMFORM_INFO(adapter); + + entry->state = BEAMFORM_ENTRY_HW_STATE_DELETE_INIT; + + if (TEST_FLAG(entry->cap, BEAMFORMER_CAP_VHT_MU)) { + info->beamformer_mu_cnt -= 1; + _rtw_memset(entry->gid_valid, 0, 8); + _rtw_memset(entry->user_position, 0, 16); + } else if (TEST_FLAG(entry->cap, BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT)) { + info->beamformer_su_cnt -= 1; + } + + if (info->beamformer_mu_cnt == 0) + info->beamforming_cap &= ~BEAMFORMEE_CAP_VHT_MU; + if (info->beamformer_su_cnt == 0) + info->beamforming_cap &= ~(BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT); +} + +static u8 _bfer_set_entry_gid(PADAPTER adapter, u8 *addr, u8 *gid, u8 *position) +{ + struct beamformer_entry bfer; + + memset(&bfer, 0, sizeof(bfer)); + memcpy(bfer.mac_addr, addr, ETH_ALEN); + + /* Parsing Membership Status Array */ + memcpy(bfer.gid_valid, gid, 8); + + /* Parsing User Position Array */ + memcpy(bfer.user_position, position, 16); + + /* Config HW GID table */ + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_GID_TABLE, (u8 *) &bfer, + sizeof(bfer), 1); + + return _SUCCESS; +} + +static struct beamformee_entry *_bfee_get_free_entry(PADAPTER adapter) +{ + u8 i = 0; + struct beamforming_info *info; + struct beamformee_entry *bfee; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (bfee->used == _FALSE) + return bfee; + } + + return NULL; +} + +static struct beamformee_entry *_bfee_get_entry_by_addr(PADAPTER adapter, u8 *ra) +{ + u8 i = 0; + struct beamforming_info *info; + struct beamformee_entry *bfee; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (bfee->used == _FALSE) + continue; + if (_rtw_memcmp(ra, bfee->mac_addr, ETH_ALEN) == _TRUE) + return bfee; + } + + return NULL; +} + +static u8 _bfee_get_first_su_entry_idx(PADAPTER adapter, struct beamformee_entry *ignore) +{ + struct beamforming_info *info; + struct beamformee_entry *bfee; + u8 i; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (ignore && (bfee == ignore)) + continue; + if (bfee->used == _FALSE) + continue; + if ((!TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_MU)) + && TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) + return i; + } + + return 0xFF; +} + +/* + * Description: + * Get the first entry index of MU Beamformee. + * + * Return Value: + * Index of the first MU sta, or 0xFF for invalid index. + * + * 2015.05.25. Created by tynli. + * + */ +static u8 _bfee_get_first_mu_entry_idx(PADAPTER adapter, struct beamformee_entry *ignore) +{ + struct beamforming_info *info; + struct beamformee_entry *bfee; + u8 i; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (ignore && (bfee == ignore)) + continue; + if (bfee->used == _FALSE) + continue; + if (TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_MU)) + return i; + } + + return 0xFF; +} + +static struct beamformee_entry *_bfee_add_entry(PADAPTER adapter, + struct sta_info *sta, u8 bf_cap, u8 sounding_dim, u8 comp_steering) +{ + struct mlme_priv *mlme; + struct beamforming_info *info; + struct beamformee_entry *bfee; + u8 *bssid; + u16 val16; + u8 i; + + + mlme = &adapter->mlmepriv; + info = GET_BEAMFORM_INFO(adapter); + + bfee = _bfee_get_entry_by_addr(adapter, sta->cmn.mac_addr); + if (!bfee) { + bfee = _bfee_get_free_entry(adapter); + if (!bfee) + return NULL; + } + + bfee->used = _TRUE; + bfee->aid = sta->cmn.aid; + bfee->mac_id = sta->cmn.mac_id; + bfee->sound_bw = sta->cmn.bw_mode; + + _get_txvector_parameter(adapter, sta, &bfee->g_id, &bfee->p_aid); + sta->cmn.bf_info.g_id = bfee->g_id; + sta->cmn.bf_info.p_aid = bfee->p_aid; + + _rtw_memcpy(bfee->mac_addr, sta->cmn.mac_addr, ETH_ALEN); + bfee->txbf = _FALSE; + bfee->sounding = _FALSE; + bfee->sound_period = 40; + _sounding_update_min_period(adapter, bfee->sound_period, _FALSE); + bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, info->sounding_info.min_sounding_period); + bfee->cap = bf_cap; + bfee->state = BEAMFORM_ENTRY_HW_STATE_ADD_INIT; + + bfee->bCandidateSoundingPeer = _FALSE; + bfee->bSoundingTimeout = _FALSE; + bfee->bDeleteSounding = _FALSE; + bfee->bApplySounding = _TRUE; + + bfee->tx_timestamp = 0; + bfee->tx_bytes = 0; + + bfee->LogStatusFailCnt = 0; + bfee->NumofSoundingDim = sounding_dim; + bfee->comp_steering_num_of_bfer = comp_steering; + bfee->bSuspendSUCap = _FALSE; + + if (TEST_FLAG(bf_cap, BEAMFORMEE_CAP_VHT_MU)) { + info->beamformee_mu_cnt += 1; + info->first_mu_bfee_index = _bfee_get_first_mu_entry_idx(adapter, NULL); + + if (_TRUE == info->bEnableSUTxBFWorkAround) { + /* When the first MU BFee added, discard SU BFee bfee's capability */ + if ((info->beamformee_mu_cnt == 1) && (info->beamformee_su_cnt > 0)) { + if (info->TargetSUBFee) { + info->TargetSUBFee->bSuspendSUCap = _TRUE; + info->TargetSUBFee->bDeleteSounding = _TRUE; + } else { + RTW_ERR("%s: UNEXPECTED!! info->TargetSUBFee is NULL!", __FUNCTION__); + } + info->TargetSUBFee = NULL; + _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO)); + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 0); + } + } + + /* Record HW idx info */ + for (i = 0; i < MAX_NUM_BEAMFORMEE_MU; i++) { + if ((info->beamformee_mu_reg_maping & BIT(i)) == 0) { + info->beamformee_mu_reg_maping |= BIT(i); + bfee->mu_reg_index = i; + break; + } + } + RTW_INFO("%s: Add BFee entry beamformee_mu_reg_maping=%#x, mu_reg_index=%d\n", + __FUNCTION__, info->beamformee_mu_reg_maping, bfee->mu_reg_index); + + } else if (TEST_FLAG(bf_cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) { + info->beamformee_su_cnt += 1; + + if (_TRUE == info->bEnableSUTxBFWorkAround) { + /* Record the first SU BFee index. We only allow the first SU BFee to be sound */ + if ((info->beamformee_su_cnt == 1) && (info->beamformee_mu_cnt == 0)) { + info->TargetSUBFee = bfee; + _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO)); + bfee->bSuspendSUCap = _FALSE; + } else { + bfee->bSuspendSUCap = _TRUE; + } + } + + /* Record HW idx info */ + for (i = 0; i < MAX_NUM_BEAMFORMEE_SU; i++) { + if ((info->beamformee_su_reg_maping & BIT(i)) == 0) { + info->beamformee_su_reg_maping |= BIT(i); + bfee->su_reg_index = i; + break; + } + } + RTW_INFO("%s: Add BFee entry beamformee_su_reg_maping=%#x, su_reg_index=%d\n", + __FUNCTION__, info->beamformee_su_reg_maping, bfee->su_reg_index); + } + + return bfee; +} + +static void _bfee_remove_entry(PADAPTER adapter, struct beamformee_entry *entry) +{ + struct beamforming_info *info; + u8 idx; + + + info = GET_BEAMFORM_INFO(adapter); + + entry->state = BEAMFORM_ENTRY_HW_STATE_DELETE_INIT; + + if (TEST_FLAG(entry->cap, BEAMFORMEE_CAP_VHT_MU)) { + info->beamformee_mu_cnt -= 1; + info->first_mu_bfee_index = _bfee_get_first_mu_entry_idx(adapter, entry); + + if (_TRUE == info->bEnableSUTxBFWorkAround) { + if ((info->beamformee_mu_cnt == 0) && (info->beamformee_su_cnt > 0)) { + idx = _bfee_get_first_su_entry_idx(adapter, NULL); + info->TargetSUBFee = &info->bfee_entry[idx]; + _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO)); + info->TargetSUBFee->bSuspendSUCap = _FALSE; + } + } + } else if (TEST_FLAG(entry->cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) { + info->beamformee_su_cnt -= 1; + + /* When the target SU BFee leaves, disable workaround */ + if ((_TRUE == info->bEnableSUTxBFWorkAround) + && (entry == info->TargetSUBFee)) { + entry->bSuspendSUCap = _TRUE; + info->TargetSUBFee = NULL; + _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO)); + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 0); + } + } + + if (info->beamformee_mu_cnt == 0) + info->beamforming_cap &= ~BEAMFORMER_CAP_VHT_MU; + if (info->beamformee_su_cnt == 0) + info->beamforming_cap &= ~(BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT); + + _sounding_update_min_period(adapter, 0, _TRUE); +} + +static enum beamforming_cap _bfee_get_entry_cap_by_macid(PADAPTER adapter, u8 macid) +{ + struct beamforming_info *info; + struct beamformee_entry *bfee; + u8 i; + + + info = GET_BEAMFORM_INFO(adapter); + + for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (bfee->used == _FALSE) + continue; + if (bfee->mac_id == macid) + return bfee->cap; + } + + return BEAMFORMING_CAP_NONE; +} + +static void _beamforming_enter(PADAPTER adapter, void *p) +{ + struct mlme_priv *mlme; + struct ht_priv *htpriv; +#ifdef CONFIG_80211AC_VHT + struct vht_priv *vhtpriv; +#endif + struct mlme_ext_priv *mlme_ext; + struct sta_info *sta, *sta_copy; + struct beamforming_info *info; + struct beamformer_entry *bfer = NULL; + struct beamformee_entry *bfee = NULL; + u8 wireless_mode; + u8 sta_bf_cap; + u8 sounding_dim = 0; /* number of sounding dimensions */ + u8 comp_steering_num = 0; /* compressed steering number */ + + + mlme = &adapter->mlmepriv; + htpriv = &mlme->htpriv; +#ifdef CONFIG_80211AC_VHT + vhtpriv = &mlme->vhtpriv; +#endif + mlme_ext = &adapter->mlmeextpriv; + info = GET_BEAMFORM_INFO(adapter); + + sta_copy = (struct sta_info *)p; + sta = rtw_get_stainfo(&adapter->stapriv, sta_copy->cmn.mac_addr); + if (!sta) { + RTW_ERR("%s: Cann't find STA info for " MAC_FMT "\n", + __FUNCTION__, MAC_ARG(sta_copy->cmn.mac_addr)); + return; + } + + RTW_INFO("%s: find STA info for " MAC_FMT "\n", + __FUNCTION__, MAC_ARG(sta_copy->cmn.mac_addr)); + + if (sta != sta_copy) { + RTW_WARN("%s: Origin sta(fake)=%p realsta=%p for " MAC_FMT "\n", + __FUNCTION__, sta_copy, sta, MAC_ARG(sta_copy->cmn.mac_addr)); + } + + /* The current setting does not support Beaforming */ + wireless_mode = sta->wireless_mode; + if ((is_supported_ht(wireless_mode) == _FALSE) + && (is_supported_vht(wireless_mode) == _FALSE)) { + RTW_WARN("%s: Not support HT or VHT mode\n", __FUNCTION__); + return; + } + + if ((0 == htpriv->beamform_cap) +#ifdef CONFIG_80211AC_VHT + && (0 == vhtpriv->beamform_cap) +#endif + ) { + RTW_INFO("The configuration disabled Beamforming! Skip...\n"); + return; + } + + _get_sta_beamform_cap(adapter, sta, + &sta_bf_cap, &sounding_dim, &comp_steering_num); + RTW_INFO("STA Beamforming Capability=0x%02X\n", sta_bf_cap); + if (sta_bf_cap == BEAMFORMING_CAP_NONE) + return; + if ((sta_bf_cap & BEAMFORMEE_CAP_HT_EXPLICIT) + || (sta_bf_cap & BEAMFORMEE_CAP_VHT_SU) + || (sta_bf_cap & BEAMFORMEE_CAP_VHT_MU)) + sta_bf_cap |= BEAMFORMEE_CAP; + if ((sta_bf_cap & BEAMFORMER_CAP_HT_EXPLICIT) + || (sta_bf_cap & BEAMFORMER_CAP_VHT_SU) + || (sta_bf_cap & BEAMFORMER_CAP_VHT_MU)) + sta_bf_cap |= BEAMFORMER_CAP; + + if (sta_bf_cap & BEAMFORMER_CAP) { + /* The other side is beamformer */ + bfer = _bfer_add_entry(adapter, sta, sta_bf_cap, sounding_dim, comp_steering_num); + if (!bfer) + RTW_ERR("%s: Fail to allocate bfer entry!\n", __FUNCTION__); + } + if (sta_bf_cap & BEAMFORMEE_CAP) { + /* The other side is beamformee */ + bfee = _bfee_add_entry(adapter, sta, sta_bf_cap, sounding_dim, comp_steering_num); + if (!bfee) + RTW_ERR("%s: Fail to allocate bfee entry!\n", __FUNCTION__); + } + if (!bfer && !bfee) + return; + + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_ENTER, (u8*)sta); + + /* Perform sounding if there is BFee */ + if ((info->beamformee_su_cnt != 0) + || (info->beamformee_mu_cnt != 0)) { + if (SOUNDING_STATE_NONE == info->sounding_info.state) { + info->sounding_info.state = SOUNDING_STATE_INIT; + /* Start sounding after 2 sec */ + _set_timer(&info->sounding_timer, 2000); + } + } +} + +static void _beamforming_reset(PADAPTER adapter) +{ + RTW_ERR("%s: Not ready!!\n", __FUNCTION__); +} + +static void _beamforming_leave(PADAPTER adapter, u8 *ra) +{ + struct beamforming_info *info; + struct beamformer_entry *bfer = NULL; + struct beamformee_entry *bfee = NULL; + u8 bHwStateAddInit = _FALSE; + + + RTW_INFO("+%s\n", __FUNCTION__); + + info = GET_BEAMFORM_INFO(adapter); + bfer = _bfer_get_entry_by_addr(adapter, ra); + bfee = _bfee_get_entry_by_addr(adapter, ra); + + if (!bfer && !bfee) { + RTW_WARN("%s: " MAC_FMT " is neither beamforming ee or er!!\n", + __FUNCTION__, MAC_ARG(ra)); + return; + } + + if (bfer) + _bfer_remove_entry(adapter, bfer); + + if (bfee) + _bfee_remove_entry(adapter, bfee); + + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, ra); + + /* Stop sounding if there is no any BFee */ + if ((info->beamformee_su_cnt == 0) + && (info->beamformee_mu_cnt == 0)) { + _cancel_timer_ex(&info->sounding_timer); + _sounding_init(&info->sounding_info); + } + + RTW_INFO("-%s\n", __FUNCTION__); +} + +static void _beamforming_sounding_down(PADAPTER adapter, u8 status) +{ + struct beamforming_info *info; + struct sounding_info *sounding; + struct beamformee_entry *bfee; + + + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + RTW_INFO("+%s: sounding=%d, status=0x%02x\n", __FUNCTION__, sounding->state, status); + + if (sounding->state == SOUNDING_STATE_MU_START) { + RTW_INFO("%s: MU sounding done\n", __FUNCTION__); + sounding->state = SOUNDING_STATE_MU_SOUNDDOWN; + RTW_INFO("%s: Set to SOUNDING_STATE_MU_SOUNDDOWN\n", __FUNCTION__); + info->SetHalSoundownOnDemandCnt++; + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status); + } else if (sounding->state == SOUNDING_STATE_SU_START) { + RTW_INFO("%s: SU entry[%d] sounding down\n", __FUNCTION__, sounding->su_bfee_curidx); + bfee = &info->bfee_entry[sounding->su_bfee_curidx]; + sounding->state = SOUNDING_STATE_SU_SOUNDDOWN; + RTW_INFO("%s: Set to SOUNDING_STATE_SU_SOUNDDOWN\n", __FUNCTION__); + + /* + * + * bfee->bSoundingTimeout this flag still cannot avoid + * old sound down event happens in the new sounding period. + * 2015.12.10 + */ + if (_TRUE == bfee->bSoundingTimeout) { + RTW_WARN("%s: The entry[%d] is bSoundingTimeout!\n", __FUNCTION__, sounding->su_bfee_curidx); + bfee->bSoundingTimeout = _FALSE; + return; + } + + if (_TRUE == status) { + /* success */ + bfee->LogStatusFailCnt = 0; + info->SetHalSoundownOnDemandCnt++; + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status); + } else if (_TRUE == bfee->bDeleteSounding) { + RTW_WARN("%s: Delete entry[%d] sounding info!\n", __FUNCTION__, sounding->su_bfee_curidx); + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status); + bfee->bDeleteSounding = _FALSE; + } else { + bfee->LogStatusFailCnt++; + RTW_WARN("%s: LogStatusFailCnt=%d\n", __FUNCTION__, bfee->LogStatusFailCnt); + if (bfee->LogStatusFailCnt > 30) { + RTW_ERR("%s: LogStatusFailCnt > 30, Stop SOUNDING!!\n", __FUNCTION__); + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_LEAVE, bfee->mac_addr, ETH_ALEN, 1); + } + } + } else { + RTW_WARN("%s: unexpected sounding state:0x%02x\n", __FUNCTION__, sounding->state); + return; + } + + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 0); +} + +static void _c2h_snd_txbf(PADAPTER adapter, u8 *buf, u8 buf_len) +{ + struct beamforming_info *info; + u8 res; + + info = GET_BEAMFORM_INFO(adapter); + + _cancel_timer_ex(&info->sounding_timeout_timer); + + res = C2H_SND_TXBF_GET_SND_RESULT(buf) ? _TRUE : _FALSE; + RTW_INFO("+%s: %s\n", __FUNCTION__, res==_TRUE?"Success":"Fail!"); + + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 1); +} + +/* + * Description: + * This function is for phydm only + */ +enum beamforming_cap rtw_bf_bfee_get_entry_cap_by_macid(void *mlme, u8 macid) +{ + PADAPTER adapter; + enum beamforming_cap cap = BEAMFORMING_CAP_NONE; + + + adapter = mlme_to_adapter((struct mlme_priv *)mlme); + cap = _bfee_get_entry_cap_by_macid(adapter, macid); + + return cap; +} + +struct beamformer_entry *rtw_bf_bfer_get_entry_by_addr(PADAPTER adapter, u8 *ra) +{ + return _bfer_get_entry_by_addr(adapter, ra); +} + +struct beamformee_entry *rtw_bf_bfee_get_entry_by_addr(PADAPTER adapter, u8 *ra) +{ + return _bfee_get_entry_by_addr(adapter, ra); +} + +void rtw_bf_get_ndpa_packet(PADAPTER adapter, union recv_frame *precv_frame) +{ + RTW_DBG("+%s\n", __FUNCTION__); +} + +u32 rtw_bf_get_report_packet(PADAPTER adapter, union recv_frame *precv_frame) +{ + u32 ret = _SUCCESS; + struct beamforming_info *info; + struct beamformee_entry *bfee = NULL; + u8 *pframe; + u32 frame_len; + u8 *ta; + u8 *frame_body; + u8 category, action; + u8 *pMIMOCtrlField, *pCSIMatrix; + u8 Nc = 0, Nr = 0, CH_W = 0, Ng = 0, CodeBook = 0; + u16 CSIMatrixLen = 0; + + + RTW_INFO("+%s\n", __FUNCTION__); + + info = GET_BEAMFORM_INFO(adapter); + pframe = precv_frame->u.hdr.rx_data; + frame_len = precv_frame->u.hdr.len; + + /* Memory comparison to see if CSI report is the same with previous one */ + ta = get_addr2_ptr(pframe); + bfee = _bfee_get_entry_by_addr(adapter, ta); + if (!bfee) + return _FAIL; + + frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + category = frame_body[0]; + action = frame_body[1]; + + if ((category == RTW_WLAN_CATEGORY_VHT) + && (action == RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING)) { + pMIMOCtrlField = pframe + 26; + Nc = (*pMIMOCtrlField) & 0x7; + Nr = ((*pMIMOCtrlField) & 0x38) >> 3; + CH_W = (((*pMIMOCtrlField) & 0xC0) >> 6); + Ng = (*(pMIMOCtrlField+1)) & 0x3; + CodeBook = ((*(pMIMOCtrlField+1)) & 0x4) >> 2; + /* + * 24+(1+1+3)+2 + * ==> MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2) + */ + pCSIMatrix = pMIMOCtrlField + 3 + Nc; + CSIMatrixLen = frame_len - 26 - 3 - Nc; + info->TargetCSIInfo.bVHT = _TRUE; + } else if ((category == RTW_WLAN_CATEGORY_HT) + && (action == RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING)) { + pMIMOCtrlField = pframe + 26; + Nc = (*pMIMOCtrlField) & 0x3; + Nr = ((*pMIMOCtrlField) & 0xC) >> 2; + CH_W = ((*pMIMOCtrlField) & 0x10) >> 4; + Ng = ((*pMIMOCtrlField) & 0x60) >> 5; + CodeBook = ((*(pMIMOCtrlField+1)) & 0x6) >> 1; + /* + * 24+(1+1+6)+2 + * ==> MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2) + */ + pCSIMatrix = pMIMOCtrlField + 6 + Nr; + CSIMatrixLen = frame_len - 26 - 6 - Nr; + info->TargetCSIInfo.bVHT = _FALSE; + } + + /* Update current CSI report info */ + if ((_TRUE == info->bEnableSUTxBFWorkAround) + && (info->TargetSUBFee == bfee)) { + if ((info->TargetCSIInfo.Nc != Nc) || (info->TargetCSIInfo.Nr != Nr) || + (info->TargetCSIInfo.ChnlWidth != CH_W) || (info->TargetCSIInfo.Ng != Ng) || + (info->TargetCSIInfo.CodeBook != CodeBook)) { + info->TargetCSIInfo.Nc = Nc; + info->TargetCSIInfo.Nr = Nr; + info->TargetCSIInfo.ChnlWidth = CH_W; + info->TargetCSIInfo.Ng = Ng; + info->TargetCSIInfo.CodeBook = CodeBook; + + rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 1); + } + } + + RTW_INFO("%s: pkt type=%d-%d, Nc=%d, Nr=%d, CH_W=%d, Ng=%d, CodeBook=%d\n", + __FUNCTION__, category, action, Nc, Nr, CH_W, Ng, CodeBook); + + return ret; +} + +u8 rtw_bf_send_vht_gid_mgnt_packet(PADAPTER adapter, u8 *ra, u8 *gid, u8 *position) +{ + /* General */ + struct xmit_priv *xmitpriv; + struct mlme_priv *mlmepriv; + struct xmit_frame *pmgntframe; + /* MISC */ + struct pkt_attrib *attrib; + struct rtw_ieee80211_hdr *wlanhdr; + u8 *pframe, *ptr; + + + xmitpriv = &adapter->xmitpriv; + mlmepriv = &adapter->mlmepriv; + + pmgntframe = alloc_mgtxmitframe(xmitpriv); + if (!pmgntframe) + return _FALSE; + + /* update attribute */ + attrib = &pmgntframe->attrib; + update_mgntframe_attrib(adapter, attrib); + attrib->rate = MGN_6M; + attrib->bwmode = CHANNEL_WIDTH_20; + attrib->subtype = WIFI_ACTION; + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET; + wlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + wlanhdr->frame_ctl = 0; + set_frame_sub_type(pframe, attrib->subtype); + set_duration(pframe, 0); + SetFragNum(pframe, 0); + SetSeqNum(pframe, 0); + + _rtw_memcpy(wlanhdr->addr1, ra, ETH_ALEN); + _rtw_memcpy(wlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN); + _rtw_memcpy(wlanhdr->addr3, get_bssid(mlmepriv), ETH_ALEN); + + pframe[24] = RTW_WLAN_CATEGORY_VHT; + pframe[25] = RTW_WLAN_ACTION_VHT_GROUPID_MANAGEMENT; + /* Set Membership Status Array */ + ptr = pframe + 26; + _rtw_memcpy(ptr, gid, 8); + /* Set User Position Array */ + ptr = pframe + 34; + _rtw_memcpy(ptr, position, 16); + + attrib->pktlen = 54; + attrib->last_txcmdsz = attrib->pktlen; + + dump_mgntframe(adapter, pmgntframe); + + return _TRUE; +} + +/* + * Description: + * On VHT GID management frame by an MU beamformee. + */ +void rtw_bf_get_vht_gid_mgnt_packet(PADAPTER adapter, union recv_frame *precv_frame) +{ + u8 *pframe; + u8 *ta, *gid, *position; + + + RTW_DBG("+%s\n", __FUNCTION__); + + pframe = precv_frame->u.hdr.rx_data; + + /* Get address by Addr2 */ + ta = get_addr2_ptr(pframe); + /* Remove signaling TA */ + ta[0] &= 0xFE; + + /* Membership Status Array */ + gid = pframe + 26; + /* User Position Array */ + position= pframe + 34; + + _bfer_set_entry_gid(adapter, ta, gid, position); +} + +void rtw_bf_init(PADAPTER adapter) +{ + struct beamforming_info *info; + + + info = GET_BEAMFORM_INFO(adapter); + info->beamforming_cap = BEAMFORMING_CAP_NONE; + info->beamforming_state = BEAMFORMING_STATE_IDLE; +/* + info->bfee_entry[MAX_BEAMFORMEE_ENTRY_NUM]; + info->bfer_entry[MAX_BEAMFORMER_ENTRY_NUM]; +*/ + info->sounding_sequence = 0; + info->beamformee_su_cnt = 0; + info->beamformer_su_cnt = 0; + info->beamformee_su_reg_maping = 0; + info->beamformer_su_reg_maping = 0; + info->beamformee_mu_cnt = 0; + info->beamformer_mu_cnt = 0; + info->beamformee_mu_reg_maping = 0; + info->first_mu_bfee_index = 0xFF; + info->mu_bfer_curidx = 0xFF; + info->cur_csi_rpt_rate = HALMAC_OFDM24; + + _sounding_init(&info->sounding_info); + rtw_init_timer(&info->sounding_timer, adapter, _sounding_timer_handler, adapter); + rtw_init_timer(&info->sounding_timeout_timer, adapter, _sounding_timeout_timer_handler, adapter); + + info->SetHalBFEnterOnDemandCnt = 0; + info->SetHalBFLeaveOnDemandCnt = 0; + info->SetHalSoundownOnDemandCnt = 0; + + info->bEnableSUTxBFWorkAround = _TRUE; + info->TargetSUBFee = NULL; + + info->sounding_running = 0; +} + +void rtw_bf_cmd_hdl(PADAPTER adapter, u8 type, u8 *pbuf) +{ + switch (type) { + case BEAMFORMING_CTRL_ENTER: + _beamforming_enter(adapter, pbuf); + break; + + case BEAMFORMING_CTRL_LEAVE: + if (pbuf == NULL) + _beamforming_reset(adapter); + else + _beamforming_leave(adapter, pbuf); + break; + + case BEAMFORMING_CTRL_START_PERIOD: + _sounding_handler(adapter); + break; + + case BEAMFORMING_CTRL_END_PERIOD: + _beamforming_sounding_down(adapter, *pbuf); + break; + + case BEAMFORMING_CTRL_SET_GID_TABLE: + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_SET_GID_TABLE, pbuf); + break; + + case BEAMFORMING_CTRL_SET_CSI_REPORT: + rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_CSI_REPORT, pbuf); + break; + + default: + break; + } +} + +u8 rtw_bf_cmd(PADAPTER adapter, s32 type, u8 *pbuf, s32 size, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 *wk_buf; + u8 res = _SUCCESS; + + + if (!enqueue) { + rtw_bf_cmd_hdl(adapter, type, pbuf); + goto exit; + } + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + if (pbuf != NULL) { + wk_buf = rtw_zmalloc(size); + if (wk_buf == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + _rtw_memcpy(wk_buf, pbuf, size); + } else { + wk_buf = NULL; + size = 0; + } + + pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID; + pdrvextra_cmd_parm->type = type; + pdrvextra_cmd_parm->size = size; + pdrvextra_cmd_parm->pbuf = wk_buf; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +void rtw_bf_update_attrib(PADAPTER adapter, struct pkt_attrib *attrib, struct sta_info *sta) +{ + if (sta) { + attrib->txbf_g_id = sta->cmn.bf_info.g_id; + attrib->txbf_p_aid = sta->cmn.bf_info.p_aid; + } +} + +void rtw_bf_c2h_handler(PADAPTER adapter, u8 id, u8 *buf, u8 buf_len) +{ + switch (id) { + case CMD_ID_C2H_SND_TXBF: + _c2h_snd_txbf(adapter, buf, buf_len); + break; + } +} + +#define toMbps(bytes, secs) (rtw_division64(bytes >> 17, secs)) +void rtw_bf_update_traffic(PADAPTER adapter) +{ + struct beamforming_info *info; + struct sounding_info *sounding; + struct beamformee_entry *bfee; + struct sta_info *sta; + u8 bfee_cnt, sounding_idx, i; + u16 tp[MAX_BEAMFORMEE_ENTRY_NUM] = {0}; + u8 tx_rate[MAX_BEAMFORMEE_ENTRY_NUM] = {0}; + u64 tx_bytes, last_bytes; + u32 time; + systime last_timestamp; + u8 set_timer = _FALSE; + + + info = GET_BEAMFORM_INFO(adapter); + sounding = &info->sounding_info; + + /* Check any bfee exist? */ + bfee_cnt = info->beamformee_su_cnt + info->beamformee_mu_cnt; + if (bfee_cnt == 0) + return; + + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (_FALSE == bfee->used) + continue; + + sta = rtw_get_stainfo(&adapter->stapriv, bfee->mac_addr); + if (!sta) { + RTW_ERR("%s: Cann't find sta_info for " MAC_FMT "!\n", __FUNCTION__, MAC_ARG(bfee->mac_addr)); + continue; + } + + last_timestamp = bfee->tx_timestamp; + last_bytes = bfee->tx_bytes; + bfee->tx_timestamp = rtw_get_current_time(); + bfee->tx_bytes = sta->sta_stats.tx_bytes; + if (last_timestamp) { + if (bfee->tx_bytes >= last_bytes) + tx_bytes = bfee->tx_bytes - last_bytes; + else + tx_bytes = bfee->tx_bytes + (~last_bytes); + time = rtw_get_time_interval_ms(last_timestamp, bfee->tx_timestamp); + time = (time > 1000) ? time/1000 : 1; + tp[i] = toMbps(tx_bytes, time); + tx_rate[i] = rtw_get_current_tx_rate(adapter, sta); + RTW_INFO("%s: BFee idx(%d), MadId(%d), TxTP=%lld bytes (%d Mbps), txrate=%d\n", + __FUNCTION__, i, bfee->mac_id, tx_bytes, tp[i], tx_rate[i]); + } + } + + sounding_idx = phydm_get_beamforming_sounding_info(GET_PDM_ODM(adapter), tp, MAX_BEAMFORMEE_ENTRY_NUM, tx_rate); + + for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) { + bfee = &info->bfee_entry[i]; + if (_FALSE == bfee->used) { + if (sounding_idx & BIT(i)) + RTW_WARN("%s: bfee(%d) not in used but need sounding?!\n", __FUNCTION__, i); + continue; + } + + if (sounding_idx & BIT(i)) { + if (_FALSE == bfee->bApplySounding) { + bfee->bApplySounding = _TRUE; + bfee->SoundCnt = 0; + set_timer = _TRUE; + } + } else { + if (_TRUE == bfee->bApplySounding) { + bfee->bApplySounding = _FALSE; + bfee->bDeleteSounding = _TRUE; + bfee->SoundCnt = 0; + set_timer = _TRUE; + } + } + } + + if (_TRUE == set_timer) { + if (SOUNDING_STATE_NONE == info->sounding_info.state) { + info->sounding_info.state = SOUNDING_STATE_INIT; + _set_timer(&info->sounding_timer, 0); + } + } +} + +#else /* !RTW_BEAMFORMING_VERSION_2 */ + +/*PHYDM_BF - (BEAMFORMING_SUPPORT == 1)*/ +u32 rtw_beamforming_get_report_frame(PADAPTER Adapter, union recv_frame *precv_frame) +{ + u32 ret = _SUCCESS; + + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(Adapter); + struct dm_struct *pDM_Odm = &(pHalData->odmpriv); + + ret = beamforming_get_report_frame(pDM_Odm, precv_frame); + return ret; +} + +void rtw_beamforming_get_ndpa_frame(PADAPTER Adapter, union recv_frame *precv_frame) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(Adapter); + struct dm_struct *pDM_Odm = &(pHalData->odmpriv); + + beamforming_get_ndpa_frame(pDM_Odm, precv_frame); +} + +void beamforming_wk_hdl(_adapter *padapter, u8 type, u8 *pbuf) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); + struct dm_struct *pDM_Odm = &(pHalData->odmpriv); + + /*(BEAMFORMING_SUPPORT == 1)- for PHYDM beamfoming*/ + switch (type) { + case BEAMFORMING_CTRL_ENTER: { + struct sta_info *psta = (void *)pbuf; + u16 staIdx = psta->cmn.mac_id; + + beamforming_enter(pDM_Odm, staIdx, adapter_mac_addr(psta->padapter)); + break; + } + case BEAMFORMING_CTRL_LEAVE: + beamforming_leave(pDM_Odm, pbuf); + break; + default: + break; + + } +} + +u8 beamforming_wk_cmd(_adapter *padapter, s32 type, u8 *pbuf, s32 size, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 res = _SUCCESS; + + /*20170214 ad_hoc mode and mp_mode not support BF*/ + if ((padapter->registrypriv.mp_mode == 1) + || (pmlmeinfo->state == WIFI_FW_ADHOC_STATE)) + return res; + + if (enqueue) { + u8 *wk_buf; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + if (pbuf != NULL) { + wk_buf = rtw_zmalloc(size); + if (wk_buf == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + _rtw_memcpy(wk_buf, pbuf, size); + } else { + wk_buf = NULL; + size = 0; + } + + pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID; + pdrvextra_cmd_parm->type = type; + pdrvextra_cmd_parm->size = size; + pdrvextra_cmd_parm->pbuf = wk_buf; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else + beamforming_wk_hdl(padapter, type, pbuf); + +exit: + + + return res; +} + +void update_attrib_txbf_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta) +{ + if (psta) { + pattrib->txbf_g_id = psta->cmn.bf_info.g_id; + pattrib->txbf_p_aid = psta->cmn.bf_info.p_aid; + } +} +#endif /* !RTW_BEAMFORMING_VERSION_2 */ + +#endif /* CONFIG_BEAMFORMING */ diff --git a/core/rtw_br_ext.c b/core/rtw_br_ext.c new file mode 100644 index 0000000..b137ba2 --- /dev/null +++ b/core/rtw_br_ext.c @@ -0,0 +1,1590 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_BR_EXT_C_ + +#ifdef __KERNEL__ + #include + #include + #include +#if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 15, 0)) + #include +#endif + #include + #include + #include +#endif + +#if 1 /* rtw_wifi_driver */ + #include +#else /* rtw_wifi_driver */ + #include "./8192cd_cfg.h" + + #ifndef __KERNEL__ + #include "./sys-support.h" + #endif + + #include "./8192cd.h" + #include "./8192cd_headers.h" + #include "./8192cd_br_ext.h" + #include "./8192cd_debug.h" +#endif /* rtw_wifi_driver */ + +#ifdef CL_IPV6_PASS + #ifdef __KERNEL__ + #include + #include + #include + #if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)) + #include + #else + #include + #endif + #endif +#endif + +#ifdef CONFIG_BR_EXT + +/* #define BR_EXT_DEBUG */ + +#define NAT25_IPV4 01 +#define NAT25_IPV6 02 +#define NAT25_IPX 03 +#define NAT25_APPLE 04 +#define NAT25_PPPOE 05 + +#define RTL_RELAY_TAG_LEN (ETH_ALEN) +#define TAG_HDR_LEN 4 + +#define MAGIC_CODE 0x8186 +#define MAGIC_CODE_LEN 2 +#define WAIT_TIME_PPPOE 5 /* waiting time for pppoe server in sec */ + +/*----------------------------------------------------------------- + How database records network address: + 0 1 2 3 4 5 6 7 8 9 10 + |----|----|----|----|----|----|----|----|----|----|----| + IPv4 |type| | IP addr | + IPX |type| Net addr | Node addr | + IPX |type| Net addr |Sckt addr| + Apple |type| Network |node| + PPPoE |type| SID | AC MAC | +-----------------------------------------------------------------*/ + + +/* Find a tag in pppoe frame and return the pointer */ +static __inline__ unsigned char *__nat25_find_pppoe_tag(struct pppoe_hdr *ph, unsigned short type) +{ + unsigned char *cur_ptr, *start_ptr; + unsigned short tagLen, tagType; + + start_ptr = cur_ptr = (unsigned char *)ph->tag; + while ((cur_ptr - start_ptr) < ntohs(ph->length)) { + /* prevent un-alignment access */ + tagType = (unsigned short)((cur_ptr[0] << 8) + cur_ptr[1]); + tagLen = (unsigned short)((cur_ptr[2] << 8) + cur_ptr[3]); + if (tagType == type) + return cur_ptr; + cur_ptr = cur_ptr + TAG_HDR_LEN + tagLen; + } + return 0; +} + + +static __inline__ int __nat25_add_pppoe_tag(struct sk_buff *skb, struct pppoe_tag *tag) +{ + struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN); + int data_len; + + data_len = tag->tag_len + TAG_HDR_LEN; + if (skb_tailroom(skb) < data_len) { + _DEBUG_ERR("skb_tailroom() failed in add SID tag!\n"); + return -1; + } + + skb_put(skb, data_len); + /* have a room for new tag */ + memmove(((unsigned char *)ph->tag + data_len), (unsigned char *)ph->tag, ntohs(ph->length)); + ph->length = htons(ntohs(ph->length) + data_len); + memcpy((unsigned char *)ph->tag, tag, data_len); + return data_len; +} + +static int skb_pull_and_merge(struct sk_buff *skb, unsigned char *src, int len) +{ + int tail_len; + unsigned long end, tail; + + if ((src + len) > skb_tail_pointer(skb) || skb->len < len) + return -1; + + tail = (unsigned long)skb_tail_pointer(skb); + end = (unsigned long)src + len; + if (tail < end) + return -1; + + tail_len = (int)(tail - end); + if (tail_len > 0) + memmove(src, src + len, tail_len); + + skb_trim(skb, skb->len - len); + return 0; +} + +static __inline__ unsigned long __nat25_timeout(_adapter *priv) +{ + unsigned long timeout; + + timeout = jiffies - NAT25_AGEING_TIME * HZ; + + return timeout; +} + + +static __inline__ int __nat25_has_expired(_adapter *priv, + struct nat25_network_db_entry *fdb) +{ + if (time_before_eq(fdb->ageing_timer, __nat25_timeout(priv))) + return 1; + + return 0; +} + + +static __inline__ void __nat25_generate_ipv4_network_addr(unsigned char *networkAddr, + unsigned int *ipAddr) +{ + memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN); + + networkAddr[0] = NAT25_IPV4; + memcpy(networkAddr + 7, (unsigned char *)ipAddr, 4); +} + + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 15, 0)) +static __inline__ void __nat25_generate_ipx_network_addr_with_node(unsigned char *networkAddr, + unsigned int *ipxNetAddr, unsigned char *ipxNodeAddr) +{ + memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN); + + networkAddr[0] = NAT25_IPX; + memcpy(networkAddr + 1, (unsigned char *)ipxNetAddr, 4); + memcpy(networkAddr + 5, ipxNodeAddr, 6); +} + + +static __inline__ void __nat25_generate_ipx_network_addr_with_socket(unsigned char *networkAddr, + unsigned int *ipxNetAddr, unsigned short *ipxSocketAddr) +{ + memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN); + + networkAddr[0] = NAT25_IPX; + memcpy(networkAddr + 1, (unsigned char *)ipxNetAddr, 4); + memcpy(networkAddr + 5, (unsigned char *)ipxSocketAddr, 2); +} + + +static __inline__ void __nat25_generate_apple_network_addr(unsigned char *networkAddr, + unsigned short *network, unsigned char *node) +{ + memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN); + + networkAddr[0] = NAT25_APPLE; + memcpy(networkAddr + 1, (unsigned char *)network, 2); + networkAddr[3] = *node; +} +#endif + + +static __inline__ void __nat25_generate_pppoe_network_addr(unsigned char *networkAddr, + unsigned char *ac_mac, unsigned short *sid) +{ + memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN); + + networkAddr[0] = NAT25_PPPOE; + memcpy(networkAddr + 1, (unsigned char *)sid, 2); + memcpy(networkAddr + 3, (unsigned char *)ac_mac, 6); +} + + +#ifdef CL_IPV6_PASS +static void __nat25_generate_ipv6_network_addr(unsigned char *networkAddr, + unsigned int *ipAddr) +{ + memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN); + + networkAddr[0] = NAT25_IPV6; + memcpy(networkAddr + 1, (unsigned char *)ipAddr, 16); +} + + +static unsigned char *scan_tlv(unsigned char *data, int len, unsigned char tag, unsigned char len8b) +{ + while (len > 0) { + if (*data == tag && *(data + 1) == len8b && len >= len8b * 8) + return data + 2; + + len -= (*(data + 1)) * 8; + data += (*(data + 1)) * 8; + } + return NULL; +} + + +static int update_nd_link_layer_addr(unsigned char *data, int len, unsigned char *replace_mac) +{ + struct icmp6hdr *icmphdr = (struct icmp6hdr *)data; + unsigned char *mac; + + if (icmphdr->icmp6_type == NDISC_ROUTER_SOLICITATION) { + if (len >= 8) { + mac = scan_tlv(&data[8], len - 8, 1, 1); + if (mac) { + RTW_INFO("Router Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n", + mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], + replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]); + memcpy(mac, replace_mac, 6); + return 1; + } + } + } else if (icmphdr->icmp6_type == NDISC_ROUTER_ADVERTISEMENT) { + if (len >= 16) { + mac = scan_tlv(&data[16], len - 16, 1, 1); + if (mac) { + RTW_INFO("Router Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n", + mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], + replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]); + memcpy(mac, replace_mac, 6); + return 1; + } + } + } else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) { + if (len >= 24) { + mac = scan_tlv(&data[24], len - 24, 1, 1); + if (mac) { + RTW_INFO("Neighbor Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n", + mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], + replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]); + memcpy(mac, replace_mac, 6); + return 1; + } + } + } else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_ADVERTISEMENT) { + if (len >= 24) { + mac = scan_tlv(&data[24], len - 24, 2, 1); + if (mac) { + RTW_INFO("Neighbor Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n", + mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], + replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]); + memcpy(mac, replace_mac, 6); + return 1; + } + } + } else if (icmphdr->icmp6_type == NDISC_REDIRECT) { + if (len >= 40) { + mac = scan_tlv(&data[40], len - 40, 2, 1); + if (mac) { + RTW_INFO("Redirect, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n", + mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], + replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]); + memcpy(mac, replace_mac, 6); + return 1; + } + } + } + return 0; +} + +#ifdef SUPPORT_RX_UNI2MCAST +static void convert_ipv6_mac_to_mc(struct sk_buff *skb) +{ + struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN); + unsigned char *dst_mac = skb->data; + + /* dst_mac[0] = 0xff; */ + /* dst_mac[1] = 0xff; */ + /*modified by qinjunjie,ipv6 multicast address ix 0x33-33-xx-xx-xx-xx*/ + dst_mac[0] = 0x33; + dst_mac[1] = 0x33; + memcpy(&dst_mac[2], &iph->daddr.s6_addr32[3], 4); +#if defined(__LINUX_2_6__) + /*modified by qinjunjie,warning:should not remove next line*/ + skb->pkt_type = PACKET_MULTICAST; +#endif +} +#endif /* CL_IPV6_PASS */ +#endif /* SUPPORT_RX_UNI2MCAST */ + + +static __inline__ int __nat25_network_hash(unsigned char *networkAddr) +{ + if (networkAddr[0] == NAT25_IPV4) { + unsigned long x; + + x = networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10]; + + return x & (NAT25_HASH_SIZE - 1); +#if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 15, 0)) + } else if (networkAddr[0] == NAT25_IPX) { + unsigned long x; + + x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ + networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10]; + + return x & (NAT25_HASH_SIZE - 1); + } else if (networkAddr[0] == NAT25_APPLE) { + unsigned long x; + + x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3]; + + return x & (NAT25_HASH_SIZE - 1); +#endif + } else if (networkAddr[0] == NAT25_PPPOE) { + unsigned long x; + + x = networkAddr[0] ^ networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8]; + + return x & (NAT25_HASH_SIZE - 1); + } +#ifdef CL_IPV6_PASS + else if (networkAddr[0] == NAT25_IPV6) { + unsigned long x; + + x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ + networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10] ^ + networkAddr[11] ^ networkAddr[12] ^ networkAddr[13] ^ networkAddr[14] ^ networkAddr[15] ^ + networkAddr[16]; + + return x & (NAT25_HASH_SIZE - 1); + } +#endif + else { + unsigned long x = 0; + int i; + + for (i = 0; i < MAX_NETWORK_ADDR_LEN; i++) + x ^= networkAddr[i]; + + return x & (NAT25_HASH_SIZE - 1); + } +} + + +static __inline__ void __network_hash_link(_adapter *priv, + struct nat25_network_db_entry *ent, int hash) +{ + /* Caller must _enter_critical_bh already! */ + /* _irqL irqL; */ + /* _enter_critical_bh(&priv->br_ext_lock, &irqL); */ + + ent->next_hash = priv->nethash[hash]; + if (ent->next_hash != NULL) + ent->next_hash->pprev_hash = &ent->next_hash; + priv->nethash[hash] = ent; + ent->pprev_hash = &priv->nethash[hash]; + + /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */ +} + + +static __inline__ void __network_hash_unlink(struct nat25_network_db_entry *ent) +{ + /* Caller must _enter_critical_bh already! */ + /* _irqL irqL; */ + /* _enter_critical_bh(&priv->br_ext_lock, &irqL); */ + + *(ent->pprev_hash) = ent->next_hash; + if (ent->next_hash != NULL) + ent->next_hash->pprev_hash = ent->pprev_hash; + ent->next_hash = NULL; + ent->pprev_hash = NULL; + + /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */ +} + + +static int __nat25_db_network_lookup_and_replace(_adapter *priv, + struct sk_buff *skb, unsigned char *networkAddr) +{ + struct nat25_network_db_entry *db; + _irqL irqL; + _enter_critical_bh(&priv->br_ext_lock, &irqL); + + db = priv->nethash[__nat25_network_hash(networkAddr)]; + while (db != NULL) { + if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) { + if (!__nat25_has_expired(priv, db)) { + /* replace the destination mac address */ + memcpy(skb->data, db->macAddr, ETH_ALEN); + atomic_inc(&db->use_count); + +#ifdef CL_IPV6_PASS + RTW_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x" + "%02x%02x%02x%02x%02x%02x\n", + db->macAddr[0], + db->macAddr[1], + db->macAddr[2], + db->macAddr[3], + db->macAddr[4], + db->macAddr[5], + db->networkAddr[0], + db->networkAddr[1], + db->networkAddr[2], + db->networkAddr[3], + db->networkAddr[4], + db->networkAddr[5], + db->networkAddr[6], + db->networkAddr[7], + db->networkAddr[8], + db->networkAddr[9], + db->networkAddr[10], + db->networkAddr[11], + db->networkAddr[12], + db->networkAddr[13], + db->networkAddr[14], + db->networkAddr[15], + db->networkAddr[16]); +#else + RTW_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n", + db->macAddr[0], + db->macAddr[1], + db->macAddr[2], + db->macAddr[3], + db->macAddr[4], + db->macAddr[5], + db->networkAddr[0], + db->networkAddr[1], + db->networkAddr[2], + db->networkAddr[3], + db->networkAddr[4], + db->networkAddr[5], + db->networkAddr[6], + db->networkAddr[7], + db->networkAddr[8], + db->networkAddr[9], + db->networkAddr[10]); +#endif + } + _exit_critical_bh(&priv->br_ext_lock, &irqL); + return 1; + } + + db = db->next_hash; + } + + _exit_critical_bh(&priv->br_ext_lock, &irqL); + return 0; +} + + +static void __nat25_db_network_insert(_adapter *priv, + unsigned char *macAddr, unsigned char *networkAddr) +{ + struct nat25_network_db_entry *db; + int hash; + _irqL irqL; + _enter_critical_bh(&priv->br_ext_lock, &irqL); + + hash = __nat25_network_hash(networkAddr); + db = priv->nethash[hash]; + while (db != NULL) { + if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) { + memcpy(db->macAddr, macAddr, ETH_ALEN); + db->ageing_timer = jiffies; + _exit_critical_bh(&priv->br_ext_lock, &irqL); + return; + } + + db = db->next_hash; + } + + db = (struct nat25_network_db_entry *) rtw_malloc(sizeof(*db)); + if (db == NULL) { + _exit_critical_bh(&priv->br_ext_lock, &irqL); + return; + } + + memcpy(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN); + memcpy(db->macAddr, macAddr, ETH_ALEN); + atomic_set(&db->use_count, 1); + db->ageing_timer = jiffies; + + __network_hash_link(priv, db, hash); + + _exit_critical_bh(&priv->br_ext_lock, &irqL); +} + + +static void __nat25_db_print(_adapter *priv) +{ + _irqL irqL; + _enter_critical_bh(&priv->br_ext_lock, &irqL); + +#ifdef BR_EXT_DEBUG + static int counter = 0; + int i, j; + struct nat25_network_db_entry *db; + + counter++; + if ((counter % 16) != 0) + return; + + for (i = 0, j = 0; i < NAT25_HASH_SIZE; i++) { + db = priv->nethash[i]; + + while (db != NULL) { +#ifdef CL_IPV6_PASS + panic_printk("NAT25: DB(%d) H(%02d) C(%d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x" + "%02x%02x%02x%02x%02x%02x\n", + j, + i, + atomic_read(&db->use_count), + db->macAddr[0], + db->macAddr[1], + db->macAddr[2], + db->macAddr[3], + db->macAddr[4], + db->macAddr[5], + db->networkAddr[0], + db->networkAddr[1], + db->networkAddr[2], + db->networkAddr[3], + db->networkAddr[4], + db->networkAddr[5], + db->networkAddr[6], + db->networkAddr[7], + db->networkAddr[8], + db->networkAddr[9], + db->networkAddr[10], + db->networkAddr[11], + db->networkAddr[12], + db->networkAddr[13], + db->networkAddr[14], + db->networkAddr[15], + db->networkAddr[16]); +#else + panic_printk("NAT25: DB(%d) H(%02d) C(%d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n", + j, + i, + atomic_read(&db->use_count), + db->macAddr[0], + db->macAddr[1], + db->macAddr[2], + db->macAddr[3], + db->macAddr[4], + db->macAddr[5], + db->networkAddr[0], + db->networkAddr[1], + db->networkAddr[2], + db->networkAddr[3], + db->networkAddr[4], + db->networkAddr[5], + db->networkAddr[6], + db->networkAddr[7], + db->networkAddr[8], + db->networkAddr[9], + db->networkAddr[10]); +#endif + j++; + + db = db->next_hash; + } + } +#endif + + _exit_critical_bh(&priv->br_ext_lock, &irqL); +} + + + + +/* + * NAT2.5 interface + */ + +void nat25_db_cleanup(_adapter *priv) +{ + int i; + _irqL irqL; + _enter_critical_bh(&priv->br_ext_lock, &irqL); + + for (i = 0; i < NAT25_HASH_SIZE; i++) { + struct nat25_network_db_entry *f; + f = priv->nethash[i]; + while (f != NULL) { + struct nat25_network_db_entry *g; + + g = f->next_hash; + if (priv->scdb_entry == f) { + memset(priv->scdb_mac, 0, ETH_ALEN); + memset(priv->scdb_ip, 0, 4); + priv->scdb_entry = NULL; + } + __network_hash_unlink(f); + rtw_mfree((u8 *) f, sizeof(struct nat25_network_db_entry)); + + f = g; + } + } + + _exit_critical_bh(&priv->br_ext_lock, &irqL); +} + + +void nat25_db_expire(_adapter *priv) +{ + int i; + _irqL irqL; + _enter_critical_bh(&priv->br_ext_lock, &irqL); + + /* if(!priv->ethBrExtInfo.nat25_disable) */ + { + for (i = 0; i < NAT25_HASH_SIZE; i++) { + struct nat25_network_db_entry *f; + f = priv->nethash[i]; + + while (f != NULL) { + struct nat25_network_db_entry *g; + g = f->next_hash; + + if (__nat25_has_expired(priv, f)) { + if (atomic_dec_and_test(&f->use_count)) { +#ifdef BR_EXT_DEBUG +#ifdef CL_IPV6_PASS + panic_printk("NAT25 Expire H(%02d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x" + "%02x%02x%02x%02x%02x%02x\n", + i, + f->macAddr[0], + f->macAddr[1], + f->macAddr[2], + f->macAddr[3], + f->macAddr[4], + f->macAddr[5], + f->networkAddr[0], + f->networkAddr[1], + f->networkAddr[2], + f->networkAddr[3], + f->networkAddr[4], + f->networkAddr[5], + f->networkAddr[6], + f->networkAddr[7], + f->networkAddr[8], + f->networkAddr[9], + f->networkAddr[10], + f->networkAddr[11], + f->networkAddr[12], + f->networkAddr[13], + f->networkAddr[14], + f->networkAddr[15], + f->networkAddr[16]); +#else + + panic_printk("NAT25 Expire H(%02d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n", + i, + f->macAddr[0], + f->macAddr[1], + f->macAddr[2], + f->macAddr[3], + f->macAddr[4], + f->macAddr[5], + f->networkAddr[0], + f->networkAddr[1], + f->networkAddr[2], + f->networkAddr[3], + f->networkAddr[4], + f->networkAddr[5], + f->networkAddr[6], + f->networkAddr[7], + f->networkAddr[8], + f->networkAddr[9], + f->networkAddr[10]); +#endif +#endif + if (priv->scdb_entry == f) { + memset(priv->scdb_mac, 0, ETH_ALEN); + memset(priv->scdb_ip, 0, 4); + priv->scdb_entry = NULL; + } + __network_hash_unlink(f); + rtw_mfree((u8 *) f, sizeof(struct nat25_network_db_entry)); + } + } + + f = g; + } + } + } + + _exit_critical_bh(&priv->br_ext_lock, &irqL); +} + + +#ifdef SUPPORT_TX_MCAST2UNI +static int checkIPMcAndReplace(_adapter *priv, struct sk_buff *skb, unsigned int *dst_ip) +{ + struct stat_info *pstat; + struct list_head *phead, *plist; + int i; + + phead = &priv->asoc_list; + plist = phead->next; + + while (plist != phead) { + pstat = list_entry(plist, struct stat_info, asoc_list); + plist = plist->next; + + if (pstat->ipmc_num == 0) + continue; + + for (i = 0; i < MAX_IP_MC_ENTRY; i++) { + if (pstat->ipmc[i].used && !memcmp(&pstat->ipmc[i].mcmac[3], ((unsigned char *)dst_ip) + 1, 3)) { + memcpy(skb->data, pstat->ipmc[i].mcmac, ETH_ALEN); + return 1; + } + } + } + return 0; +} +#endif + +int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method) +{ + unsigned short protocol; + unsigned char networkAddr[MAX_NETWORK_ADDR_LEN]; + + if (skb == NULL) + return -1; + + if ((method <= NAT25_MIN) || (method >= NAT25_MAX)) + return -1; + + protocol = *((unsigned short *)(skb->data + 2 * ETH_ALEN)); + + /*---------------------------------------------------*/ + /* Handle IP frame */ + /*---------------------------------------------------*/ + if (protocol == __constant_htons(ETH_P_IP)) { + struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN); + + if (((unsigned char *)(iph) + (iph->ihl << 2)) >= (skb->data + ETH_HLEN + skb->len)) { + DEBUG_WARN("NAT25: malformed IP packet !\n"); + return -1; + } + + switch (method) { + case NAT25_CHECK: + return -1; + + case NAT25_INSERT: { + /* some muticast with source IP is all zero, maybe other case is illegal */ + /* in class A, B, C, host address is all zero or all one is illegal */ + if (iph->saddr == 0) + return 0; + RTW_INFO("NAT25: Insert IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr); + __nat25_generate_ipv4_network_addr(networkAddr, &iph->saddr); + /* record source IP address and , source mac address into db */ + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + + __nat25_db_print(priv); + } + return 0; + + case NAT25_LOOKUP: { + RTW_INFO("NAT25: Lookup IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr); +#ifdef SUPPORT_TX_MCAST2UNI + if (priv->pshare->rf_ft_var.mc2u_disable || + ((((OPMODE & (WIFI_STATION_STATE | WIFI_ASOC_STATE)) + == (WIFI_STATION_STATE | WIFI_ASOC_STATE)) && + !checkIPMcAndReplace(priv, skb, &iph->daddr)) || + (OPMODE & WIFI_ADHOC_STATE))) +#endif + { + __nat25_generate_ipv4_network_addr(networkAddr, &iph->daddr); + + if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) { + if (*((unsigned char *)&iph->daddr + 3) == 0xff) { + /* L2 is unicast but L3 is broadcast, make L2 bacome broadcast */ + RTW_INFO("NAT25: Set DA as boardcast\n"); + memset(skb->data, 0xff, ETH_ALEN); + } else { + /* forward unknow IP packet to upper TCP/IP */ + RTW_INFO("NAT25: Replace DA with BR's MAC\n"); + if ((*(u32 *)priv->br_mac) == 0 && (*(u16 *)(priv->br_mac + 4)) == 0) { + void netdev_br_init(struct net_device *netdev); + printk("Re-init netdev_br_init() due to br_mac==0!\n"); + netdev_br_init(priv->pnetdev); + } + memcpy(skb->data, priv->br_mac, ETH_ALEN); + } + } + } + } + return 0; + + default: + return -1; + } + } + + /*---------------------------------------------------*/ + /* Handle ARP frame */ + /*---------------------------------------------------*/ + else if (protocol == __constant_htons(ETH_P_ARP)) { + struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN); + unsigned char *arp_ptr = (unsigned char *)(arp + 1); + unsigned int *sender, *target; + + if (arp->ar_pro != __constant_htons(ETH_P_IP)) { + DEBUG_WARN("NAT25: arp protocol unknown (%4x)!\n", htons(arp->ar_pro)); + return -1; + } + + switch (method) { + case NAT25_CHECK: + return 0; /* skb_copy for all ARP frame */ + + case NAT25_INSERT: { + RTW_INFO("NAT25: Insert ARP, MAC=%02x%02x%02x%02x%02x%02x\n", arp_ptr[0], + arp_ptr[1], arp_ptr[2], arp_ptr[3], arp_ptr[4], arp_ptr[5]); + + /* change to ARP sender mac address to wlan STA address */ + memcpy(arp_ptr, GET_MY_HWADDR(priv), ETH_ALEN); + + arp_ptr += arp->ar_hln; + sender = (unsigned int *)arp_ptr; + + __nat25_generate_ipv4_network_addr(networkAddr, sender); + + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + + __nat25_db_print(priv); + } + return 0; + + case NAT25_LOOKUP: { + RTW_INFO("NAT25: Lookup ARP\n"); + + arp_ptr += arp->ar_hln; + sender = (unsigned int *)arp_ptr; + arp_ptr += (arp->ar_hln + arp->ar_pln); + target = (unsigned int *)arp_ptr; + + __nat25_generate_ipv4_network_addr(networkAddr, target); + + __nat25_db_network_lookup_and_replace(priv, skb, networkAddr); + + /* change to ARP target mac address to Lookup result */ + arp_ptr = (unsigned char *)(arp + 1); + arp_ptr += (arp->ar_hln + arp->ar_pln); + memcpy(arp_ptr, skb->data, ETH_ALEN); + } + return 0; + + default: + return -1; + } + } + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 15, 0)) + /*---------------------------------------------------*/ + /* Handle IPX and Apple Talk frame */ + /*---------------------------------------------------*/ + else if ((protocol == __constant_htons(ETH_P_IPX)) || + (protocol == __constant_htons(ETH_P_ATALK)) || + (protocol == __constant_htons(ETH_P_AARP))) { + unsigned char ipx_header[2] = {0xFF, 0xFF}; + struct ipxhdr *ipx = NULL; + struct elapaarp *ea = NULL; + struct ddpehdr *ddp = NULL; + unsigned char *framePtr = skb->data + ETH_HLEN; + + if (protocol == __constant_htons(ETH_P_IPX)) { + RTW_INFO("NAT25: Protocol=IPX (Ethernet II)\n"); + ipx = (struct ipxhdr *)framePtr; + } else { /* if(protocol <= __constant_htons(ETH_FRAME_LEN)) */ + if (!memcmp(ipx_header, framePtr, 2)) { + RTW_INFO("NAT25: Protocol=IPX (Ethernet 802.3)\n"); + ipx = (struct ipxhdr *)framePtr; + } else { + unsigned char ipx_8022_type = 0xE0; + unsigned char snap_8022_type = 0xAA; + + if (*framePtr == snap_8022_type) { + unsigned char ipx_snap_id[5] = {0x0, 0x0, 0x0, 0x81, 0x37}; /* IPX SNAP ID */ + unsigned char aarp_snap_id[5] = {0x00, 0x00, 0x00, 0x80, 0xF3}; /* Apple Talk AARP SNAP ID */ + unsigned char ddp_snap_id[5] = {0x08, 0x00, 0x07, 0x80, 0x9B}; /* Apple Talk DDP SNAP ID */ + + framePtr += 3; /* eliminate the 802.2 header */ + + if (!memcmp(ipx_snap_id, framePtr, 5)) { + framePtr += 5; /* eliminate the SNAP header */ + + RTW_INFO("NAT25: Protocol=IPX (Ethernet SNAP)\n"); + ipx = (struct ipxhdr *)framePtr; + } else if (!memcmp(aarp_snap_id, framePtr, 5)) { + framePtr += 5; /* eliminate the SNAP header */ + + ea = (struct elapaarp *)framePtr; + } else if (!memcmp(ddp_snap_id, framePtr, 5)) { + framePtr += 5; /* eliminate the SNAP header */ + + ddp = (struct ddpehdr *)framePtr; + } else { + DEBUG_WARN("NAT25: Protocol=Ethernet SNAP %02x%02x%02x%02x%02x\n", framePtr[0], + framePtr[1], framePtr[2], framePtr[3], framePtr[4]); + return -1; + } + } else if (*framePtr == ipx_8022_type) { + framePtr += 3; /* eliminate the 802.2 header */ + + if (!memcmp(ipx_header, framePtr, 2)) { + RTW_INFO("NAT25: Protocol=IPX (Ethernet 802.2)\n"); + ipx = (struct ipxhdr *)framePtr; + } else + return -1; + } + } + } + + /* IPX */ + if (ipx != NULL) { + switch (method) { + case NAT25_CHECK: + if (!memcmp(skb->data + ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) { + RTW_INFO("NAT25: Check IPX skb_copy\n"); + return 0; + } + return -1; + + case NAT25_INSERT: { + RTW_INFO("NAT25: Insert IPX, Dest=%08x,%02x%02x%02x%02x%02x%02x,%04x Source=%08x,%02x%02x%02x%02x%02x%02x,%04x\n", + ipx->ipx_dest.net, + ipx->ipx_dest.node[0], + ipx->ipx_dest.node[1], + ipx->ipx_dest.node[2], + ipx->ipx_dest.node[3], + ipx->ipx_dest.node[4], + ipx->ipx_dest.node[5], + ipx->ipx_dest.sock, + ipx->ipx_source.net, + ipx->ipx_source.node[0], + ipx->ipx_source.node[1], + ipx->ipx_source.node[2], + ipx->ipx_source.node[3], + ipx->ipx_source.node[4], + ipx->ipx_source.node[5], + ipx->ipx_source.sock); + + if (!memcmp(skb->data + ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) { + RTW_INFO("NAT25: Use IPX Net, and Socket as network addr\n"); + + __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock); + + /* change IPX source node addr to wlan STA address */ + memcpy(ipx->ipx_source.node, GET_MY_HWADDR(priv), ETH_ALEN); + } else + __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node); + + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + + __nat25_db_print(priv); + } + return 0; + + case NAT25_LOOKUP: { + if (!memcmp(GET_MY_HWADDR(priv), ipx->ipx_dest.node, ETH_ALEN)) { + RTW_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n"); + + __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock); + + __nat25_db_network_lookup_and_replace(priv, skb, networkAddr); + + /* replace IPX destination node addr with Lookup destination MAC addr */ + memcpy(ipx->ipx_dest.node, skb->data, ETH_ALEN); + } else { + __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node); + + __nat25_db_network_lookup_and_replace(priv, skb, networkAddr); + } + } + return 0; + + default: + return -1; + } + } + + /* AARP */ + else if (ea != NULL) { + /* Sanity check fields. */ + if (ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN) { + DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n"); + return -1; + } + + switch (method) { + case NAT25_CHECK: + return 0; + + case NAT25_INSERT: { + /* change to AARP source mac address to wlan STA address */ + memcpy(ea->hw_src, GET_MY_HWADDR(priv), ETH_ALEN); + + RTW_INFO("NAT25: Insert AARP, Source=%d,%d Destination=%d,%d\n", + ea->pa_src_net, + ea->pa_src_node, + ea->pa_dst_net, + ea->pa_dst_node); + + __nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node); + + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + + __nat25_db_print(priv); + } + return 0; + + case NAT25_LOOKUP: { + RTW_INFO("NAT25: Lookup AARP, Source=%d,%d Destination=%d,%d\n", + ea->pa_src_net, + ea->pa_src_node, + ea->pa_dst_net, + ea->pa_dst_node); + + __nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node); + + __nat25_db_network_lookup_and_replace(priv, skb, networkAddr); + + /* change to AARP destination mac address to Lookup result */ + memcpy(ea->hw_dst, skb->data, ETH_ALEN); + } + return 0; + + default: + return -1; + } + } + + /* DDP */ + else if (ddp != NULL) { + switch (method) { + case NAT25_CHECK: + return -1; + + case NAT25_INSERT: { + RTW_INFO("NAT25: Insert DDP, Source=%d,%d Destination=%d,%d\n", + ddp->deh_snet, + ddp->deh_snode, + ddp->deh_dnet, + ddp->deh_dnode); + + __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode); + + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + + __nat25_db_print(priv); + } + return 0; + + case NAT25_LOOKUP: { + RTW_INFO("NAT25: Lookup DDP, Source=%d,%d Destination=%d,%d\n", + ddp->deh_snet, + ddp->deh_snode, + ddp->deh_dnet, + ddp->deh_dnode); + + __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode); + + __nat25_db_network_lookup_and_replace(priv, skb, networkAddr); + } + return 0; + + default: + return -1; + } + } + + return -1; + } +#endif + + /*---------------------------------------------------*/ + /* Handle PPPoE frame */ + /*---------------------------------------------------*/ + else if ((protocol == __constant_htons(ETH_P_PPP_DISC)) || + (protocol == __constant_htons(ETH_P_PPP_SES))) { + struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN); + unsigned short *pMagic; + + switch (method) { + case NAT25_CHECK: + if (ph->sid == 0) + return 0; + return 1; + + case NAT25_INSERT: + if (ph->sid == 0) { /* Discovery phase according to tag */ + if (ph->code == PADI_CODE || ph->code == PADR_CODE) { + if (priv->ethBrExtInfo.addPPPoETag) { + struct pppoe_tag *tag, *pOldTag; + unsigned char tag_buf[40]; + int old_tag_len = 0; + + tag = (struct pppoe_tag *)tag_buf; + pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID)); + if (pOldTag) { /* if SID existed, copy old value and delete it */ + old_tag_len = ntohs(pOldTag->tag_len); + if (old_tag_len + TAG_HDR_LEN + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN > sizeof(tag_buf)) { + DEBUG_ERR("SID tag length too long!\n"); + return -1; + } + + memcpy(tag->tag_data + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN, + pOldTag->tag_data, old_tag_len); + + if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN + old_tag_len) < 0) { + DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n"); + return -1; + } + ph->length = htons(ntohs(ph->length) - TAG_HDR_LEN - old_tag_len); + } + + tag->tag_type = PTT_RELAY_SID; + tag->tag_len = htons(MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN + old_tag_len); + + /* insert the magic_code+client mac in relay tag */ + pMagic = (unsigned short *)tag->tag_data; + *pMagic = htons(MAGIC_CODE); + memcpy(tag->tag_data + MAGIC_CODE_LEN, skb->data + ETH_ALEN, ETH_ALEN); + + /* Add relay tag */ + if (__nat25_add_pppoe_tag(skb, tag) < 0) + return -1; + + RTW_INFO("NAT25: Insert PPPoE, forward %s packet\n", + (ph->code == PADI_CODE ? "PADI" : "PADR")); + } else { /* not add relay tag */ + if (priv->pppoe_connection_in_progress && + memcmp(skb->data + ETH_ALEN, priv->pppoe_addr, ETH_ALEN)) { + DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n"); + return -2; + } + + if (priv->pppoe_connection_in_progress == 0) + memcpy(priv->pppoe_addr, skb->data + ETH_ALEN, ETH_ALEN); + + priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE; + } + } else + return -1; + } else { /* session phase */ + RTW_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name); + + __nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid)); + + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + + __nat25_db_print(priv); + + if (!priv->ethBrExtInfo.addPPPoETag && + priv->pppoe_connection_in_progress && + !memcmp(skb->data + ETH_ALEN, priv->pppoe_addr, ETH_ALEN)) + priv->pppoe_connection_in_progress = 0; + } + return 0; + + case NAT25_LOOKUP: + if (ph->code == PADO_CODE || ph->code == PADS_CODE) { + if (priv->ethBrExtInfo.addPPPoETag) { + struct pppoe_tag *tag; + unsigned char *ptr; + unsigned short tagType, tagLen; + int offset = 0; + + ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID)); + if (ptr == 0) { + DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n"); + return -1; + } + + tag = (struct pppoe_tag *)ptr; + tagType = (unsigned short)((ptr[0] << 8) + ptr[1]); + tagLen = (unsigned short)((ptr[2] << 8) + ptr[3]); + + if ((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN))) { + DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen); + return -1; + } + + pMagic = (unsigned short *)tag->tag_data; + if (ntohs(*pMagic) != MAGIC_CODE) { + DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n", + (ph->code == PADO_CODE ? "PADO" : "PADS")); + return -1; + } + + memcpy(skb->data, tag->tag_data + MAGIC_CODE_LEN, ETH_ALEN); + + if (tagLen > MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN) + offset = TAG_HDR_LEN; + + if (skb_pull_and_merge(skb, ptr + offset, TAG_HDR_LEN + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN - offset) < 0) { + DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n"); + return -1; + } + ph->length = htons(ntohs(ph->length) - (TAG_HDR_LEN + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN - offset)); + if (offset > 0) + tag->tag_len = htons(tagLen - MAGIC_CODE_LEN - RTL_RELAY_TAG_LEN); + + RTW_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n", + (ph->code == PADO_CODE ? "PADO" : "PADS"), skb->dev->name); + } else { /* not add relay tag */ + if (!priv->pppoe_connection_in_progress) { + DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n"); + return -1; + } + memcpy(skb->data, priv->pppoe_addr, ETH_ALEN); + priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE; + } + } else { + if (ph->sid != 0) { + RTW_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name); + __nat25_generate_pppoe_network_addr(networkAddr, skb->data + ETH_ALEN, &(ph->sid)); + + __nat25_db_network_lookup_and_replace(priv, skb, networkAddr); + + __nat25_db_print(priv); + } else + return -1; + + } + return 0; + + default: + return -1; + } + } + + /*---------------------------------------------------*/ + /* Handle EAP frame */ + /*---------------------------------------------------*/ + else if (protocol == __constant_htons(0x888e)) { + switch (method) { + case NAT25_CHECK: + return -1; + + case NAT25_INSERT: + return 0; + + case NAT25_LOOKUP: + return 0; + + default: + return -1; + } + } + + /*---------------------------------------------------*/ + /* Handle C-Media proprietary frame */ + /*---------------------------------------------------*/ + else if ((protocol == __constant_htons(0xe2ae)) || + (protocol == __constant_htons(0xe2af))) { + switch (method) { + case NAT25_CHECK: + return -1; + + case NAT25_INSERT: + return 0; + + case NAT25_LOOKUP: + return 0; + + default: + return -1; + } + } + + /*---------------------------------------------------*/ + /* Handle IPV6 frame */ + /*---------------------------------------------------*/ +#ifdef CL_IPV6_PASS + else if (protocol == __constant_htons(ETH_P_IPV6)) { + struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN); + + if (sizeof(*iph) >= (skb->len - ETH_HLEN)) { + DEBUG_WARN("NAT25: malformed IPv6 packet !\n"); + return -1; + } + + switch (method) { + case NAT25_CHECK: + if (skb->data[0] & 1) + return 0; + return -1; + + case NAT25_INSERT: { + RTW_INFO("NAT25: Insert IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x," + " DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n", + iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3], + iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7], + iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3], + iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]); + + if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) { + __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr); + __nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr); + __nat25_db_print(priv); + + if (iph->nexthdr == IPPROTO_ICMPV6 && + skb->len > (ETH_HLEN + sizeof(*iph) + 4)) { + if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph), + skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) { + struct icmp6hdr *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph)); + hdr->icmp6_cksum = 0; + hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr, + iph->payload_len, + IPPROTO_ICMPV6, + csum_partial((__u8 *)hdr, iph->payload_len, 0)); + } + } + } + } + return 0; + + case NAT25_LOOKUP: + RTW_INFO("NAT25: Lookup IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x," + " DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n", + iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3], + iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7], + iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3], + iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]); + + + __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr); + if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) { +#ifdef SUPPORT_RX_UNI2MCAST + if (iph->daddr.s6_addr[0] == 0xff) + convert_ipv6_mac_to_mc(skb); +#endif + } + return 0; + + default: + return -1; + } + } +#endif /* CL_IPV6_PASS */ + + return -1; +} + + +int nat25_handle_frame(_adapter *priv, struct sk_buff *skb) +{ +#ifdef BR_EXT_DEBUG + if ((!priv->ethBrExtInfo.nat25_disable) && (!(skb->data[0] & 1))) { + panic_printk("NAT25: Input Frame: DA=%02x%02x%02x%02x%02x%02x SA=%02x%02x%02x%02x%02x%02x\n", + skb->data[0], + skb->data[1], + skb->data[2], + skb->data[3], + skb->data[4], + skb->data[5], + skb->data[6], + skb->data[7], + skb->data[8], + skb->data[9], + skb->data[10], + skb->data[11]); + } +#endif + + if (!(skb->data[0] & 1)) { + int is_vlan_tag = 0, i, retval = 0; + unsigned short vlan_hdr = 0; + + if (*((unsigned short *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_8021Q)) { + is_vlan_tag = 1; + vlan_hdr = *((unsigned short *)(skb->data + ETH_ALEN * 2 + 2)); + for (i = 0; i < 6; i++) + *((unsigned short *)(skb->data + ETH_ALEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + ETH_ALEN * 2 - 2 - i * 2)); + skb_pull(skb, 4); + } + + if (!priv->ethBrExtInfo.nat25_disable) { + _irqL irqL; + _enter_critical_bh(&priv->br_ext_lock, &irqL); + /* + * This function look up the destination network address from + * the NAT2.5 database. Return value = -1 means that the + * corresponding network protocol is NOT support. + */ + if (!priv->ethBrExtInfo.nat25sc_disable && + (*((unsigned short *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP)) && + !memcmp(priv->scdb_ip, skb->data + ETH_HLEN + 16, 4)) { + memcpy(skb->data, priv->scdb_mac, ETH_ALEN); + + _exit_critical_bh(&priv->br_ext_lock, &irqL); + } else { + _exit_critical_bh(&priv->br_ext_lock, &irqL); + + retval = nat25_db_handle(priv, skb, NAT25_LOOKUP); + } + } else { + if (((*((unsigned short *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP)) && + !memcmp(priv->br_ip, skb->data + ETH_HLEN + 16, 4)) || + ((*((unsigned short *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_ARP)) && + !memcmp(priv->br_ip, skb->data + ETH_HLEN + 24, 4))) { + /* for traffic to upper TCP/IP */ + retval = nat25_db_handle(priv, skb, NAT25_LOOKUP); + } + } + + if (is_vlan_tag) { + skb_push(skb, 4); + for (i = 0; i < 6; i++) + *((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2)); + *((unsigned short *)(skb->data + ETH_ALEN * 2)) = __constant_htons(ETH_P_8021Q); + *((unsigned short *)(skb->data + ETH_ALEN * 2 + 2)) = vlan_hdr; + } + + if (retval == -1) { + /* DEBUG_ERR("NAT25: Lookup fail!\n"); */ + return -1; + } + } + + return 0; +} + +#if 0 +void mac_clone(_adapter *priv, unsigned char *addr) +{ + struct sockaddr sa; + + memcpy(sa.sa_data, addr, ETH_ALEN); + RTW_INFO("MAC Clone: Addr=%02x%02x%02x%02x%02x%02x\n", + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); + rtl8192cd_set_hwaddr(priv->dev, &sa); +} + + +int mac_clone_handle_frame(_adapter *priv, struct sk_buff *skb) +{ + if (priv->ethBrExtInfo.macclone_enable && !priv->macclone_completed) { + if (!(skb->data[ETH_ALEN] & 1)) { /* check any other particular MAC add */ + if (memcmp(skb->data + ETH_ALEN, GET_MY_HWADDR(priv), ETH_ALEN) && + ((priv->dev->br_port) && + memcmp(skb->data + ETH_ALEN, priv->br_mac, ETH_ALEN))) { + mac_clone(priv, skb->data + ETH_ALEN); + priv->macclone_completed = 1; + } + } + } + + return 0; +} +#endif /* 0 */ + +#define SERVER_PORT 67 +#define CLIENT_PORT 68 +#define DHCP_MAGIC 0x63825363 +#define BROADCAST_FLAG 0x8000 + +struct dhcpMessage { + u_int8_t op; + u_int8_t htype; + u_int8_t hlen; + u_int8_t hops; + u_int32_t xid; + u_int16_t secs; + u_int16_t flags; + u_int32_t ciaddr; + u_int32_t yiaddr; + u_int32_t siaddr; + u_int32_t giaddr; + u_int8_t chaddr[16]; + u_int8_t sname[64]; + u_int8_t file[128]; + u_int32_t cookie; + u_int8_t options[308]; /* 312 - cookie */ +}; + +void dhcp_flag_bcast(_adapter *priv, struct sk_buff *skb) +{ + if (skb == NULL) + return; + + if (!priv->ethBrExtInfo.dhcp_bcst_disable) { + unsigned short protocol = *((unsigned short *)(skb->data + 2 * ETH_ALEN)); + + if (protocol == __constant_htons(ETH_P_IP)) { /* IP */ + struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN); + + if (iph->protocol == IPPROTO_UDP) { /* UDP */ + struct udphdr *udph = (struct udphdr *)((SIZE_PTR)iph + (iph->ihl << 2)); + + if ((udph->source == __constant_htons(CLIENT_PORT)) + && (udph->dest == __constant_htons(SERVER_PORT))) { /* DHCP request */ + struct dhcpMessage *dhcph = + (struct dhcpMessage *)((SIZE_PTR)udph + sizeof(struct udphdr)); + + if (dhcph->cookie == __constant_htonl(DHCP_MAGIC)) { /* match magic word */ + if (!(dhcph->flags & htons(BROADCAST_FLAG))) { /* if not broadcast */ + register int sum = 0; + + RTW_INFO("DHCP: change flag of DHCP request to broadcast.\n"); + /* or BROADCAST flag */ + dhcph->flags |= htons(BROADCAST_FLAG); + /* recalculate checksum */ + sum = ~(udph->check) & 0xffff; + sum += dhcph->flags; + while (sum >> 16) + sum = (sum & 0xffff) + (sum >> 16); + udph->check = ~sum; + } + } + } + } + } + } +} + + +void *scdb_findEntry(_adapter *priv, unsigned char *macAddr, + unsigned char *ipAddr) +{ + unsigned char networkAddr[MAX_NETWORK_ADDR_LEN]; + struct nat25_network_db_entry *db; + int hash; + /* _irqL irqL; */ + /* _enter_critical_bh(&priv->br_ext_lock, &irqL); */ + + __nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr); + hash = __nat25_network_hash(networkAddr); + db = priv->nethash[hash]; + while (db != NULL) { + if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) { + /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */ + return (void *)db; + } + + db = db->next_hash; + } + + /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */ + return NULL; +} + +#endif /* CONFIG_BR_EXT */ diff --git a/core/rtw_bt_mp.c b/core/rtw_bt_mp.c new file mode 100644 index 0000000..ce7aa29 --- /dev/null +++ b/core/rtw_bt_mp.c @@ -0,0 +1,1575 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + + +#include +#include + +#if defined(CONFIG_RTL8723B) + #include +#endif + +#if defined(CONFIG_RTL8723B) || defined(CONFIG_RTL8821A) +void MPh2c_timeout_handle(void *FunctionContext) +{ + PADAPTER pAdapter; + PMPT_CONTEXT pMptCtx; + + + RTW_INFO("[MPT], MPh2c_timeout_handle\n"); + + pAdapter = (PADAPTER)FunctionContext; + pMptCtx = &pAdapter->mppriv.mpt_ctx; + + pMptCtx->bMPh2c_timeout = _TRUE; + + if ((_FALSE == pMptCtx->MptH2cRspEvent) + || ((_TRUE == pMptCtx->MptH2cRspEvent) + && (_FALSE == pMptCtx->MptBtC2hEvent))) + _rtw_up_sema(&pMptCtx->MPh2c_Sema); +} + +u32 WaitC2Hevent(PADAPTER pAdapter, u8 *C2H_event, u32 delay_time) +{ + PMPT_CONTEXT pMptCtx = &(pAdapter->mppriv.mpt_ctx); + pMptCtx->bMPh2c_timeout = _FALSE; + + if (pAdapter->registrypriv.mp_mode == 0) { + RTW_INFO("[MPT], Error!! WaitC2Hevent mp_mode == 0!!\n"); + return _FALSE; + } + + _set_timer(&pMptCtx->MPh2c_timeout_timer, delay_time); + + _rtw_down_sema(&pMptCtx->MPh2c_Sema); + + if (pMptCtx->bMPh2c_timeout == _TRUE) { + *C2H_event = _FALSE; + + return _FALSE; + } + + /* for safty, cancel timer here again */ + _cancel_timer_ex(&pMptCtx->MPh2c_timeout_timer); + + return _TRUE; +} + +BT_CTRL_STATUS +mptbt_CheckC2hFrame( + PADAPTER Adapter, + PBT_H2C pH2c, + PBT_EXT_C2H pExtC2h +) +{ + BT_CTRL_STATUS c2hStatus = BT_STATUS_C2H_SUCCESS; + + /* RTW_INFO("[MPT], MPT rsp C2H hex: %x %x %x %x %x %x\n"), pExtC2h , pExtC2h+1 ,pExtC2h+2 ,pExtC2h+3 ,pExtC2h+4 ,pExtC2h+5); */ + + RTW_INFO("[MPT], statusCode = 0x%x\n", pExtC2h->statusCode); + RTW_INFO("[MPT], retLen = %d\n", pExtC2h->retLen); + RTW_INFO("[MPT], opCodeVer : req/rsp=%d/%d\n", pH2c->opCodeVer, pExtC2h->opCodeVer); + RTW_INFO("[MPT], reqNum : req/rsp=%d/%d\n", pH2c->reqNum, pExtC2h->reqNum); + if (pExtC2h->reqNum != pH2c->reqNum) { + c2hStatus = BT_STATUS_C2H_REQNUM_MISMATCH; + RTW_INFO("[MPT], Error!! C2H reqNum Mismatch!!\n"); + } else if (pExtC2h->opCodeVer != pH2c->opCodeVer) { + c2hStatus = BT_STATUS_OPCODE_L_VERSION_MISMATCH; + RTW_INFO("[MPT], Error!! OPCode version L mismatch!!\n"); + } + + return c2hStatus; +} + +BT_CTRL_STATUS +mptbt_SendH2c( + PADAPTER Adapter, + PBT_H2C pH2c, + u16 h2cCmdLen +) +{ + /* KIRQL OldIrql = KeGetCurrentIrql(); */ + BT_CTRL_STATUS h2cStatus = BT_STATUS_H2C_SUCCESS; + PMPT_CONTEXT pMptCtx = &(Adapter->mppriv.mpt_ctx); + u8 i; + + RTW_INFO("[MPT], mptbt_SendH2c()=========>\n"); + + /* PlatformResetEvent(&pMptCtx->MptH2cRspEvent); */ + /* PlatformResetEvent(&pMptCtx->MptBtC2hEvent); */ + + /* if(OldIrql == PASSIVE_LEVEL) + * { */ + /* RTPRINT_DATA(FMPBT, FMPBT_H2C_CONTENT, ("[MPT], MPT H2C hex:\n"), pH2c, h2cCmdLen); */ + + for (i = 0; i < BT_H2C_MAX_RETRY; i++) { + RTW_INFO("[MPT], Send H2C command to wifi!!!\n"); + + pMptCtx->MptH2cRspEvent = _FALSE; + pMptCtx->MptBtC2hEvent = _FALSE; + +#if defined(CONFIG_RTL8723B) + rtl8723b_set_FwBtMpOper_cmd(Adapter, pH2c->opCode, pH2c->opCodeVer, pH2c->reqNum, pH2c->buf); +#endif + pMptCtx->h2cReqNum++; + pMptCtx->h2cReqNum %= 16; + + if (WaitC2Hevent(Adapter, &pMptCtx->MptH2cRspEvent, 100)) { + RTW_INFO("[MPT], Received WiFi MptH2cRspEvent!!!\n"); + if (WaitC2Hevent(Adapter, &pMptCtx->MptBtC2hEvent, 400)) { + RTW_INFO("[MPT], Received MptBtC2hEvent!!!\n"); + break; + } else { + RTW_INFO("[MPT], Error!!BT MptBtC2hEvent timeout!!\n"); + h2cStatus = BT_STATUS_H2C_BT_NO_RSP; + } + } else { + RTW_INFO("[MPT], Error!!WiFi MptH2cRspEvent timeout!!\n"); + h2cStatus = BT_STATUS_H2C_TIMTOUT; + } + } + /* } + * else + * { + * RT_ASSERT(FALSE, ("[MPT], mptbt_SendH2c() can only run under PASSIVE_LEVEL!!\n")); + * h2cStatus = BT_STATUS_WRONG_LEVEL; + * } */ + + RTW_INFO("[MPT], mptbt_SendH2c()<=========\n"); + return h2cStatus; +} + + + +BT_CTRL_STATUS +mptbt_CheckBtRspStatus( + PADAPTER Adapter, + PBT_EXT_C2H pExtC2h +) +{ + BT_CTRL_STATUS retStatus = BT_OP_STATUS_SUCCESS; + + switch (pExtC2h->statusCode) { + case BT_OP_STATUS_SUCCESS: + retStatus = BT_STATUS_BT_OP_SUCCESS; + RTW_INFO("[MPT], BT status : BT_STATUS_SUCCESS\n"); + break; + case BT_OP_STATUS_VERSION_MISMATCH: + retStatus = BT_STATUS_OPCODE_L_VERSION_MISMATCH; + RTW_INFO("[MPT], BT status : BT_STATUS_OPCODE_L_VERSION_MISMATCH\n"); + break; + case BT_OP_STATUS_UNKNOWN_OPCODE: + retStatus = BT_STATUS_UNKNOWN_OPCODE_L; + RTW_INFO("[MPT], BT status : BT_STATUS_UNKNOWN_OPCODE_L\n"); + break; + case BT_OP_STATUS_ERROR_PARAMETER: + retStatus = BT_STATUS_PARAMETER_FORMAT_ERROR_L; + RTW_INFO("[MPT], BT status : BT_STATUS_PARAMETER_FORMAT_ERROR_L\n"); + break; + default: + retStatus = BT_STATUS_UNKNOWN_STATUS_L; + RTW_INFO("[MPT], BT status : BT_STATUS_UNKNOWN_STATUS_L\n"); + break; + } + + return retStatus; +} + + + +BT_CTRL_STATUS +mptbt_BtFwOpCodeProcess( + PADAPTER Adapter, + u8 btFwOpCode, + u8 opCodeVer, + u8 *pH2cPar, + u8 h2cParaLen +) +{ + u8 H2C_Parameter[6] = {0}; + PBT_H2C pH2c = (PBT_H2C)&H2C_Parameter[0]; + PMPT_CONTEXT pMptCtx = &(Adapter->mppriv.mpt_ctx); + PBT_EXT_C2H pExtC2h = (PBT_EXT_C2H)&pMptCtx->c2hBuf[0]; + u16 paraLen = 0, i; + BT_CTRL_STATUS h2cStatus = BT_STATUS_H2C_SUCCESS, c2hStatus = BT_STATUS_C2H_SUCCESS; + BT_CTRL_STATUS retStatus = BT_STATUS_H2C_BT_NO_RSP; + + if (Adapter->registrypriv.mp_mode == 0) { + RTW_INFO("[MPT], Error!! mptbt_BtFwOpCodeProces mp_mode == 0!!\n"); + return _FALSE; + } + + pH2c->opCode = btFwOpCode; + pH2c->opCodeVer = opCodeVer; + pH2c->reqNum = pMptCtx->h2cReqNum; + /* PlatformMoveMemory(&pH2c->buf[0], pH2cPar, h2cParaLen); */ + /* _rtw_memcpy(&pH2c->buf[0], pH2cPar, h2cParaLen); */ + _rtw_memcpy(pH2c->buf, pH2cPar, h2cParaLen); + + RTW_INFO("[MPT], pH2c->opCode=%d\n", pH2c->opCode); + RTW_INFO("[MPT], pH2c->opCodeVer=%d\n", pH2c->opCodeVer); + RTW_INFO("[MPT], pH2c->reqNum=%d\n", pH2c->reqNum); + RTW_INFO("[MPT], h2c parameter length=%d\n", h2cParaLen); + for (i = 0; i < h2cParaLen; i++) + RTW_INFO("[MPT], parameter[%d]=0x%02x\n", i, pH2c->buf[i]); + + h2cStatus = mptbt_SendH2c(Adapter, pH2c, h2cParaLen + 2); + if (BT_STATUS_H2C_SUCCESS == h2cStatus) { + /* if reach here, it means H2C get the correct c2h response, */ + c2hStatus = mptbt_CheckC2hFrame(Adapter, pH2c, pExtC2h); + if (BT_STATUS_C2H_SUCCESS == c2hStatus) + retStatus = mptbt_CheckBtRspStatus(Adapter, pExtC2h); + else { + RTW_INFO("[MPT], Error!! C2H failed for pH2c->opCode=%d\n", pH2c->opCode); + /* check c2h status error, return error status code to upper layer. */ + retStatus = c2hStatus; + } + } else { + RTW_INFO("[MPT], Error!! H2C failed for pH2c->opCode=%d\n", pH2c->opCode); + /* check h2c status error, return error status code to upper layer. */ + retStatus = h2cStatus; + } + + return retStatus; +} + + + + +u16 +mptbt_BtReady( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode; + u8 btOpcodeVer = 0; + PMPT_CONTEXT pMptCtx = &(Adapter->mppriv.mpt_ctx); + PBT_EXT_C2H pExtC2h = (PBT_EXT_C2H)&pMptCtx->c2hBuf[0]; + u8 i; + u8 btFwVer = 0, bdAddr[6] = {0}; + u16 btRealFwVer = 0; + u16 *pu2Tmp = NULL; + + /* */ + /* check upper layer parameters */ + /* */ + + /* 1. check upper layer opcode version */ + if (pBtReq->opCodeVer != 1) { + RTW_INFO("[MPT], Error!! Upper OP code version not match!!!\n"); + pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH; + return paraLen; + } + + pBtRsp->pParamStart[0] = MP_BT_NOT_READY; + paraLen = 10; + /* */ + /* execute lower layer opcodes */ + /* */ + + /* Get BT FW version */ + /* fill h2c parameters */ + btOpcode = BT_LO_OP_GET_BT_VERSION; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } else { + pu2Tmp = (u16 *)&pExtC2h->buf[0]; + btRealFwVer = *pu2Tmp; + btFwVer = pExtC2h->buf[1]; + RTW_INFO("[MPT], btRealFwVer=0x%x, btFwVer=0x%x\n", btRealFwVer, btFwVer); + } + + /* Get BD Address */ + /* fill h2c parameters */ + btOpcode = BT_LO_OP_GET_BD_ADDR_L; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } else { + bdAddr[5] = pExtC2h->buf[0]; + bdAddr[4] = pExtC2h->buf[1]; + bdAddr[3] = pExtC2h->buf[2]; + } + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_GET_BD_ADDR_H; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } else { + bdAddr[2] = pExtC2h->buf[0]; + bdAddr[1] = pExtC2h->buf[1]; + bdAddr[0] = pExtC2h->buf[2]; + } + RTW_INFO("[MPT], Local BDAddr:"); + for (i = 0; i < 6; i++) + RTW_INFO(" 0x%x ", bdAddr[i]); + pBtRsp->status = BT_STATUS_SUCCESS; + pBtRsp->pParamStart[0] = MP_BT_READY; + pu2Tmp = (u16 *)&pBtRsp->pParamStart[1]; + *pu2Tmp = btRealFwVer; + pBtRsp->pParamStart[3] = btFwVer; + for (i = 0; i < 6; i++) + pBtRsp->pParamStart[4 + i] = bdAddr[5 - i]; + + return paraLen; +} + +void mptbt_close_WiFiRF(PADAPTER Adapter) +{ + phy_set_bb_reg(Adapter, 0x824, 0xF, 0x0); + phy_set_bb_reg(Adapter, 0x824, 0x700000, 0x0); + phy_set_rf_reg(Adapter, RF_PATH_A, 0x0, 0xF0000, 0x0); +} + +void mptbt_open_WiFiRF(PADAPTER Adapter) +{ + phy_set_bb_reg(Adapter, 0x824, 0x700000, 0x3); + phy_set_bb_reg(Adapter, 0x824, 0xF, 0x2); + phy_set_rf_reg(Adapter, RF_PATH_A, 0x0, 0xF0000, 0x3); +} + +u32 mptbt_switch_RF(PADAPTER Adapter, u8 Enter) +{ + u16 tmp_2byte = 0; + + /* Enter test mode */ + if (Enter) { + /* 1>. close WiFi RF */ + mptbt_close_WiFiRF(Adapter); + + /* 2>. change ant switch to BT */ + tmp_2byte = rtw_read16(Adapter, 0x860); + tmp_2byte = tmp_2byte | BIT(9); + tmp_2byte = tmp_2byte & (~BIT(8)); + rtw_write16(Adapter, 0x860, tmp_2byte); + rtw_write16(Adapter, 0x870, 0x300); + } else { + /* 1>. Open WiFi RF */ + mptbt_open_WiFiRF(Adapter); + + /* 2>. change ant switch back */ + tmp_2byte = rtw_read16(Adapter, 0x860); + tmp_2byte = tmp_2byte | BIT(8); + tmp_2byte = tmp_2byte & (~BIT(9)); + rtw_write16(Adapter, 0x860, tmp_2byte); + rtw_write16(Adapter, 0x870, 0x300); + } + + return 0; +} + +u16 +mptbt_BtSetMode( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode; + u8 btOpcodeVer = 0; + u8 btModeToSet = 0; + + /* */ + /* check upper layer parameters */ + /* */ + /* 1. check upper layer opcode version */ + if (pBtReq->opCodeVer != 1) { + RTW_INFO("[MPT], Error!! Upper OP code version not match!!!\n"); + pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH; + return paraLen; + } + /* 2. check upper layer parameter length */ + if (1 == pBtReq->paraLength) { + btModeToSet = pBtReq->pParamStart[0]; + RTW_INFO("[MPT], BtTestMode=%d\n", btModeToSet); + } else { + RTW_INFO("[MPT], Error!! wrong parameter length=%d (should be 1)\n", pBtReq->paraLength); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + + /* */ + /* execute lower layer opcodes */ + /* */ + + /* 1. fill h2c parameters */ + /* check bt mode */ + btOpcode = BT_LO_OP_SET_BT_MODE; + if (btModeToSet >= MP_BT_MODE_MAX) { + pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + mptbt_switch_RF(Adapter, 1); + + h2cParaBuf[0] = btModeToSet; + h2cParaLen = 1; + /* 2. execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* 3. construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS == retStatus) + pBtRsp->status = BT_STATUS_SUCCESS; + else { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + } + + return paraLen; +} + + +void +MPTBT_FwC2hBtMpCtrl( + PADAPTER Adapter, + u8 *tmpBuf, + u8 length +) +{ + u32 i; + PMPT_CONTEXT pMptCtx = &(Adapter->mppriv.mpt_ctx); + PBT_EXT_C2H pExtC2h = (PBT_EXT_C2H)tmpBuf; + + if (GET_HAL_DATA(Adapter)->bBTFWReady == _FALSE || Adapter->registrypriv.mp_mode == 0) { + /* RTW_INFO("Ignore C2H BT MP Info since not in MP mode\n"); */ + return; + } + if (length > 32 || length < 3) { + RTW_INFO("\n [MPT], pExtC2h->buf hex: length=%d > 32 || < 3\n", length); + return; + } + + /* cancel_timeout for h2c handle */ + _cancel_timer_ex(&pMptCtx->MPh2c_timeout_timer); + + for (i = 0; i < length; i++) + RTW_INFO("[MPT], %s, buf[%d]=0x%02x ", __FUNCTION__, i, tmpBuf[i]); + RTW_INFO("[MPT], pExtC2h->extendId=0x%x\n", pExtC2h->extendId); + + switch (pExtC2h->extendId) { + case EXT_C2H_WIFI_FW_ACTIVE_RSP: + RTW_INFO("[MPT], EXT_C2H_WIFI_FW_ACTIVE_RSP\n"); +#if 0 + RTW_INFO("[MPT], pExtC2h->buf hex:\n"); + for (i = 0; i < (length - 3); i++) + RTW_INFO(" 0x%x ", pExtC2h->buf[i]); +#endif + if ((_FALSE == pMptCtx->bMPh2c_timeout) + && (_FALSE == pMptCtx->MptH2cRspEvent)) { + pMptCtx->MptH2cRspEvent = _TRUE; + _rtw_up_sema(&pMptCtx->MPh2c_Sema); + } + break; + + case EXT_C2H_TRIG_BY_BT_FW: + RTW_INFO("[MPT], EXT_C2H_TRIG_BY_BT_FW\n"); + _rtw_memcpy(&pMptCtx->c2hBuf[0], tmpBuf, length); + RTW_INFO("[MPT], pExtC2h->statusCode=0x%x\n", pExtC2h->statusCode); + RTW_INFO("[MPT], pExtC2h->retLen=0x%x\n", pExtC2h->retLen); + RTW_INFO("[MPT], pExtC2h->opCodeVer=0x%x\n", pExtC2h->opCodeVer); + RTW_INFO("[MPT], pExtC2h->reqNum=0x%x\n", pExtC2h->reqNum); + for (i = 0; i < (length - 3); i++) + RTW_INFO("[MPT], pExtC2h->buf[%d]=0x%02x\n", i, pExtC2h->buf[i]); + + if ((_FALSE == pMptCtx->bMPh2c_timeout) + && (_TRUE == pMptCtx->MptH2cRspEvent) + && (_FALSE == pMptCtx->MptBtC2hEvent)) { + pMptCtx->MptBtC2hEvent = _TRUE; + _rtw_up_sema(&pMptCtx->MPh2c_Sema); + } + break; + + default: + RTW_INFO("[MPT], EXT_C2H Target not found,pExtC2h->extendId =%d ,pExtC2h->reqNum=%d\n", pExtC2h->extendId, pExtC2h->reqNum); + break; + } + + + +} + + +u16 +mptbt_BtGetGeneral( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + PMPT_CONTEXT pMptCtx = &(Adapter->mppriv.mpt_ctx); + PBT_EXT_C2H pExtC2h = (PBT_EXT_C2H)&pMptCtx->c2hBuf[0]; + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode, bdAddr[6] = {0}; + u8 btOpcodeVer = 0; + u8 getType = 0, i; + u16 getParaLen = 0, validParaLen = 0; + u8 regType = 0, reportType = 0; + u32 regAddr = 0, regValue = 0; + u32 *pu4Tmp; + u16 *pu2Tmp; + u8 *pu1Tmp; + + /* */ + /* check upper layer parameters */ + /* */ + + /* check upper layer opcode version */ + if (pBtReq->opCodeVer != 1) { + RTW_INFO("[MPT], Error!! Upper OP code version not match!!!\n"); + pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH; + return paraLen; + } + /* check upper layer parameter length */ + if (pBtReq->paraLength < 1) { + RTW_INFO("[MPT], Error!! wrong parameter length=%d (should larger than 1)\n", pBtReq->paraLength); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + getParaLen = pBtReq->paraLength - 1; + getType = pBtReq->pParamStart[0]; + + RTW_INFO("[MPT], getType=%d, getParaLen=%d\n", getType, getParaLen); + + /* check parameter first */ + switch (getType) { + case BT_GGET_REG: + RTW_INFO("[MPT], [BT_GGET_REG]\n"); + validParaLen = 5; + if (getParaLen == validParaLen) { + btOpcode = BT_LO_OP_READ_REG; + regType = pBtReq->pParamStart[1]; + pu4Tmp = (u32 *)&pBtReq->pParamStart[2]; + regAddr = *pu4Tmp; + RTW_INFO("[MPT], BT_GGET_REG regType=0x%02x, regAddr=0x%08x!!\n", + regType, regAddr); + if (regType >= BT_REG_MAX) { + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + if (((BT_REG_RF == regType) && (regAddr > 0x7f)) || + ((BT_REG_MODEM == regType) && (regAddr > 0x1ff)) || + ((BT_REG_BLUEWIZE == regType) && (regAddr > 0xfff)) || + ((BT_REG_VENDOR == regType) && (regAddr > 0xfff)) || + ((BT_REG_LE == regType) && (regAddr > 0xfff))) { + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + } + } + break; + case BT_GGET_STATUS: + RTW_INFO("[MPT], [BT_GGET_STATUS]\n"); + validParaLen = 0; + break; + case BT_GGET_REPORT: + RTW_INFO("[MPT], [BT_GGET_REPORT]\n"); + validParaLen = 1; + if (getParaLen == validParaLen) { + reportType = pBtReq->pParamStart[1]; + RTW_INFO("[MPT], BT_GGET_REPORT reportType=0x%x!!\n", reportType); + if (reportType >= BT_REPORT_MAX) { + pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + } + break; + default: { + RTW_INFO("[MPT], Error!! getType=%d, out of range\n", getType); + pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + break; + } + if (getParaLen != validParaLen) { + RTW_INFO("[MPT], Error!! wrong parameter length=%d for BT_GET_GEN_CMD cmd id=0x%x, paraLen should=0x%x\n", + getParaLen, getType, validParaLen); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + + /* */ + /* execute lower layer opcodes */ + /* */ + if (BT_GGET_REG == getType) { + /* fill h2c parameters */ + /* here we should write reg value first then write the address, adviced by Austin */ + btOpcode = BT_LO_OP_READ_REG; + h2cParaBuf[0] = regType; + h2cParaBuf[1] = pBtReq->pParamStart[2]; + h2cParaBuf[2] = pBtReq->pParamStart[3]; + h2cParaLen = 3; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + pu2Tmp = (u16 *)&pExtC2h->buf[0]; + regValue = *pu2Tmp; + RTW_INFO("[MPT], read reg regType=0x%02x, regAddr=0x%08x, regValue=0x%04x\n", + regType, regAddr, regValue); + + pu4Tmp = (u32 *)&pBtRsp->pParamStart[0]; + *pu4Tmp = regValue; + paraLen = 4; + } else if (BT_GGET_STATUS == getType) { + btOpcode = BT_LO_OP_GET_BT_STATUS; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + pBtRsp->pParamStart[0] = pExtC2h->buf[0]; + pBtRsp->pParamStart[1] = pExtC2h->buf[1]; + RTW_INFO("[MPT], read bt status, testMode=0x%x, testStatus=0x%x\n", + pBtRsp->pParamStart[0], pBtRsp->pParamStart[1]); + paraLen = 2; + } else if (BT_GGET_REPORT == getType) { + switch (reportType) { + case BT_REPORT_RX_PACKET_CNT: { + RTW_INFO("[MPT], [Rx Packet Counts]\n"); + btOpcode = BT_LO_OP_GET_RX_PKT_CNT_L; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[0] = pExtC2h->buf[0]; + pBtRsp->pParamStart[1] = pExtC2h->buf[1]; + + btOpcode = BT_LO_OP_GET_RX_PKT_CNT_H; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[2] = pExtC2h->buf[0]; + pBtRsp->pParamStart[3] = pExtC2h->buf[1]; + paraLen = 4; + } + break; + case BT_REPORT_RX_ERROR_BITS: { + RTW_INFO("[MPT], [Rx Error Bits]\n"); + btOpcode = BT_LO_OP_GET_RX_ERROR_BITS_L; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[0] = pExtC2h->buf[0]; + pBtRsp->pParamStart[1] = pExtC2h->buf[1]; + + btOpcode = BT_LO_OP_GET_RX_ERROR_BITS_H; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[2] = pExtC2h->buf[0]; + pBtRsp->pParamStart[3] = pExtC2h->buf[1]; + paraLen = 4; + } + break; + case BT_REPORT_RSSI: { + RTW_INFO("[MPT], [RSSI]\n"); + btOpcode = BT_LO_OP_GET_RSSI; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[0] = pExtC2h->buf[0]; + pBtRsp->pParamStart[1] = pExtC2h->buf[1]; + paraLen = 2; + } + break; + case BT_REPORT_CFO_HDR_QUALITY: { + RTW_INFO("[MPT], [CFO & Header Quality]\n"); + btOpcode = BT_LO_OP_GET_CFO_HDR_QUALITY_L; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[0] = pExtC2h->buf[0]; + pBtRsp->pParamStart[1] = pExtC2h->buf[1]; + + btOpcode = BT_LO_OP_GET_CFO_HDR_QUALITY_H; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + pBtRsp->pParamStart[2] = pExtC2h->buf[0]; + pBtRsp->pParamStart[3] = pExtC2h->buf[1]; + paraLen = 4; + } + break; + case BT_REPORT_CONNECT_TARGET_BD_ADDR: { + RTW_INFO("[MPT], [Connected Target BD ADDR]\n"); + btOpcode = BT_LO_OP_GET_TARGET_BD_ADDR_L; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + bdAddr[5] = pExtC2h->buf[0]; + bdAddr[4] = pExtC2h->buf[1]; + bdAddr[3] = pExtC2h->buf[2]; + + btOpcode = BT_LO_OP_GET_TARGET_BD_ADDR_H; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + bdAddr[2] = pExtC2h->buf[0]; + bdAddr[1] = pExtC2h->buf[1]; + bdAddr[0] = pExtC2h->buf[2]; + + RTW_INFO("[MPT], Connected Target BDAddr:%s", bdAddr); + for (i = 0; i < 6; i++) + pBtRsp->pParamStart[i] = bdAddr[5 - i]; + paraLen = 6; + } + break; + default: + pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + break; + } + } + + pBtRsp->status = BT_STATUS_SUCCESS; + return paraLen; +} + + + +u16 +mptbt_BtSetGeneral( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode; + u8 btOpcodeVer = 0; + u8 setType = 0; + u16 setParaLen = 0, validParaLen = 0; + u8 regType = 0, bdAddr[6] = {0}, calVal = 0; + u32 regAddr = 0, regValue = 0; + u32 *pu4Tmp; + u16 *pu2Tmp; + u8 *pu1Tmp; + + /* */ + /* check upper layer parameters */ + /* */ + + /* check upper layer opcode version */ + if (pBtReq->opCodeVer != 1) { + RTW_INFO("[MPT], Error!! Upper OP code version not match!!!\n"); + pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH; + return paraLen; + } + /* check upper layer parameter length */ + if (pBtReq->paraLength < 1) { + RTW_INFO("[MPT], Error!! wrong parameter length=%d (should larger than 1)\n", pBtReq->paraLength); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + setParaLen = pBtReq->paraLength - 1; + setType = pBtReq->pParamStart[0]; + + RTW_INFO("[MPT], setType=%d, setParaLen=%d\n", setType, setParaLen); + + /* check parameter first */ + switch (setType) { + case BT_GSET_REG: + RTW_INFO("[MPT], [BT_GSET_REG]\n"); + validParaLen = 9; + if (setParaLen == validParaLen) { + btOpcode = BT_LO_OP_WRITE_REG_VALUE; + regType = pBtReq->pParamStart[1]; + pu4Tmp = (u32 *)&pBtReq->pParamStart[2]; + regAddr = *pu4Tmp; + pu4Tmp = (u32 *)&pBtReq->pParamStart[6]; + regValue = *pu4Tmp; + RTW_INFO("[MPT], BT_GSET_REG regType=0x%x, regAddr=0x%x, regValue=0x%x!!\n", + regType, regAddr, regValue); + if (regType >= BT_REG_MAX) { + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + if (((BT_REG_RF == regType) && (regAddr > 0x7f)) || + ((BT_REG_MODEM == regType) && (regAddr > 0x1ff)) || + ((BT_REG_BLUEWIZE == regType) && (regAddr > 0xfff)) || + ((BT_REG_VENDOR == regType) && (regAddr > 0xfff)) || + ((BT_REG_LE == regType) && (regAddr > 0xfff))) { + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + } + } + break; + case BT_GSET_RESET: + RTW_INFO("[MPT], [BT_GSET_RESET]\n"); + validParaLen = 0; + break; + case BT_GSET_TARGET_BD_ADDR: + RTW_INFO("[MPT], [BT_GSET_TARGET_BD_ADDR]\n"); + validParaLen = 6; + if (setParaLen == validParaLen) { + btOpcode = BT_LO_OP_SET_TARGET_BD_ADDR_H; + if ((pBtReq->pParamStart[1] == 0) && + (pBtReq->pParamStart[2] == 0) && + (pBtReq->pParamStart[3] == 0) && + (pBtReq->pParamStart[4] == 0) && + (pBtReq->pParamStart[5] == 0) && + (pBtReq->pParamStart[6] == 0)) { + RTW_INFO("[MPT], Error!! targetBDAddr=all zero\n"); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + if ((pBtReq->pParamStart[1] == 0xff) && + (pBtReq->pParamStart[2] == 0xff) && + (pBtReq->pParamStart[3] == 0xff) && + (pBtReq->pParamStart[4] == 0xff) && + (pBtReq->pParamStart[5] == 0xff) && + (pBtReq->pParamStart[6] == 0xff)) { + RTW_INFO("[MPT], Error!! targetBDAddr=all 0xf\n"); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + bdAddr[0] = pBtReq->pParamStart[6]; + bdAddr[1] = pBtReq->pParamStart[5]; + bdAddr[2] = pBtReq->pParamStart[4]; + bdAddr[3] = pBtReq->pParamStart[3]; + bdAddr[4] = pBtReq->pParamStart[2]; + bdAddr[5] = pBtReq->pParamStart[1]; + RTW_INFO("[MPT], target BDAddr:%x,%x,%x,%x,%x,%x\n", + bdAddr[0], bdAddr[1], bdAddr[2], bdAddr[3], bdAddr[4], bdAddr[5]); + } + break; + case BT_GSET_TX_PWR_FINETUNE: + RTW_INFO("[MPT], [BT_GSET_TX_PWR_FINETUNE]\n"); + validParaLen = 1; + if (setParaLen == validParaLen) { + btOpcode = BT_LO_OP_SET_TX_POWER_CALIBRATION; + calVal = pBtReq->pParamStart[1]; + if ((calVal < 1) || (calVal > 9)) { + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + RTW_INFO("[MPT], calVal=%d\n", calVal); + } + break; + case BT_SET_TRACKING_INTERVAL: + RTW_INFO("[MPT], [BT_SET_TRACKING_INTERVAL] setParaLen =%d\n", setParaLen); + + validParaLen = 1; + if (setParaLen == validParaLen) + calVal = pBtReq->pParamStart[1]; + break; + case BT_SET_THERMAL_METER: + RTW_INFO("[MPT], [BT_SET_THERMAL_METER] setParaLen =%d\n", setParaLen); + validParaLen = 1; + if (setParaLen == validParaLen) + calVal = pBtReq->pParamStart[1]; + break; + case BT_ENABLE_CFO_TRACKING: + RTW_INFO("[MPT], [BT_ENABLE_CFO_TRACKING] setParaLen =%d\n", setParaLen); + validParaLen = 1; + if (setParaLen == validParaLen) + calVal = pBtReq->pParamStart[1]; + break; + case BT_GSET_UPDATE_BT_PATCH: + + break; + default: { + RTW_INFO("[MPT], Error!! setType=%d, out of range\n", setType); + pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + break; + } + if (setParaLen != validParaLen) { + RTW_INFO("[MPT], Error!! wrong parameter length=%d for BT_SET_GEN_CMD cmd id=0x%x, paraLen should=0x%x\n", + setParaLen, setType, validParaLen); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + + /* */ + /* execute lower layer opcodes */ + /* */ + if (BT_GSET_REG == setType) { + /* fill h2c parameters */ + /* here we should write reg value first then write the address, adviced by Austin */ + btOpcode = BT_LO_OP_WRITE_REG_VALUE; + h2cParaBuf[0] = pBtReq->pParamStart[6]; + h2cParaBuf[1] = pBtReq->pParamStart[7]; + h2cParaBuf[2] = pBtReq->pParamStart[8]; + h2cParaLen = 3; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + /* write reg address */ + btOpcode = BT_LO_OP_WRITE_REG_ADDR; + h2cParaBuf[0] = regType; + h2cParaBuf[1] = pBtReq->pParamStart[2]; + h2cParaBuf[2] = pBtReq->pParamStart[3]; + h2cParaLen = 3; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } else if (BT_GSET_RESET == setType) { + btOpcode = BT_LO_OP_RESET; + h2cParaLen = 0; + /* execute h2c and check respond c2h from bt fw is correct or not */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } else if (BT_GSET_TARGET_BD_ADDR == setType) { + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_TARGET_BD_ADDR_L; + h2cParaBuf[0] = pBtReq->pParamStart[1]; + h2cParaBuf[1] = pBtReq->pParamStart[2]; + h2cParaBuf[2] = pBtReq->pParamStart[3]; + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + btOpcode = BT_LO_OP_SET_TARGET_BD_ADDR_H; + h2cParaBuf[0] = pBtReq->pParamStart[4]; + h2cParaBuf[1] = pBtReq->pParamStart[5]; + h2cParaBuf[2] = pBtReq->pParamStart[6]; + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } else if (BT_GSET_TX_PWR_FINETUNE == setType) { + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_TX_POWER_CALIBRATION; + h2cParaBuf[0] = calVal; + h2cParaLen = 1; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } else if (BT_SET_TRACKING_INTERVAL == setType) { + /* BT_LO_OP_SET_TRACKING_INTERVAL = 0x22, */ + /* BT_LO_OP_SET_THERMAL_METER = 0x23, */ + /* BT_LO_OP_ENABLE_CFO_TRACKING = 0x24, */ + btOpcode = BT_LO_OP_SET_TRACKING_INTERVAL; + h2cParaBuf[0] = calVal; + h2cParaLen = 1; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } else if (BT_SET_THERMAL_METER == setType) { + btOpcode = BT_LO_OP_SET_THERMAL_METER; + h2cParaBuf[0] = calVal; + h2cParaLen = 1; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } else if (BT_ENABLE_CFO_TRACKING == setType) { + btOpcode = BT_LO_OP_ENABLE_CFO_TRACKING; + h2cParaBuf[0] = calVal; + h2cParaLen = 1; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + } + + pBtRsp->status = BT_STATUS_SUCCESS; + return paraLen; +} + + + +u16 +mptbt_BtSetTxRxPars( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode; + u8 btOpcodeVer = 0; + PBT_TXRX_PARAMETERS pTxRxPars = (PBT_TXRX_PARAMETERS)&pBtReq->pParamStart[0]; + u16 lenTxRx = sizeof(BT_TXRX_PARAMETERS); + u8 i; + u8 bdAddr[6] = {0}; + + /* */ + /* check upper layer parameters */ + /* */ + + /* 1. check upper layer opcode version */ + if (pBtReq->opCodeVer != 1) { + RTW_INFO("[MPT], Error!! Upper OP code version not match!!!\n"); + pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH; + return paraLen; + } + /* 2. check upper layer parameter length */ + if (pBtReq->paraLength == sizeof(BT_TXRX_PARAMETERS)) { + RTW_INFO("[MPT], pTxRxPars->txrxChannel=0x%x\n", pTxRxPars->txrxChannel); + RTW_INFO("[MPT], pTxRxPars->txrxTxPktCnt=0x%8x\n", pTxRxPars->txrxTxPktCnt); + RTW_INFO("[MPT], pTxRxPars->txrxTxPktInterval=0x%x\n", pTxRxPars->txrxTxPktInterval); + RTW_INFO("[MPT], pTxRxPars->txrxPayloadType=0x%x\n", pTxRxPars->txrxPayloadType); + RTW_INFO("[MPT], pTxRxPars->txrxPktType=0x%x\n", pTxRxPars->txrxPktType); + RTW_INFO("[MPT], pTxRxPars->txrxPayloadLen=0x%x\n", pTxRxPars->txrxPayloadLen); + RTW_INFO("[MPT], pTxRxPars->txrxPktHeader=0x%x\n", pTxRxPars->txrxPktHeader); + RTW_INFO("[MPT], pTxRxPars->txrxWhitenCoeff=0x%x\n", pTxRxPars->txrxWhitenCoeff); + bdAddr[0] = pTxRxPars->txrxBdaddr[5]; + bdAddr[1] = pTxRxPars->txrxBdaddr[4]; + bdAddr[2] = pTxRxPars->txrxBdaddr[3]; + bdAddr[3] = pTxRxPars->txrxBdaddr[2]; + bdAddr[4] = pTxRxPars->txrxBdaddr[1]; + bdAddr[5] = pTxRxPars->txrxBdaddr[0]; + RTW_INFO("[MPT], pTxRxPars->txrxBdaddr: %s", &bdAddr[0]); + RTW_INFO("[MPT], pTxRxPars->txrxTxGainIndex=0x%x\n", pTxRxPars->txrxTxGainIndex); + } else { + RTW_INFO("[MPT], Error!! pBtReq->paraLength=%d, correct Len=%d\n", pBtReq->paraLength, lenTxRx); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + + /* */ + /* execute lower layer opcodes */ + /* */ + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_PKT_HEADER; + if (pTxRxPars->txrxPktHeader > 0x3ffff) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxPktHeader=0x%x is out of range, (should be between 0x0~0x3ffff)\n", pTxRxPars->txrxPktHeader); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + h2cParaBuf[0] = (u8)(pTxRxPars->txrxPktHeader & 0xff); + h2cParaBuf[1] = (u8)((pTxRxPars->txrxPktHeader & 0xff00) >> 8); + h2cParaBuf[2] = (u8)((pTxRxPars->txrxPktHeader & 0xff0000) >> 16); + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_PKT_TYPE_LEN; + { + u16 payloadLenLimit = 0; + switch (pTxRxPars->txrxPktType) { + case MP_BT_PKT_DH1: + payloadLenLimit = 27 * 8; + break; + case MP_BT_PKT_DH3: + payloadLenLimit = 183 * 8; + break; + case MP_BT_PKT_DH5: + payloadLenLimit = 339 * 8; + break; + case MP_BT_PKT_2DH1: + payloadLenLimit = 54 * 8; + break; + case MP_BT_PKT_2DH3: + payloadLenLimit = 367 * 8; + break; + case MP_BT_PKT_2DH5: + payloadLenLimit = 679 * 8; + break; + case MP_BT_PKT_3DH1: + payloadLenLimit = 83 * 8; + break; + case MP_BT_PKT_3DH3: + payloadLenLimit = 552 * 8; + break; + case MP_BT_PKT_3DH5: + payloadLenLimit = 1021 * 8; + break; + case MP_BT_PKT_LE: + payloadLenLimit = 39 * 8; + break; + default: { + RTW_INFO("[MPT], Error!! Unknown pTxRxPars->txrxPktType=0x%x\n", pTxRxPars->txrxPktType); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + break; + } + + if (pTxRxPars->txrxPayloadLen > payloadLenLimit) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxPayloadLen=0x%x, (should smaller than %d)\n", + pTxRxPars->txrxPayloadLen, payloadLenLimit); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + + h2cParaBuf[0] = pTxRxPars->txrxPktType; + h2cParaBuf[1] = (u8)((pTxRxPars->txrxPayloadLen & 0xff)); + h2cParaBuf[2] = (u8)((pTxRxPars->txrxPayloadLen & 0xff00) >> 8); + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_PKT_CNT_L_PL_TYPE; + if (pTxRxPars->txrxPayloadType > MP_BT_PAYLOAD_MAX) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxPayloadType=0x%x, (should be between 0~4)\n", pTxRxPars->txrxPayloadType); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + h2cParaBuf[0] = (u8)((pTxRxPars->txrxTxPktCnt & 0xff)); + h2cParaBuf[1] = (u8)((pTxRxPars->txrxTxPktCnt & 0xff00) >> 8); + h2cParaBuf[2] = pTxRxPars->txrxPayloadType; + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_PKT_CNT_H_PKT_INTV; + if (pTxRxPars->txrxTxPktInterval > 15) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxTxPktInterval=0x%x, (should be between 0~15)\n", pTxRxPars->txrxTxPktInterval); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + h2cParaBuf[0] = (u8)((pTxRxPars->txrxTxPktCnt & 0xff0000) >> 16); + h2cParaBuf[1] = (u8)((pTxRxPars->txrxTxPktCnt & 0xff000000) >> 24); + h2cParaBuf[2] = pTxRxPars->txrxTxPktInterval; + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_WHITENCOEFF; + { + h2cParaBuf[0] = pTxRxPars->txrxWhitenCoeff; + h2cParaLen = 1; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_CHNL_TX_GAIN; + if ((pTxRxPars->txrxChannel > 78) || + (pTxRxPars->txrxTxGainIndex > 7)) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxChannel=0x%x, (should be between 0~78)\n", pTxRxPars->txrxChannel); + RTW_INFO("[MPT], Error!! pTxRxPars->txrxTxGainIndex=0x%x, (should be between 0~7)\n", pTxRxPars->txrxTxGainIndex); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + h2cParaBuf[0] = pTxRxPars->txrxChannel; + h2cParaBuf[1] = pTxRxPars->txrxTxGainIndex; + h2cParaLen = 2; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + /* fill h2c parameters */ + btOpcode = BT_LO_OP_SET_BD_ADDR_L; + if ((pTxRxPars->txrxBdaddr[0] == 0) && + (pTxRxPars->txrxBdaddr[1] == 0) && + (pTxRxPars->txrxBdaddr[2] == 0) && + (pTxRxPars->txrxBdaddr[3] == 0) && + (pTxRxPars->txrxBdaddr[4] == 0) && + (pTxRxPars->txrxBdaddr[5] == 0)) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxBdaddr=all zero\n"); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + if ((pTxRxPars->txrxBdaddr[0] == 0xff) && + (pTxRxPars->txrxBdaddr[1] == 0xff) && + (pTxRxPars->txrxBdaddr[2] == 0xff) && + (pTxRxPars->txrxBdaddr[3] == 0xff) && + (pTxRxPars->txrxBdaddr[4] == 0xff) && + (pTxRxPars->txrxBdaddr[5] == 0xff)) { + RTW_INFO("[MPT], Error!! pTxRxPars->txrxBdaddr=all 0xf\n"); + pBtRsp->status = (btOpcode << 8) | BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } + + { + h2cParaBuf[0] = pTxRxPars->txrxBdaddr[0]; + h2cParaBuf[1] = pTxRxPars->txrxBdaddr[1]; + h2cParaBuf[2] = pTxRxPars->txrxBdaddr[2]; + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + btOpcode = BT_LO_OP_SET_BD_ADDR_H; + { + h2cParaBuf[0] = pTxRxPars->txrxBdaddr[3]; + h2cParaBuf[1] = pTxRxPars->txrxBdaddr[4]; + h2cParaBuf[2] = pTxRxPars->txrxBdaddr[5]; + h2cParaLen = 3; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + /* ckeck bt return status. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + pBtRsp->status = BT_STATUS_SUCCESS; + return paraLen; +} + + + +u16 +mptbt_BtTestCtrl( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode; + u8 btOpcodeVer = 0; + u8 testCtrl = 0; + + /* */ + /* check upper layer parameters */ + /* */ + + /* 1. check upper layer opcode version */ + if (pBtReq->opCodeVer != 1) { + RTW_INFO("[MPT], Error!! Upper OP code version not match!!!\n"); + pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH; + return paraLen; + } + /* 2. check upper layer parameter length */ + if (1 == pBtReq->paraLength) { + testCtrl = pBtReq->pParamStart[0]; + RTW_INFO("[MPT], testCtrl=%d\n", testCtrl); + } else { + RTW_INFO("[MPT], Error!! wrong parameter length=%d (should be 1)\n", pBtReq->paraLength); + pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U; + return paraLen; + } + + /* */ + /* execute lower layer opcodes */ + /* */ + + /* 1. fill h2c parameters */ + /* check bt mode */ + btOpcode = BT_LO_OP_TEST_CTRL; + if (testCtrl >= MP_BT_TEST_MAX) { + RTW_INFO("[MPT], Error!! testCtrl=0x%x, (should be between smaller or equal to 0x%x)\n", + testCtrl, MP_BT_TEST_MAX - 1); + pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U; + return paraLen; + } else { + h2cParaBuf[0] = testCtrl; + h2cParaLen = 1; + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); + } + + /* 3. construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + pBtRsp->status = BT_STATUS_SUCCESS; + return paraLen; +} + + +u16 +mptbt_TestBT( + PADAPTER Adapter, + PBT_REQ_CMD pBtReq, + PBT_RSP_CMD pBtRsp +) +{ + + u8 h2cParaBuf[6] = {0}; + u8 h2cParaLen = 0; + u16 paraLen = 0; + u8 retStatus = BT_STATUS_BT_OP_SUCCESS; + u8 btOpcode; + u8 btOpcodeVer = 0; + u8 testCtrl = 0; + + /* 1. fill h2c parameters */ + btOpcode = 0x11; + h2cParaBuf[0] = 0x11; + h2cParaBuf[1] = 0x0; + h2cParaBuf[2] = 0x0; + h2cParaBuf[3] = 0x0; + h2cParaBuf[4] = 0x0; + h2cParaLen = 1; + /* retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen); */ + retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, h2cParaBuf, h2cParaLen); + + + /* 3. construct respond status code and data. */ + if (BT_STATUS_BT_OP_SUCCESS != retStatus) { + pBtRsp->status = ((btOpcode << 8) | retStatus); + RTW_INFO("[MPT], Error!! status code=0x%x\n", pBtRsp->status); + return paraLen; + } + + pBtRsp->status = BT_STATUS_SUCCESS; + return paraLen; +} + +void +mptbt_BtControlProcess( + PADAPTER Adapter, + void *pInBuf +) +{ + u8 H2C_Parameter[6] = {0}; + PBT_H2C pH2c = (PBT_H2C)&H2C_Parameter[0]; + PMPT_CONTEXT pMptCtx = &(Adapter->mppriv.mpt_ctx); + PBT_REQ_CMD pBtReq = (PBT_REQ_CMD)pInBuf; + PBT_RSP_CMD pBtRsp; + u8 i; + + + RTW_INFO("[MPT], mptbt_BtControlProcess()=========>\n"); + + RTW_INFO("[MPT], input opCodeVer=%d\n", pBtReq->opCodeVer); + RTW_INFO("[MPT], input OpCode=%d\n", pBtReq->OpCode); + RTW_INFO("[MPT], paraLength=%d\n", pBtReq->paraLength); + if (pBtReq->paraLength) { + /* RTW_INFO("[MPT], parameters(hex):0x%x %d\n",&pBtReq->pParamStart[0], pBtReq->paraLength); */ + } + + _rtw_memset((void *)pMptCtx->mptOutBuf, 0, 100); + pMptCtx->mptOutLen = 4; /* length of (BT_RSP_CMD.status+BT_RSP_CMD.paraLength) */ + + pBtRsp = (PBT_RSP_CMD)pMptCtx->mptOutBuf; + pBtRsp->status = BT_STATUS_SUCCESS; + pBtRsp->paraLength = 0x0; + + /* The following we should maintain the User OP codes sent by upper layer */ + switch (pBtReq->OpCode) { + case BT_UP_OP_BT_READY: + RTW_INFO("[MPT], OPcode : [BT_READY]\n"); + pBtRsp->paraLength = mptbt_BtReady(Adapter, pBtReq, pBtRsp); + break; + case BT_UP_OP_BT_SET_MODE: + RTW_INFO("[MPT], OPcode : [BT_SET_MODE]\n"); + pBtRsp->paraLength = mptbt_BtSetMode(Adapter, pBtReq, pBtRsp); + break; + case BT_UP_OP_BT_SET_TX_RX_PARAMETER: + RTW_INFO("[MPT], OPcode : [BT_SET_TXRX_PARAMETER]\n"); + pBtRsp->paraLength = mptbt_BtSetTxRxPars(Adapter, pBtReq, pBtRsp); + break; + case BT_UP_OP_BT_SET_GENERAL: + RTW_INFO("[MPT], OPcode : [BT_SET_GENERAL]\n"); + pBtRsp->paraLength = mptbt_BtSetGeneral(Adapter, pBtReq, pBtRsp); + break; + case BT_UP_OP_BT_GET_GENERAL: + RTW_INFO("[MPT], OPcode : [BT_GET_GENERAL]\n"); + pBtRsp->paraLength = mptbt_BtGetGeneral(Adapter, pBtReq, pBtRsp); + break; + case BT_UP_OP_BT_TEST_CTRL: + RTW_INFO("[MPT], OPcode : [BT_TEST_CTRL]\n"); + pBtRsp->paraLength = mptbt_BtTestCtrl(Adapter, pBtReq, pBtRsp); + break; + case BT_UP_OP_TEST_BT: + RTW_INFO("[MPT], OPcode : [TEST_BT]\n"); + pBtRsp->paraLength = mptbt_TestBT(Adapter, pBtReq, pBtRsp); + break; + default: + RTW_INFO("[MPT], Error!! OPcode : UNDEFINED!!!!\n"); + pBtRsp->status = BT_STATUS_UNKNOWN_OPCODE_U; + pBtRsp->paraLength = 0x0; + break; + } + + pMptCtx->mptOutLen += pBtRsp->paraLength; + + RTW_INFO("[MPT], pMptCtx->mptOutLen=%d, pBtRsp->paraLength=%d\n", pMptCtx->mptOutLen, pBtRsp->paraLength); + RTW_INFO("[MPT], mptbt_BtControlProcess()<=========\n"); +} + +#endif diff --git a/core/rtw_btcoex.c b/core/rtw_btcoex.c new file mode 100644 index 0000000..5081cdd --- /dev/null +++ b/core/rtw_btcoex.c @@ -0,0 +1,1817 @@ +/****************************************************************************** + * + * Copyright(c) 2013 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#include +#include +#ifdef CONFIG_BT_COEXIST +#include + +void rtw_btcoex_Initialize(PADAPTER padapter) +{ + hal_btcoex_Initialize(padapter); +} + +void rtw_btcoex_PowerOnSetting(PADAPTER padapter) +{ + hal_btcoex_PowerOnSetting(padapter); +} + +void rtw_btcoex_AntInfoSetting(PADAPTER padapter) +{ + hal_btcoex_AntInfoSetting(padapter); +} + +void rtw_btcoex_PowerOffSetting(PADAPTER padapter) +{ + hal_btcoex_PowerOffSetting(padapter); +} + +void rtw_btcoex_PreLoadFirmware(PADAPTER padapter) +{ + hal_btcoex_PreLoadFirmware(padapter); +} + +void rtw_btcoex_HAL_Initialize(PADAPTER padapter, u8 bWifiOnly) +{ + hal_btcoex_InitHwConfig(padapter, bWifiOnly); +} + +void rtw_btcoex_IpsNotify(PADAPTER padapter, u8 type) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_IpsNotify(padapter, type); +} + +void rtw_btcoex_LpsNotify(PADAPTER padapter, u8 type) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_LpsNotify(padapter, type); +} + +void rtw_btcoex_ScanNotify(PADAPTER padapter, u8 type) +{ + PHAL_DATA_TYPE pHalData; +#ifdef CONFIG_BT_COEXIST_SOCKET_TRX + struct bt_coex_info *pcoex_info = &padapter->coex_info; + PBT_MGNT pBtMgnt = &pcoex_info->BtMgnt; +#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */ + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + if (_FALSE == type) { + #ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(padapter, WIFI_UNDER_SURVEY)) + return; + #endif + + if (DEV_MGMT_TX_NUM(adapter_to_dvobj(padapter)) + || DEV_ROCH_NUM(adapter_to_dvobj(padapter))) + return; + } + +#ifdef CONFIG_BT_COEXIST_SOCKET_TRX + if (pBtMgnt->ExtConfig.bEnableWifiScanNotify) + rtw_btcoex_SendScanNotify(padapter, type); +#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */ + + hal_btcoex_ScanNotify(padapter, type); +} + +static void _rtw_btcoex_connect_notify(PADAPTER padapter, u8 action) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + +#ifdef DBG_CONFIG_ERROR_RESET + if (_TRUE == rtw_hal_sreset_inprogress(padapter)) { + RTW_INFO(FUNC_ADPT_FMT ": [BTCoex] under reset, skip notify!\n", + FUNC_ADPT_ARG(padapter)); + return; + } +#endif /* DBG_CONFIG_ERROR_RESET */ + +#ifdef CONFIG_CONCURRENT_MODE + if (_FALSE == action) { + if (rtw_mi_buddy_check_fwstate(padapter, WIFI_UNDER_LINKING)) + return; + } +#endif + + hal_btcoex_ConnectNotify(padapter, action); +} + +void rtw_btcoex_MediaStatusNotify(PADAPTER padapter, u8 mediaStatus) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + +#ifdef DBG_CONFIG_ERROR_RESET + if (_TRUE == rtw_hal_sreset_inprogress(padapter)) { + RTW_INFO(FUNC_ADPT_FMT ": [BTCoex] under reset, skip notify!\n", + FUNC_ADPT_ARG(padapter)); + return; + } +#endif /* DBG_CONFIG_ERROR_RESET */ + +#ifdef CONFIG_CONCURRENT_MODE + if (RT_MEDIA_DISCONNECT == mediaStatus) { + if (rtw_mi_buddy_check_fwstate(padapter, WIFI_ASOC_STATE)) + return; + } +#endif /* CONFIG_CONCURRENT_MODE */ + + if ((RT_MEDIA_CONNECT == mediaStatus) + && (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == _TRUE)) + rtw_hal_set_hwreg(padapter, HW_VAR_DL_RSVD_PAGE, NULL); + + hal_btcoex_MediaStatusNotify(padapter, mediaStatus); +} + +void rtw_btcoex_SpecialPacketNotify(PADAPTER padapter, u8 pktType) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_SpecialPacketNotify(padapter, pktType); +} + +void rtw_btcoex_IQKNotify(PADAPTER padapter, u8 state) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_IQKNotify(padapter, state); +} + +void rtw_btcoex_WLRFKNotify(PADAPTER padapter, u8 path, u8 type, u8 state) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_WLRFKNotify(padapter, path, type, state); +} + +void rtw_btcoex_BtInfoNotify(PADAPTER padapter, u8 length, u8 *tmpBuf) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_BtInfoNotify(padapter, length, tmpBuf); +} + +void rtw_btcoex_BtMpRptNotify(PADAPTER padapter, u8 length, u8 *tmpBuf) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + if (padapter->registrypriv.mp_mode == 1) + return; + + hal_btcoex_BtMpRptNotify(padapter, length, tmpBuf); +} + +void rtw_btcoex_SuspendNotify(PADAPTER padapter, u8 state) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_SuspendNotify(padapter, state); +} + +void rtw_btcoex_HaltNotify(PADAPTER padapter) +{ + PHAL_DATA_TYPE pHalData; + u8 do_halt = 1; + + pHalData = GET_HAL_DATA(padapter); + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + do_halt = 0; + + if (_FALSE == padapter->bup) { + RTW_INFO(FUNC_ADPT_FMT ": bup=%d Skip!\n", + FUNC_ADPT_ARG(padapter), padapter->bup); + do_halt = 0; + } + + if (rtw_is_surprise_removed(padapter)) { + RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=%s Skip!\n", + FUNC_ADPT_ARG(padapter), rtw_is_surprise_removed(padapter) ? "True" : "False"); + do_halt = 0; + } + + hal_btcoex_HaltNotify(padapter, do_halt); +} + +void rtw_btcoex_switchband_notify(u8 under_scan, u8 band_type) +{ + hal_btcoex_switchband_notify(under_scan, band_type); +} + +void rtw_btcoex_WlFwDbgInfoNotify(PADAPTER padapter, u8* tmpBuf, u8 length) +{ + hal_btcoex_WlFwDbgInfoNotify(padapter, tmpBuf, length); +} + +void rtw_btcoex_rx_rate_change_notify(PADAPTER padapter, u8 is_data_frame, u8 rate_id) +{ + hal_btcoex_rx_rate_change_notify(padapter, is_data_frame, rate_id); +} + +void rtw_btcoex_SwitchBtTRxMask(PADAPTER padapter) +{ + hal_btcoex_SwitchBtTRxMask(padapter); +} + +void rtw_btcoex_Switch(PADAPTER padapter, u8 enable) +{ + hal_btcoex_SetBTCoexist(padapter, enable); +} + +u8 rtw_btcoex_IsBtDisabled(PADAPTER padapter) +{ + return hal_btcoex_IsBtDisabled(padapter); +} + +void rtw_btcoex_Handler(PADAPTER padapter) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + + if (_FALSE == pHalData->EEPROMBluetoothCoexist) + return; + + hal_btcoex_Hanlder(padapter); +} + +s32 rtw_btcoex_IsBTCoexRejectAMPDU(PADAPTER padapter) +{ + s32 coexctrl; + + coexctrl = hal_btcoex_IsBTCoexRejectAMPDU(padapter); + + return coexctrl; +} + +s32 rtw_btcoex_IsBTCoexCtrlAMPDUSize(PADAPTER padapter) +{ + s32 coexctrl; + + coexctrl = hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter); + + return coexctrl; +} + +u32 rtw_btcoex_GetAMPDUSize(PADAPTER padapter) +{ + u32 size; + + size = hal_btcoex_GetAMPDUSize(padapter); + + return size; +} + +void rtw_btcoex_SetManualControl(PADAPTER padapter, u8 manual) +{ + if (_TRUE == manual) + hal_btcoex_SetManualControl(padapter, _TRUE); + else + hal_btcoex_SetManualControl(padapter, _FALSE); +} + +void rtw_btcoex_set_policy_control(PADAPTER padapter, u8 btc_policy) +{ + hal_btcoex_set_policy_control(padapter, btc_policy); +} + +u8 rtw_btcoex_1Ant(PADAPTER padapter) +{ + return hal_btcoex_1Ant(padapter); +} + +u8 rtw_btcoex_IsBtControlLps(PADAPTER padapter) +{ + return hal_btcoex_IsBtControlLps(padapter); +} + +u8 rtw_btcoex_IsLpsOn(PADAPTER padapter) +{ + return hal_btcoex_IsLpsOn(padapter); +} + +u8 rtw_btcoex_RpwmVal(PADAPTER padapter) +{ + return hal_btcoex_RpwmVal(padapter); +} + +u8 rtw_btcoex_LpsVal(PADAPTER padapter) +{ + return hal_btcoex_LpsVal(padapter); +} + +u32 rtw_btcoex_GetRaMask(PADAPTER padapter) +{ + return hal_btcoex_GetRaMask(padapter); +} + +u8 rtw_btcoex_query_reduced_wl_pwr_lvl(PADAPTER padapter) +{ + return hal_btcoex_query_reduced_wl_pwr_lvl(padapter); +} + +void rtw_btcoex_set_reduced_wl_pwr_lvl(PADAPTER padapter, u8 val) +{ + hal_btcoex_set_reduced_wl_pwr_lvl(padapter, val); +} + +void rtw_btcoex_do_reduce_wl_pwr_lvl(PADAPTER padapter) +{ + hal_btcoex_do_reduce_wl_pwr_lvl(padapter); +} + +void rtw_btcoex_RecordPwrMode(PADAPTER padapter, u8 *pCmdBuf, u8 cmdLen) +{ + hal_btcoex_RecordPwrMode(padapter, pCmdBuf, cmdLen); +} + +void rtw_btcoex_DisplayBtCoexInfo(PADAPTER padapter, u8 *pbuf, u32 bufsize) +{ + hal_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize); +} + +void rtw_btcoex_SetDBG(PADAPTER padapter, u32 *pDbgModule) +{ + hal_btcoex_SetDBG(padapter, pDbgModule); +} + +u32 rtw_btcoex_GetDBG(PADAPTER padapter, u8 *pStrBuf, u32 bufSize) +{ + return hal_btcoex_GetDBG(padapter, pStrBuf, bufSize); +} + +u8 rtw_btcoex_IncreaseScanDeviceNum(PADAPTER padapter) +{ + return hal_btcoex_IncreaseScanDeviceNum(padapter); +} + +u8 rtw_btcoex_IsBtLinkExist(PADAPTER padapter) +{ + return hal_btcoex_IsBtLinkExist(padapter); +} + +void rtw_btcoex_SetBtPatchVersion(PADAPTER padapter, u16 btHciVer, u16 btPatchVer) +{ + hal_btcoex_SetBtPatchVersion(padapter, btHciVer, btPatchVer); +} + +void rtw_btcoex_SetHciVersion(PADAPTER padapter, u16 hciVersion) +{ + hal_btcoex_SetHciVersion(padapter, hciVersion); +} + +void rtw_btcoex_StackUpdateProfileInfo(void) +{ + hal_btcoex_StackUpdateProfileInfo(); +} + +void rtw_btcoex_pta_off_on_notify(PADAPTER padapter, u8 bBTON) +{ + hal_btcoex_pta_off_on_notify(padapter, bBTON); +} + +#ifdef CONFIG_RF4CE_COEXIST +void rtw_btcoex_SetRf4ceLinkState(PADAPTER padapter, u8 state) +{ + hal_btcoex_set_rf4ce_link_state(state); +} + +u8 rtw_btcoex_GetRf4ceLinkState(PADAPTER padapter) +{ + return hal_btcoex_get_rf4ce_link_state(); +} +#endif + +/* ================================================== + * Below Functions are called by BT-Coex + * ================================================== */ +void rtw_btcoex_rx_ampdu_apply(PADAPTER padapter) +{ + rtw_rx_ampdu_apply(padapter); +} + +void rtw_btcoex_LPS_Enter(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrpriv; + u8 lpsVal; + + + pwrpriv = adapter_to_pwrctl(padapter); + + pwrpriv->bpower_saving = _TRUE; + lpsVal = rtw_btcoex_LpsVal(padapter); + rtw_set_ps_mode(padapter, PS_MODE_MIN, 0, lpsVal, "BTCOEX"); +} + +u8 rtw_btcoex_LPS_Leave(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrpriv; + + + pwrpriv = adapter_to_pwrctl(padapter); + + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) { + rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "BTCOEX"); + pwrpriv->bpower_saving = _FALSE; + } + + return _TRUE; +} + +u16 rtw_btcoex_btreg_read(PADAPTER padapter, u8 type, u16 addr, u32 *data) +{ + return hal_btcoex_btreg_read(padapter, type, addr, data); +} + +u16 rtw_btcoex_btreg_write(PADAPTER padapter, u8 type, u16 addr, u16 val) +{ + return hal_btcoex_btreg_write(padapter, type, addr, val); +} + +u16 rtw_btcoex_btset_testmode(PADAPTER padapter, u8 type) +{ + return hal_btcoex_btset_testode(padapter, type); +} + +u8 rtw_btcoex_get_reduce_wl_txpwr(PADAPTER padapter) +{ + return rtw_btcoex_query_reduced_wl_pwr_lvl(padapter); +} + +u8 rtw_btcoex_get_bt_coexist(PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + return pHalData->EEPROMBluetoothCoexist; +} + +u8 rtw_btcoex_get_chip_type(PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + return pHalData->EEPROMBluetoothType; +} + +u8 rtw_btcoex_get_pg_ant_num(PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + return pHalData->EEPROMBluetoothAntNum == Ant_x2 ? 2 : 1; +} + +u8 rtw_btcoex_get_pg_single_ant_path(PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + return pHalData->ant_path; +} + +u8 rtw_btcoex_get_pg_rfe_type(PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + return pHalData->rfe_type; +} + +u8 rtw_btcoex_is_tfbga_package_type(PADAPTER padapter) +{ +#ifdef CONFIG_RTL8723B + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + if ((pHalData->PackageType == PACKAGE_TFBGA79) || (pHalData->PackageType == PACKAGE_TFBGA80) + || (pHalData->PackageType == PACKAGE_TFBGA90)) + return _TRUE; +#endif + + return _FALSE; +} + +u8 rtw_btcoex_get_ant_div_cfg(PADAPTER padapter) +{ + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + + return (pHalData->AntDivCfg == 0) ? _FALSE : _TRUE; +} + +/* ================================================== + * Below Functions are BT-Coex socket related function + * ================================================== */ + +#ifdef CONFIG_BT_COEXIST_SOCKET_TRX +_adapter *pbtcoexadapter; /* = NULL; */ /* do not initialise globals to 0 or NULL */ +u8 rtw_btcoex_btinfo_cmd(_adapter *adapter, u8 *buf, u16 len) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + u8 *btinfo; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + btinfo = rtw_zmalloc(len); + if (btinfo == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = BTINFO_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = len; + pdrvextra_cmd_parm->pbuf = btinfo; + + _rtw_memcpy(btinfo, buf, len); + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +u8 rtw_btcoex_send_event_to_BT(_adapter *padapter, u8 status, u8 event_code, u8 opcode_low, u8 opcode_high, u8 *dbg_msg) +{ + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + pEvent->EventCode = event_code; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = opcode_low; + pEvent->Data[2] = opcode_high; + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; +#if 0 + rtw_btcoex_dump_tx_msg((u8 *)pEvent, tx_event_length, dbg_msg); +#endif + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + + return status; +} + +/* +Ref: +Realtek Wi-Fi Driver +Host Controller Interface for +Bluetooth 3.0 + HS V1.4 2013/02/07 + +Window team code & BT team code + */ + + +u8 rtw_btcoex_parse_BT_info_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ +#define BT_INFO_LENGTH 8 + + u8 curPollEnable = pcmd[0]; + u8 curPollTime = pcmd[1]; + u8 btInfoReason = pcmd[2]; + u8 btInfoLen = pcmd[3]; + u8 btinfo[BT_INFO_LENGTH]; + + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + rtw_HCI_event *pEvent; + + /* RTW_INFO("%s\n",__func__); + RTW_INFO("current Poll Enable: %d, currrent Poll Time: %d\n",curPollEnable,curPollTime); + RTW_INFO("BT Info reason: %d, BT Info length: %d\n",btInfoReason,btInfoLen); + RTW_INFO("%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n" + ,pcmd[4],pcmd[5],pcmd[6],pcmd[7],pcmd[8],pcmd[9],pcmd[10],pcmd[11]);*/ + + _rtw_memset(btinfo, 0, BT_INFO_LENGTH); + +#if 1 + if (BT_INFO_LENGTH != btInfoLen) { + status = HCI_STATUS_INVALID_HCI_CMD_PARA_VALUE; + RTW_INFO("Error BT Info Length: %d\n", btInfoLen); + /* return _FAIL; */ + } else +#endif + { + if (0x1 == btInfoReason || 0x2 == btInfoReason) { + _rtw_memcpy(btinfo, &pcmd[4], btInfoLen); + btinfo[0] = btInfoReason; + rtw_btcoex_btinfo_cmd(padapter, btinfo, btInfoLen); + } else + RTW_INFO("Other BT info reason\n"); + } + + /* send complete event to BT */ + { + + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_INFO_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_INFO_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; +#if 0 + rtw_btcoex_dump_tx_msg((u8 *)pEvent, tx_event_length, "BT_info_event"); +#endif + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_BT_patch_ver_info_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + u16 btPatchVer = 0x0, btHciVer = 0x0; + /* u16 *pU2tmp; */ + + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + + btHciVer = pcmd[0] | pcmd[1] << 8; + btPatchVer = pcmd[2] | pcmd[3] << 8; + + + RTW_INFO("%s, cmd:%02x %02x %02x %02x\n", __func__, pcmd[0] , pcmd[1] , pcmd[2] , pcmd[3]); + RTW_INFO("%s, HCI Ver:%d, Patch Ver:%d\n", __func__, btHciVer, btPatchVer); + + rtw_btcoex_SetBtPatchVersion(padapter, btHciVer, btPatchVer); + + + /* send complete event to BT */ + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_PATCH_VERSION_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_PATCH_VERSION_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; +#if 0 + rtw_btcoex_dump_tx_msg((u8 *)pEvent, tx_event_length, "BT_patch_event"); +#endif + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_HCI_Ver_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + u16 hciver = pcmd[0] | pcmd[1] << 8; + + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + + struct bt_coex_info *pcoex_info = &padapter->coex_info; + PBT_MGNT pBtMgnt = &pcoex_info->BtMgnt; + pBtMgnt->ExtConfig.HCIExtensionVer = hciver; + RTW_INFO("%s, HCI Version: %d\n", __func__, pBtMgnt->ExtConfig.HCIExtensionVer); + if (pBtMgnt->ExtConfig.HCIExtensionVer < 4) { + status = HCI_STATUS_INVALID_HCI_CMD_PARA_VALUE; + RTW_INFO("%s, Version = %d, HCI Version < 4\n", __func__, pBtMgnt->ExtConfig.HCIExtensionVer); + } else + rtw_btcoex_SetHciVersion(padapter, hciver); + /* send complete event to BT */ + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_EXTENSION_VERSION_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_EXTENSION_VERSION_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } + +} + +u8 rtw_btcoex_parse_WIFI_scan_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + + struct bt_coex_info *pcoex_info = &padapter->coex_info; + PBT_MGNT pBtMgnt = &pcoex_info->BtMgnt; + pBtMgnt->ExtConfig.bEnableWifiScanNotify = pcmd[0]; + RTW_INFO("%s, bEnableWifiScanNotify: %d\n", __func__, pBtMgnt->ExtConfig.bEnableWifiScanNotify); + + /* send complete event to BT */ + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_ENABLE_WIFI_SCAN_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_ENABLE_WIFI_SCAN_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_HCI_link_status_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + struct bt_coex_info *pcoex_info = &padapter->coex_info; + PBT_MGNT pBtMgnt = &pcoex_info->BtMgnt; + /* PBT_DBG pBtDbg=&padapter->MgntInfo.BtInfo.BtDbg; */ + u8 i, numOfHandle = 0, numOfAcl = 0; + u16 conHandle; + u8 btProfile, btCoreSpec, linkRole; + u8 *pTriple; + + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + + /* pBtDbg->dbgHciInfo.hciCmdCntLinkStatusNotify++; */ + /* RT_DISP_DATA(FIOCTL, IOCTL_BT_HCICMD_EXT, "LinkStatusNotify, Hex Data :\n", */ + /* &pHciCmd->Data[0], pHciCmd->Length); */ + + RTW_INFO("BTLinkStatusNotify\n"); + + /* Current only RTL8723 support this command. */ + /* pBtMgnt->bSupportProfile = TRUE; */ + pBtMgnt->bSupportProfile = _FALSE; + + pBtMgnt->ExtConfig.NumberOfACL = 0; + pBtMgnt->ExtConfig.NumberOfSCO = 0; + + numOfHandle = pcmd[0]; + /* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, ("numOfHandle = 0x%x\n", numOfHandle)); */ + /* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, ("HCIExtensionVer = %d\n", pBtMgnt->ExtConfig.HCIExtensionVer)); */ + RTW_INFO("numOfHandle = 0x%x\n", numOfHandle); + RTW_INFO("HCIExtensionVer = %d\n", pBtMgnt->ExtConfig.HCIExtensionVer); + + pTriple = &pcmd[1]; + for (i = 0; i < numOfHandle; i++) { + if (pBtMgnt->ExtConfig.HCIExtensionVer < 1) { + conHandle = *((u8 *)&pTriple[0]); + btProfile = pTriple[2]; + btCoreSpec = pTriple[3]; + if (BT_PROFILE_SCO == btProfile) + pBtMgnt->ExtConfig.NumberOfSCO++; + else { + pBtMgnt->ExtConfig.NumberOfACL++; + pBtMgnt->ExtConfig.aclLink[i].ConnectHandle = conHandle; + pBtMgnt->ExtConfig.aclLink[i].BTProfile = btProfile; + pBtMgnt->ExtConfig.aclLink[i].BTCoreSpec = btCoreSpec; + } + /* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, */ + /* ("Connection_Handle=0x%x, BTProfile=%d, BTSpec=%d\n", */ + /* conHandle, btProfile, btCoreSpec)); */ + RTW_INFO("Connection_Handle=0x%x, BTProfile=%d, BTSpec=%d\n", conHandle, btProfile, btCoreSpec); + pTriple += 4; + } else if (pBtMgnt->ExtConfig.HCIExtensionVer >= 1) { + conHandle = *((u16 *)&pTriple[0]); + btProfile = pTriple[2]; + btCoreSpec = pTriple[3]; + linkRole = pTriple[4]; + if (BT_PROFILE_SCO == btProfile) + pBtMgnt->ExtConfig.NumberOfSCO++; + else { + pBtMgnt->ExtConfig.NumberOfACL++; + pBtMgnt->ExtConfig.aclLink[i].ConnectHandle = conHandle; + pBtMgnt->ExtConfig.aclLink[i].BTProfile = btProfile; + pBtMgnt->ExtConfig.aclLink[i].BTCoreSpec = btCoreSpec; + pBtMgnt->ExtConfig.aclLink[i].linkRole = linkRole; + } + /* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, */ + RTW_INFO("Connection_Handle=0x%x, BTProfile=%d, BTSpec=%d, LinkRole=%d\n", + conHandle, btProfile, btCoreSpec, linkRole); + pTriple += 5; + } + } + rtw_btcoex_StackUpdateProfileInfo(); + + /* send complete event to BT */ + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_LINK_STATUS_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_LINK_STATUS_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } + + +} + +u8 rtw_btcoex_parse_HCI_BT_coex_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_COEX_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_COEX_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_HCI_BT_operation_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + RTW_INFO("%s, OP code: %d\n", __func__, pcmd[0]); + + switch (pcmd[0]) { + case HCI_BT_OP_NONE: + RTW_INFO("[bt operation] : Operation None!!\n"); + break; + case HCI_BT_OP_INQUIRY_START: + RTW_INFO("[bt operation] : Inquiry start!!\n"); + break; + case HCI_BT_OP_INQUIRY_FINISH: + RTW_INFO("[bt operation] : Inquiry finished!!\n"); + break; + case HCI_BT_OP_PAGING_START: + RTW_INFO("[bt operation] : Paging is started!!\n"); + break; + case HCI_BT_OP_PAGING_SUCCESS: + RTW_INFO("[bt operation] : Paging complete successfully!!\n"); + break; + case HCI_BT_OP_PAGING_UNSUCCESS: + RTW_INFO("[bt operation] : Paging complete unsuccessfully!!\n"); + break; + case HCI_BT_OP_PAIRING_START: + RTW_INFO("[bt operation] : Pairing start!!\n"); + break; + case HCI_BT_OP_PAIRING_FINISH: + RTW_INFO("[bt operation] : Pairing finished!!\n"); + break; + case HCI_BT_OP_BT_DEV_ENABLE: + RTW_INFO("[bt operation] : BT Device is enabled!!\n"); + break; + case HCI_BT_OP_BT_DEV_DISABLE: + RTW_INFO("[bt operation] : BT Device is disabled!!\n"); + break; + default: + RTW_INFO("[bt operation] : Unknown, error!!\n"); + break; + } + + /* send complete event to BT */ + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_OPERATION_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_OPERATION_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_BT_AFH_MAP_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_AFH_MAP_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_AFH_MAP_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_BT_register_val_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_REGISTER_VALUE_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_REGISTER_VALUE_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_HCI_BT_abnormal_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_BT_ABNORMAL_NOTIFY, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_ABNORMAL_NOTIFY, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +u8 rtw_btcoex_parse_HCI_query_RF_status_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen) +{ + u8 localBuf[6] = ""; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + rtw_HCI_event *pEvent; + RTW_HCI_STATUS status = HCI_STATUS_SUCCESS; + + { + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + + pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE; + pEvent->Data[0] = 0x1; /* packet # */ + pEvent->Data[1] = HCIOPCODELOW(HCI_QUERY_RF_STATUS, OGF_EXTENSION); + pEvent->Data[2] = HCIOPCODEHIGHT(HCI_QUERY_RF_STATUS, OGF_EXTENSION); + len = len + 3; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + pRetPar[0] = status; /* status */ + + len++; + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; + + status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + return status; + /* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */ + } +} + +/***************************************** +* HCI cmd format : +*| 15 - 0 | +*| OPcode (OCF|OGF<<10) | +*| 15 - 8 |7 - 0 | +*|Cmd para |Cmd para Length | +*|Cmd para...... | +******************************************/ + +/* bit 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 + * | OCF | OGF | */ +void rtw_btcoex_parse_hci_extend_cmd(_adapter *padapter, u8 *pcmd, u16 len, const u16 hci_OCF) +{ + + RTW_INFO("%s: OCF: %x\n", __func__, hci_OCF); + switch (hci_OCF) { + case HCI_EXTENSION_VERSION_NOTIFY: + RTW_INFO("HCI_EXTENSION_VERSION_NOTIFY\n"); + rtw_btcoex_parse_HCI_Ver_notify_cmd(padapter, pcmd, len); + break; + case HCI_LINK_STATUS_NOTIFY: + RTW_INFO("HCI_LINK_STATUS_NOTIFY\n"); + rtw_btcoex_parse_HCI_link_status_notify_cmd(padapter, pcmd, len); + break; + case HCI_BT_OPERATION_NOTIFY: + /* only for 8723a 2ant */ + RTW_INFO("HCI_BT_OPERATION_NOTIFY\n"); + rtw_btcoex_parse_HCI_BT_operation_notify_cmd(padapter, pcmd, len); + /* */ + break; + case HCI_ENABLE_WIFI_SCAN_NOTIFY: + RTW_INFO("HCI_ENABLE_WIFI_SCAN_NOTIFY\n"); + rtw_btcoex_parse_WIFI_scan_notify_cmd(padapter, pcmd, len); + break; + case HCI_QUERY_RF_STATUS: + /* only for 8723b 2ant */ + RTW_INFO("HCI_QUERY_RF_STATUS\n"); + rtw_btcoex_parse_HCI_query_RF_status_cmd(padapter, pcmd, len); + break; + case HCI_BT_ABNORMAL_NOTIFY: + RTW_INFO("HCI_BT_ABNORMAL_NOTIFY\n"); + rtw_btcoex_parse_HCI_BT_abnormal_notify_cmd(padapter, pcmd, len); + break; + case HCI_BT_INFO_NOTIFY: + RTW_INFO("HCI_BT_INFO_NOTIFY\n"); + rtw_btcoex_parse_BT_info_notify_cmd(padapter, pcmd, len); + break; + case HCI_BT_COEX_NOTIFY: + RTW_INFO("HCI_BT_COEX_NOTIFY\n"); + rtw_btcoex_parse_HCI_BT_coex_notify_cmd(padapter, pcmd, len); + break; + case HCI_BT_PATCH_VERSION_NOTIFY: + RTW_INFO("HCI_BT_PATCH_VERSION_NOTIFY\n"); + rtw_btcoex_parse_BT_patch_ver_info_cmd(padapter, pcmd, len); + break; + case HCI_BT_AFH_MAP_NOTIFY: + RTW_INFO("HCI_BT_AFH_MAP_NOTIFY\n"); + rtw_btcoex_parse_BT_AFH_MAP_notify_cmd(padapter, pcmd, len); + break; + case HCI_BT_REGISTER_VALUE_NOTIFY: + RTW_INFO("HCI_BT_REGISTER_VALUE_NOTIFY\n"); + rtw_btcoex_parse_BT_register_val_notify_cmd(padapter, pcmd, len); + break; + default: + RTW_INFO("ERROR!!! Unknown OCF: %x\n", hci_OCF); + break; + + } +} + +void rtw_btcoex_parse_hci_cmd(_adapter *padapter, u8 *pcmd, u16 len) +{ + u16 opcode = pcmd[0] | pcmd[1] << 8; + u16 hci_OGF = HCI_OGF(opcode); + u16 hci_OCF = HCI_OCF(opcode); + u8 cmdlen = len - 3; + u8 pare_len = pcmd[2]; + + RTW_INFO("%s OGF: %x,OCF: %x\n", __func__, hci_OGF, hci_OCF); + switch (hci_OGF) { + case OGF_EXTENSION: + RTW_INFO("HCI_EXTENSION_CMD_OGF\n"); + rtw_btcoex_parse_hci_extend_cmd(padapter, &pcmd[3], cmdlen, hci_OCF); + break; + default: + RTW_INFO("Other OGF: %x\n", hci_OGF); + break; + } +} + +u16 rtw_btcoex_parse_recv_data(u8 *msg, u8 msg_size) +{ + u8 cmp_msg1[32] = attend_ack; + u8 cmp_msg2[32] = leave_ack; + u8 cmp_msg3[32] = bt_leave; + u8 cmp_msg4[32] = invite_req; + u8 cmp_msg5[32] = attend_req; + u8 cmp_msg6[32] = invite_rsp; + u8 res = OTHER; + + if (_rtw_memcmp(cmp_msg1, msg, msg_size) == _TRUE) { + /*RTW_INFO("%s, msg:%s\n",__func__,msg);*/ + res = RX_ATTEND_ACK; + } else if (_rtw_memcmp(cmp_msg2, msg, msg_size) == _TRUE) { + /*RTW_INFO("%s, msg:%s\n",__func__,msg);*/ + res = RX_LEAVE_ACK; + } else if (_rtw_memcmp(cmp_msg3, msg, msg_size) == _TRUE) { + /*RTW_INFO("%s, msg:%s\n",__func__,msg);*/ + res = RX_BT_LEAVE; + } else if (_rtw_memcmp(cmp_msg4, msg, msg_size) == _TRUE) { + /*RTW_INFO("%s, msg:%s\n",__func__,msg);*/ + res = RX_INVITE_REQ; + } else if (_rtw_memcmp(cmp_msg5, msg, msg_size) == _TRUE) + res = RX_ATTEND_REQ; + else if (_rtw_memcmp(cmp_msg6, msg, msg_size) == _TRUE) + res = RX_INVITE_RSP; + else { + /*RTW_INFO("%s, %s\n", __func__, msg);*/ + res = OTHER; + } + + /*RTW_INFO("%s, res:%d\n", __func__, res);*/ + + return res; +} + +void rtw_btcoex_recvmsgbysocket(void *data) +{ + u8 recv_data[255]; + u8 tx_msg[255] = leave_ack; + u32 len = 0; + u16 recv_length = 0; + u16 parse_res = 0; +#if 0 + u8 para_len = 0, polling_enable = 0, poling_interval = 0, reason = 0, btinfo_len = 0; + u8 btinfo[BT_INFO_LEN] = {0}; +#endif + + struct bt_coex_info *pcoex_info = NULL; + struct sock *sk = NULL; + struct sk_buff *skb = NULL; + + /*RTW_INFO("%s\n",__func__);*/ + + if (pbtcoexadapter == NULL) { + RTW_INFO("%s: btcoexadapter NULL!\n", __func__); + return; + } + + pcoex_info = &pbtcoexadapter->coex_info; + sk = pcoex_info->sk_store; + + if (sk == NULL) { + RTW_INFO("%s: critical error when receive socket data!\n", __func__); + return; + } + + len = skb_queue_len(&sk->sk_receive_queue); + while (len > 0) { + skb = skb_dequeue(&sk->sk_receive_queue); + + /*important: cut the udp header from skb->data! header length is 8 byte*/ + recv_length = skb->len - 8; + _rtw_memset(recv_data, 0, sizeof(recv_data)); + _rtw_memcpy(recv_data, skb->data + 8, recv_length); + + parse_res = rtw_btcoex_parse_recv_data(recv_data, recv_length); +#if 0 + if (RX_ATTEND_ACK == parse_res) { + /* attend ack */ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_ATTEND_ACK!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + } else if (RX_ATTEND_REQ == parse_res) { + /* attend req from BT */ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_BT_ATTEND_REQ!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_sendmsgbysocket(pbtcoexadapter, attend_ack, sizeof(attend_ack), _FALSE); + } else if (RX_INVITE_REQ == parse_res) { + /* invite req from BT */ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_INVITE_REQ!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_sendmsgbysocket(pbtcoexadapter, invite_rsp, sizeof(invite_rsp), _FALSE); + } else if (RX_INVITE_RSP == parse_res) { + /* invite rsp */ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_INVITE_RSP!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + } else if (RX_LEAVE_ACK == parse_res) { + /* mean BT know wifi will leave */ + pcoex_info->BT_attend = _FALSE; + RTW_INFO("RX_LEAVE_ACK!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + } else if (RX_BT_LEAVE == parse_res) { + /* BT leave */ + rtw_btcoex_sendmsgbysocket(pbtcoexadapter, leave_ack, sizeof(leave_ack), _FALSE); /* no ack */ + pcoex_info->BT_attend = _FALSE; + RTW_INFO("RX_BT_LEAVE!sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + } else { + /* todo: check if recv data are really hci cmds */ + if (_TRUE == pcoex_info->BT_attend) + rtw_btcoex_parse_hci_cmd(pbtcoexadapter, recv_data, recv_length); + } +#endif + switch (parse_res) { + case RX_ATTEND_ACK: + /* attend ack */ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_ATTEND_ACK!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend); + break; + + case RX_ATTEND_REQ: + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_BT_ATTEND_REQ!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_sendmsgbysocket(pbtcoexadapter, attend_ack, sizeof(attend_ack), _FALSE); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend); + break; + + case RX_INVITE_REQ: + /* invite req from BT */ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_INVITE_REQ!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_sendmsgbysocket(pbtcoexadapter, invite_rsp, sizeof(invite_rsp), _FALSE); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend); + break; + + case RX_INVITE_RSP: + /*invite rsp*/ + pcoex_info->BT_attend = _TRUE; + RTW_INFO("RX_INVITE_RSP!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend); + break; + + case RX_LEAVE_ACK: + /* mean BT know wifi will leave */ + pcoex_info->BT_attend = _FALSE; + RTW_INFO("RX_LEAVE_ACK!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend); + break; + + case RX_BT_LEAVE: + /* BT leave */ + rtw_btcoex_sendmsgbysocket(pbtcoexadapter, leave_ack, sizeof(leave_ack), _FALSE); /* no ack */ + pcoex_info->BT_attend = _FALSE; + RTW_INFO("RX_BT_LEAVE!sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend); + break; + + default: + if (_TRUE == pcoex_info->BT_attend) + rtw_btcoex_parse_hci_cmd(pbtcoexadapter, recv_data, recv_length); + else + RTW_INFO("ERROR!! BT is UP\n"); + break; + + } + + len--; + kfree_skb(skb); + } +} + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 15, 0)) + void rtw_btcoex_recvmsg_init(struct sock *sk_in, s32 bytes) +#else + void rtw_btcoex_recvmsg_init(struct sock *sk_in) +#endif +{ + struct bt_coex_info *pcoex_info = NULL; + + if (pbtcoexadapter == NULL) { + RTW_INFO("%s: btcoexadapter NULL\n", __func__); + return; + } + pcoex_info = &pbtcoexadapter->coex_info; + pcoex_info->sk_store = sk_in; + if (pcoex_info->btcoex_wq != NULL) + queue_delayed_work(pcoex_info->btcoex_wq, &pcoex_info->recvmsg_work, 0); + else + RTW_INFO("%s: BTCOEX workqueue NULL\n", __func__); +} + +u8 rtw_btcoex_sendmsgbysocket(_adapter *padapter, u8 *msg, u8 msg_size, bool force) +{ + u8 error; + struct msghdr udpmsg; + mm_segment_t oldfs; + struct iovec iov; + struct bt_coex_info *pcoex_info = &padapter->coex_info; + + /* RTW_INFO("%s: msg:%s, force:%s\n", __func__, msg, force == _TRUE?"TRUE":"FALSE"); */ + if (_FALSE == force) { + if (_FALSE == pcoex_info->BT_attend) { + RTW_INFO("TX Blocked: WiFi-BT disconnected\n"); + return _FAIL; + } + } + + iov.iov_base = (void *)msg; + iov.iov_len = msg_size; + udpmsg.msg_name = &pcoex_info->bt_sockaddr; + udpmsg.msg_namelen = sizeof(struct sockaddr_in); +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 19, 0)) + /* referece:sock_xmit in kernel code + * WRITE for sock_sendmsg, READ for sock_recvmsg + * third parameter for msg_iovlen + * last parameter for iov_len + */ + iov_iter_init(&udpmsg.msg_iter, WRITE, &iov, 1, msg_size); +#else + udpmsg.msg_iov = &iov; + udpmsg.msg_iovlen = 1; +#endif + udpmsg.msg_control = NULL; + udpmsg.msg_controllen = 0; + udpmsg.msg_flags = MSG_DONTWAIT | MSG_NOSIGNAL; + oldfs = get_fs(); + set_fs(KERNEL_DS); + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 1, 0)) + error = sock_sendmsg(pcoex_info->udpsock, &udpmsg); +#else + error = sock_sendmsg(pcoex_info->udpsock, &udpmsg, msg_size); +#endif + set_fs(oldfs); + if (error < 0) { + RTW_INFO("Error when sendimg msg, error:%d\n", error); + return _FAIL; + } else + return _SUCCESS; +} + +u8 rtw_btcoex_create_kernel_socket(_adapter *padapter) +{ + s8 kernel_socket_err; + u8 tx_msg[255] = attend_req; + struct bt_coex_info *pcoex_info = &padapter->coex_info; + s32 sock_reuse = 1; + u8 status = _FAIL; + + RTW_INFO("%s CONNECT_PORT %d\n", __func__, CONNECT_PORT); + + if (NULL == pcoex_info) { + RTW_INFO("coex_info: NULL\n"); + status = _FAIL; + } + + kernel_socket_err = sock_create(PF_INET, SOCK_DGRAM, 0, &pcoex_info->udpsock); + + if (kernel_socket_err < 0) { + RTW_INFO("Error during creation of socket error:%d\n", kernel_socket_err); + status = _FAIL; + } else { + _rtw_memset(&(pcoex_info->wifi_sockaddr), 0, sizeof(pcoex_info->wifi_sockaddr)); + pcoex_info->wifi_sockaddr.sin_family = AF_INET; + pcoex_info->wifi_sockaddr.sin_port = htons(CONNECT_PORT); + pcoex_info->wifi_sockaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + + _rtw_memset(&(pcoex_info->bt_sockaddr), 0, sizeof(pcoex_info->bt_sockaddr)); + pcoex_info->bt_sockaddr.sin_family = AF_INET; + pcoex_info->bt_sockaddr.sin_port = htons(CONNECT_PORT_BT); + pcoex_info->bt_sockaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + + pcoex_info->sk_store = NULL; + kernel_socket_err = pcoex_info->udpsock->ops->bind(pcoex_info->udpsock, (struct sockaddr *)&pcoex_info->wifi_sockaddr, + sizeof(pcoex_info->wifi_sockaddr)); + if (kernel_socket_err == 0) { + RTW_INFO("binding socket success\n"); + pcoex_info->udpsock->sk->sk_data_ready = rtw_btcoex_recvmsg_init; + pcoex_info->sock_open |= KERNEL_SOCKET_OK; + pcoex_info->BT_attend = _FALSE; + RTW_INFO("WIFI sending attend_req\n"); + rtw_btcoex_sendmsgbysocket(padapter, attend_req, sizeof(attend_req), _TRUE); + status = _SUCCESS; + } else { + pcoex_info->BT_attend = _FALSE; + sock_release(pcoex_info->udpsock); /* bind fail release socket */ + RTW_INFO("Error binding socket: %d\n", kernel_socket_err); + status = _FAIL; + } + + } + + return status; +} + +void rtw_btcoex_close_kernel_socket(_adapter *padapter) +{ + struct bt_coex_info *pcoex_info = &padapter->coex_info; + if (pcoex_info->sock_open & KERNEL_SOCKET_OK) { + RTW_INFO("release kernel socket\n"); + sock_release(pcoex_info->udpsock); + pcoex_info->sock_open &= ~(KERNEL_SOCKET_OK); + if (_TRUE == pcoex_info->BT_attend) + pcoex_info->BT_attend = _FALSE; + + RTW_INFO("sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend); + } +} + +void rtw_btcoex_init_socket(_adapter *padapter) +{ + + u8 is_invite = _FALSE; + struct bt_coex_info *pcoex_info = &padapter->coex_info; + RTW_INFO("%s\n", __func__); + if (_FALSE == pcoex_info->is_exist) { + _rtw_memset(pcoex_info, 0, sizeof(struct bt_coex_info)); + pcoex_info->btcoex_wq = create_workqueue("BTCOEX"); + INIT_DELAYED_WORK(&pcoex_info->recvmsg_work, + (void *)rtw_btcoex_recvmsgbysocket); + pbtcoexadapter = padapter; + /* We expect BT is off if BT don't send ack to wifi */ + RTW_INFO("We expect BT is off if BT send ack to wifi\n"); + rtw_btcoex_pta_off_on_notify(pbtcoexadapter, _FALSE); + if (rtw_btcoex_create_kernel_socket(padapter) == _SUCCESS) + pcoex_info->is_exist = _TRUE; + else { + pcoex_info->is_exist = _FALSE; + pbtcoexadapter = NULL; + } + + RTW_INFO("%s: pbtcoexadapter:%p, coex_info->is_exist: %s\n" + , __func__, pbtcoexadapter, pcoex_info->is_exist == _TRUE ? "TRUE" : "FALSE"); + } +} + +void rtw_btcoex_close_socket(_adapter *padapter) +{ + struct bt_coex_info *pcoex_info = &padapter->coex_info; + + RTW_INFO("%s--coex_info->is_exist: %s, pcoex_info->BT_attend:%s\n" + , __func__, pcoex_info->is_exist == _TRUE ? "TRUE" : "FALSE", pcoex_info->BT_attend == _TRUE ? "TRUE" : "FALSE"); + + if (_TRUE == pcoex_info->is_exist) { + if (_TRUE == pcoex_info->BT_attend) { + /*inform BT wifi leave*/ + rtw_btcoex_sendmsgbysocket(padapter, wifi_leave, sizeof(wifi_leave), _FALSE); + msleep(50); + } + + if (pcoex_info->btcoex_wq != NULL) { + flush_workqueue(pcoex_info->btcoex_wq); + destroy_workqueue(pcoex_info->btcoex_wq); + } + + rtw_btcoex_close_kernel_socket(padapter); + pbtcoexadapter = NULL; + pcoex_info->is_exist = _FALSE; + } +} + +void rtw_btcoex_dump_tx_msg(u8 *tx_msg, u8 len, u8 *msg_name) +{ + u8 i = 0; + RTW_INFO("======> Msg name: %s\n", msg_name); + for (i = 0; i < len; i++) + printk("%02x ", tx_msg[i]); + printk("\n"); + RTW_INFO("Msg name: %s <======\n", msg_name); +} + +/* Porting from Windows team */ +void rtw_btcoex_SendEventExtBtCoexControl(PADAPTER padapter, u8 bNeedDbgRsp, u8 dataLen, void *pData) +{ + u8 len = 0, tx_event_length = 0; + u8 localBuf[32] = ""; + u8 *pRetPar; + u8 opCode = 0; + u8 *pInBuf = (u8 *)pData; + u8 *pOpCodeContent; + rtw_HCI_event *pEvent; + + opCode = pInBuf[0]; + + RTW_INFO("%s, OPCode:%02x\n", __func__, opCode); + + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + /* len += bthci_ExtensionEventHeaderRtk(&localBuf[0], */ + /* HCI_EVENT_EXT_BT_COEX_CONTROL); */ + pEvent->EventCode = HCI_EVENT_EXTENSION_RTK; + pEvent->Data[0] = HCI_EVENT_EXT_BT_COEX_CONTROL; /* extension event code */ + len++; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + _rtw_memcpy(&pRetPar[0], pData, dataLen); + + len += dataLen; + + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; +#if 0 + rtw_btcoex_dump_tx_msg((u8 *)pEvent, tx_event_length, "BT COEX CONTROL", _FALSE); +#endif + rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + +} + +/* Porting from Windows team */ +void rtw_btcoex_SendEventExtBtInfoControl(PADAPTER padapter, u8 dataLen, void *pData) +{ + rtw_HCI_event *pEvent; + u8 *pRetPar; + u8 len = 0, tx_event_length = 0; + u8 localBuf[32] = ""; + + struct bt_coex_info *pcoex_info = &padapter->coex_info; + PBT_MGNT pBtMgnt = &pcoex_info->BtMgnt; + + /* RTW_INFO("%s\n",__func__);*/ + if (pBtMgnt->ExtConfig.HCIExtensionVer < 4) { /* not support */ + RTW_INFO("ERROR: HCIExtensionVer = %d, HCIExtensionVer<4 !!!!\n", pBtMgnt->ExtConfig.HCIExtensionVer); + return; + } + + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + /* len += bthci_ExtensionEventHeaderRtk(&localBuf[0], */ + /* HCI_EVENT_EXT_BT_INFO_CONTROL); */ + pEvent->EventCode = HCI_EVENT_EXTENSION_RTK; + pEvent->Data[0] = HCI_EVENT_EXT_BT_INFO_CONTROL; /* extension event code */ + len++; + + /* Return parameters starts from here */ + pRetPar = &pEvent->Data[len]; + _rtw_memcpy(&pRetPar[0], pData, dataLen); + + len += dataLen; + + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; +#if 0 + rtw_btcoex_dump_tx_msg((u8 *)pEvent, tx_event_length, "BT INFO CONTROL"); +#endif + rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); + +} + +void rtw_btcoex_SendScanNotify(PADAPTER padapter, u8 scanType) +{ + u8 len = 0, tx_event_length = 0; + u8 localBuf[7] = ""; + u8 *pRetPar; + u8 *pu1Temp; + rtw_HCI_event *pEvent; + struct bt_coex_info *pcoex_info = &padapter->coex_info; + PBT_MGNT pBtMgnt = &pcoex_info->BtMgnt; + + /* if(!pBtMgnt->BtOperationOn) + * return; */ + + pEvent = (rtw_HCI_event *)(&localBuf[0]); + + /* len += bthci_ExtensionEventHeaderRtk(&localBuf[0], + * HCI_EVENT_EXT_WIFI_SCAN_NOTIFY); */ + + pEvent->EventCode = HCI_EVENT_EXTENSION_RTK; + pEvent->Data[0] = HCI_EVENT_EXT_WIFI_SCAN_NOTIFY; /* extension event code */ + len++; + + /* Return parameters starts from here */ + /* pRetPar = &PPacketIrpEvent->Data[len]; */ + /* pu1Temp = (u8 *)&pRetPar[0]; */ + /* *pu1Temp = scanType; */ + pEvent->Data[len] = scanType; + len += 1; + + pEvent->Length = len; + + /* total tx event length + EventCode length + sizeof(length) */ + tx_event_length = pEvent->Length + 2; +#if 0 + rtw_btcoex_dump_tx_msg((u8 *)pEvent, tx_event_length, "WIFI SCAN OPERATION"); +#endif + rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, _FALSE); +} +#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */ +#endif /* CONFIG_BT_COEXIST */ + +void rtw_btcoex_set_ant_info(PADAPTER padapter) +{ +#ifdef CONFIG_BT_COEXIST + PHAL_DATA_TYPE hal = GET_HAL_DATA(padapter); + + if (hal->EEPROMBluetoothCoexist == _TRUE) { + u8 bMacPwrCtrlOn = _FALSE; + + rtw_btcoex_AntInfoSetting(padapter); + rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn); + if (bMacPwrCtrlOn == _TRUE) + rtw_btcoex_PowerOnSetting(padapter); + } + else +#endif + rtw_btcoex_wifionly_AntInfoSetting(padapter); +} + +void rtw_btcoex_connect_notify(PADAPTER padapter, u8 join_type) +{ +#ifdef CONFIG_BT_COEXIST + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + + if (pHalData->EEPROMBluetoothCoexist == _TRUE) + _rtw_btcoex_connect_notify(padapter, join_type ? _FALSE : _TRUE); + else +#endif /* CONFIG_BT_COEXIST */ + rtw_btcoex_wifionly_connect_notify(padapter); +} + diff --git a/core/rtw_btcoex_wifionly.c b/core/rtw_btcoex_wifionly.c new file mode 100644 index 0000000..d9872b0 --- /dev/null +++ b/core/rtw_btcoex_wifionly.c @@ -0,0 +1,47 @@ +/****************************************************************************** + * + * Copyright(c) 2013 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#include +#include +#include + +void rtw_btcoex_wifionly_switchband_notify(PADAPTER padapter) +{ + hal_btcoex_wifionly_switchband_notify(padapter); +} + +void rtw_btcoex_wifionly_scan_notify(PADAPTER padapter) +{ + hal_btcoex_wifionly_scan_notify(padapter); +} + +void rtw_btcoex_wifionly_connect_notify(PADAPTER padapter) +{ + hal_btcoex_wifionly_connect_notify(padapter); +} + +void rtw_btcoex_wifionly_hw_config(PADAPTER padapter) +{ + hal_btcoex_wifionly_hw_config(padapter); +} + +void rtw_btcoex_wifionly_initialize(PADAPTER padapter) +{ + hal_btcoex_wifionly_initlizevariables(padapter); +} + +void rtw_btcoex_wifionly_AntInfoSetting(PADAPTER padapter) +{ + hal_btcoex_wifionly_AntInfoSetting(padapter); +} diff --git a/core/rtw_chplan.c b/core/rtw_chplan.c new file mode 100644 index 0000000..82b3671 --- /dev/null +++ b/core/rtw_chplan.c @@ -0,0 +1,2516 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2018 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_CHPLAN_C_ + +#include + +#define RTW_DOMAIN_MAP_VER "54" +#define RTW_DOMAIN_MAP_M_VER "g" +#define RTW_COUNTRY_MAP_VER "27" + +struct ch_list_t { + u8 *len_ch_attr; +}; + +#define CLA_2G_12_14_PASSIVE BIT0 + +#define CLA_5G_B1_PASSIVE BIT0 +#define CLA_5G_B2_PASSIVE BIT1 +#define CLA_5G_B3_PASSIVE BIT2 +#define CLA_5G_B4_PASSIVE BIT3 +#define CLA_5G_B2_DFS BIT4 +#define CLA_5G_B3_DFS BIT5 +#define CLA_5G_B4_DFS BIT6 + +#define CH_LIST_ENT(_len, arg...) \ + {.len_ch_attr = (u8[_len + 2]) {_len, ##arg}, } + +#define CH_LIST_LEN(_ch_list) (_ch_list.len_ch_attr[0]) +#define CH_LIST_CH(_ch_list, _i) (_ch_list.len_ch_attr[_i + 1]) +#define CH_LIST_ATTRIB(_ch_list) (_ch_list.len_ch_attr[CH_LIST_LEN(_ch_list) + 1]) + +enum rtw_chd_2g { + RTW_CHD_2G_00 = 0, + RTW_CHD_2G_01 = 1, + RTW_CHD_2G_02 = 2, + RTW_CHD_2G_03 = 3, + RTW_CHD_2G_04 = 4, + RTW_CHD_2G_05 = 5, + RTW_CHD_2G_06 = 6, + + RTW_CHD_2G_MAX, + RTW_CHD_2G_NULL = RTW_CHD_2G_00, +}; + +enum rtw_chd_5g { + RTW_CHD_5G_00 = 0, + RTW_CHD_5G_01 = 1, + RTW_CHD_5G_02 = 2, + RTW_CHD_5G_03 = 3, + RTW_CHD_5G_04 = 4, + RTW_CHD_5G_05 = 5, + RTW_CHD_5G_06 = 6, + RTW_CHD_5G_07 = 7, + RTW_CHD_5G_08 = 8, + RTW_CHD_5G_09 = 9, + RTW_CHD_5G_10 = 10, + RTW_CHD_5G_11 = 11, + RTW_CHD_5G_12 = 12, + RTW_CHD_5G_13 = 13, + RTW_CHD_5G_14 = 14, + RTW_CHD_5G_15 = 15, + RTW_CHD_5G_16 = 16, + RTW_CHD_5G_17 = 17, + RTW_CHD_5G_18 = 18, + RTW_CHD_5G_19 = 19, + RTW_CHD_5G_20 = 20, + RTW_CHD_5G_21 = 21, + RTW_CHD_5G_22 = 22, + RTW_CHD_5G_23 = 23, + RTW_CHD_5G_24 = 24, + RTW_CHD_5G_25 = 25, + RTW_CHD_5G_26 = 26, + RTW_CHD_5G_27 = 27, + RTW_CHD_5G_28 = 28, + RTW_CHD_5G_29 = 29, + RTW_CHD_5G_30 = 30, + RTW_CHD_5G_31 = 31, + RTW_CHD_5G_32 = 32, + RTW_CHD_5G_33 = 33, + RTW_CHD_5G_34 = 34, + RTW_CHD_5G_35 = 35, + RTW_CHD_5G_36 = 36, + RTW_CHD_5G_37 = 37, + RTW_CHD_5G_38 = 38, + RTW_CHD_5G_39 = 39, + RTW_CHD_5G_40 = 40, + RTW_CHD_5G_41 = 41, + RTW_CHD_5G_42 = 42, + RTW_CHD_5G_43 = 43, + RTW_CHD_5G_44 = 44, + RTW_CHD_5G_45 = 45, + RTW_CHD_5G_46 = 46, + RTW_CHD_5G_47 = 47, + RTW_CHD_5G_48 = 48, + RTW_CHD_5G_49 = 49, + RTW_CHD_5G_50 = 50, + RTW_CHD_5G_51 = 51, + + RTW_CHD_5G_MAX, + RTW_CHD_5G_NULL = RTW_CHD_5G_00, +}; + +static const struct ch_list_t rtw_channel_def_2g[] = { + /* 0, RTW_CHD_2G_00 */ CH_LIST_ENT(0, 0), + /* 1, RTW_CHD_2G_01 */ CH_LIST_ENT(13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, CLA_2G_12_14_PASSIVE), + /* 2, RTW_CHD_2G_02 */ CH_LIST_ENT(13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 0), + /* 3, RTW_CHD_2G_03 */ CH_LIST_ENT(11, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 0), + /* 4, RTW_CHD_2G_04 */ CH_LIST_ENT(14, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 0), + /* 5, RTW_CHD_2G_05 */ CH_LIST_ENT(4, 10, 11, 12, 13, 0), + /* 6, RTW_CHD_2G_06 */ CH_LIST_ENT(14, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, CLA_2G_12_14_PASSIVE), +}; + +#if CONFIG_IEEE80211_BAND_5GHZ +static const struct ch_list_t rtw_channel_def_5g[] = { + /* 0, RTW_CHD_5G_00 */ CH_LIST_ENT(0, 0), + /* 1, RTW_CHD_5G_01 */ CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 2, RTW_CHD_5G_02 */ CH_LIST_ENT(19, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 3, RTW_CHD_5G_03 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 4, RTW_CHD_5G_04 */ CH_LIST_ENT(22, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 5, RTW_CHD_5G_05 */ CH_LIST_ENT(19, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 6, RTW_CHD_5G_06 */ CH_LIST_ENT(9, 36, 40, 44, 48, 149, 153, 157, 161, 165, 0), + /* 7, RTW_CHD_5G_07 */ CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165, CLA_5G_B2_DFS), + /* 8, RTW_CHD_5G_08 */ CH_LIST_ENT(12, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, CLA_5G_B2_DFS), + /* 9, RTW_CHD_5G_09 */ CH_LIST_ENT(5, 149, 153, 157, 161, 165, 0), + /* 10, RTW_CHD_5G_10 */ CH_LIST_ENT(8, 36, 40, 44, 48, 52, 56, 60, 64, CLA_5G_B2_DFS), + /* 11, RTW_CHD_5G_11 */ CH_LIST_ENT(11, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, CLA_5G_B3_DFS), + /* 12, RTW_CHD_5G_12 */ CH_LIST_ENT(16, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 13, RTW_CHD_5G_13 */ CH_LIST_ENT(8, 56, 60, 64, 149, 153, 157, 161, 165, CLA_5G_B2_DFS), + /* 14, RTW_CHD_5G_14 */ CH_LIST_ENT(4, 36, 40, 44, 48, 0), + /* 15, RTW_CHD_5G_15 */ CH_LIST_ENT(4, 149, 153, 157, 161, 0), + /* 16, RTW_CHD_5G_16 */ CH_LIST_ENT(11, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 0), + /* 17, RTW_CHD_5G_17 */ CH_LIST_ENT(16, 36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 18, RTW_CHD_5G_18 */ CH_LIST_ENT(17, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 19, RTW_CHD_5G_19 */ CH_LIST_ENT(16, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 20, RTW_CHD_5G_20 */ CH_LIST_ENT(20, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 21, RTW_CHD_5G_21 */ CH_LIST_ENT(11, 36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 22, RTW_CHD_5G_22 */ CH_LIST_ENT(25, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 23, RTW_CHD_5G_23 */ CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 24, RTW_CHD_5G_24 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE | CLA_5G_B4_PASSIVE), + /* 25, RTW_CHD_5G_25 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE), + /* 26, RTW_CHD_5G_26 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE), + /* 27, RTW_CHD_5G_27 */ CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE), + /* 28, RTW_CHD_5G_28 */ CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165, CLA_5G_B2_PASSIVE), + /* 29, RTW_CHD_5G_29 */ CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE), + /* 30, RTW_CHD_5G_30 */ CH_LIST_ENT(9, 36, 40, 44, 48, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B4_PASSIVE), + /* 31, RTW_CHD_5G_31 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE | CLA_5G_B4_PASSIVE), + /* 32, RTW_CHD_5G_32 */ CH_LIST_ENT(9, 52, 56, 60, 64, 149, 153, 157, 161, 165, CLA_5G_B2_DFS), + /* 33, RTW_CHD_5G_33 */ CH_LIST_ENT(22, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 144, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 34, RTW_CHD_5G_34 */ CH_LIST_ENT(13, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B3_DFS), + /* 35, RTW_CHD_5G_35 */ CH_LIST_ENT(8, 100, 104, 108, 112, 116, 132, 136, 140, CLA_5G_B3_DFS), + /* 36, RTW_CHD_5G_36 */ CH_LIST_ENT(25, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE | CLA_5G_B2_DFS | CLA_5G_B3_PASSIVE | CLA_5G_B3_DFS | CLA_5G_B4_PASSIVE), + /* 37, RTW_CHD_5G_37 */ CH_LIST_ENT(8, 36, 40, 44, 48, 52, 56, 60, 64, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE), + /* 38, RTW_CHD_5G_38 */ CH_LIST_ENT(16, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 39, RTW_CHD_5G_39 */ CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_DFS | CLA_5G_B3_DFS | CLA_5G_B4_DFS), + /* 40, RTW_CHD_5G_40 */ CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 41, RTW_CHD_5G_41 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS | CLA_5G_B4_PASSIVE), + /* 42, RTW_CHD_5G_42 */ CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_DFS | CLA_5G_B3_DFS | CLA_5G_B4_PASSIVE), + /* 43, RTW_CHD_5G_43 */ CH_LIST_ENT(23, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE | CLA_5G_B4_PASSIVE), + /* 44, RTW_CHD_5G_44 */ CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE | CLA_5G_B4_PASSIVE), + /* 45, RTW_CHD_5G_45 */ CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165, CLA_5G_B1_PASSIVE | CLA_5G_B2_PASSIVE | CLA_5G_B4_PASSIVE), + /* 46, RTW_CHD_5G_46 */ CH_LIST_ENT(12, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, CLA_5G_B2_PASSIVE), + /* 47, RTW_CHD_5G_47 */ CH_LIST_ENT(19, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, CLA_5G_B2_PASSIVE | CLA_5G_B3_PASSIVE), + /* 48, RTW_CHD_5G_48 */ CH_LIST_ENT(20, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 49, RTW_CHD_5G_49 */ CH_LIST_ENT(17, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 50, RTW_CHD_5G_50 */ CH_LIST_ENT(17, 36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, 144, 149, 153, 157, 161, 165, CLA_5G_B2_DFS | CLA_5G_B3_DFS), + /* 51, RTW_CHD_5G_51 */ CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, CLA_5G_B2_DFS | CLA_5G_B3_DFS), +}; +#endif /* CONFIG_IEEE80211_BAND_5GHZ */ + +struct chplan_ent_t { + u8 regd_2g; /* value of REGULATION_TXPWR_LMT */ + u8 chd_2g; +#if CONFIG_IEEE80211_BAND_5GHZ + u8 regd_5g; /* value of REGULATION_TXPWR_LMT */ + u8 chd_5g; +#endif +}; + +#if CONFIG_IEEE80211_BAND_5GHZ +#define CHPLAN_ENT(_regd_2g, _chd_2g, _regd_5g, _chd_5g) {.regd_2g = _regd_2g, .chd_2g = _chd_2g, .regd_5g = _regd_5g, .chd_5g = _chd_5g} +#else +#define CHPLAN_ENT(_regd_2g, _chd_2g, _regd_5g, _chd_5g) {.regd_2g = _regd_2g, .chd_2g = _chd_2g} +#endif + +#define CHPLAN_ENT_NOT_DEFINED CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_NULL, TXPWR_LMT_NONE, RTW_CHD_5G_NULL) + +static const struct chplan_ent_t RTW_ChannelPlanMap[] = { + /* 0x00 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_49), + /* 0x01 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_50), + /* 0x02 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_03, TXPWR_LMT_ETSI, RTW_CHD_5G_07), + /* 0x03 */ CHPLAN_ENT(TXPWR_LMT_ACMA, RTW_CHD_2G_02, TXPWR_LMT_ACMA, RTW_CHD_5G_33), + /* 0x04 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_51), + /* 0x05 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x06 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x07 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x08 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x09 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x0A */ CHPLAN_ENT_NOT_DEFINED, + /* 0x0B */ CHPLAN_ENT_NOT_DEFINED, + /* 0x0C */ CHPLAN_ENT_NOT_DEFINED, + /* 0x0D */ CHPLAN_ENT_NOT_DEFINED, + /* 0x0E */ CHPLAN_ENT_NOT_DEFINED, + /* 0x0F */ CHPLAN_ENT_NOT_DEFINED, + /* 0x10 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x11 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x12 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x13 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x14 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x15 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x16 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x17 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x18 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x19 */ CHPLAN_ENT_NOT_DEFINED, + /* 0x1A */ CHPLAN_ENT_NOT_DEFINED, + /* 0x1B */ CHPLAN_ENT_NOT_DEFINED, + /* 0x1C */ CHPLAN_ENT_NOT_DEFINED, + /* 0x1D */ CHPLAN_ENT_NOT_DEFINED, + /* 0x1E */ CHPLAN_ENT_NOT_DEFINED, + /* 0x1F */ CHPLAN_ENT_NOT_DEFINED, + /* 0x20 */ CHPLAN_ENT(TXPWR_LMT_WW, RTW_CHD_2G_01, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x21 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x22 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x23 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_04, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x24 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_05, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x25 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_03), + /* 0x26 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_02), + /* 0x27 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_04, TXPWR_LMT_MKK, RTW_CHD_5G_02), + /* 0x28 */ CHPLAN_ENT(TXPWR_LMT_KCC, RTW_CHD_2G_01, TXPWR_LMT_KCC, RTW_CHD_5G_05), + /* 0x29 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_06), + /* 0x2A */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x2B */ CHPLAN_ENT(TXPWR_LMT_IC, RTW_CHD_2G_02, TXPWR_LMT_IC, RTW_CHD_5G_33), + /* 0x2C */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_02, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x2D */ CHPLAN_ENT(TXPWR_LMT_CHILE, RTW_CHD_2G_01, TXPWR_LMT_CHILE, RTW_CHD_5G_22), + /* 0x2E */ CHPLAN_ENT(TXPWR_LMT_WW, RTW_CHD_2G_03, TXPWR_LMT_WW, RTW_CHD_5G_37), + /* 0x2F */ CHPLAN_ENT(TXPWR_LMT_CHILE, RTW_CHD_2G_01, TXPWR_LMT_CHILE, RTW_CHD_5G_38), + /* 0x30 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_07), + /* 0x31 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_08), + /* 0x32 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_09), + /* 0x33 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_10), + /* 0x34 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_01), + /* 0x35 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_03), + /* 0x36 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_04), + /* 0x37 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_04, TXPWR_LMT_MKK, RTW_CHD_5G_10), + /* 0x38 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_04, TXPWR_LMT_MKK, RTW_CHD_5G_11), + /* 0x39 */ CHPLAN_ENT(TXPWR_LMT_NCC, RTW_CHD_2G_03, TXPWR_LMT_NCC, RTW_CHD_5G_12), + /* 0x3A */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_02), + /* 0x3B */ CHPLAN_ENT(TXPWR_LMT_ACMA, RTW_CHD_2G_02, TXPWR_LMT_ACMA, RTW_CHD_5G_01), + /* 0x3C */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_10), + /* 0x3D */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_15), + /* 0x3E */ CHPLAN_ENT(TXPWR_LMT_KCC, RTW_CHD_2G_02, TXPWR_LMT_KCC, RTW_CHD_5G_03), + /* 0x3F */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_22), + /* 0x40 */ CHPLAN_ENT(TXPWR_LMT_NCC, RTW_CHD_2G_03, TXPWR_LMT_NCC, RTW_CHD_5G_13), + /* 0x41 */ CHPLAN_ENT(TXPWR_LMT_WW, RTW_CHD_2G_06, TXPWR_LMT_NONE, RTW_CHD_5G_00), + /* 0x42 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_14), + /* 0x43 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_06), + /* 0x44 */ CHPLAN_ENT(TXPWR_LMT_NCC, RTW_CHD_2G_03, TXPWR_LMT_NCC, RTW_CHD_5G_09), + /* 0x45 */ CHPLAN_ENT(TXPWR_LMT_ACMA, RTW_CHD_2G_01, TXPWR_LMT_ACMA, RTW_CHD_5G_01), + /* 0x46 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_15), + /* 0x47 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_10), + /* 0x48 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_07), + /* 0x49 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_06), + /* 0x4A */ CHPLAN_ENT(TXPWR_LMT_IC, RTW_CHD_2G_03, TXPWR_LMT_IC, RTW_CHD_5G_33), + /* 0x4B */ CHPLAN_ENT(TXPWR_LMT_KCC, RTW_CHD_2G_02, TXPWR_LMT_KCC, RTW_CHD_5G_22), + /* 0x4C */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_28), + /* 0x4D */ CHPLAN_ENT(TXPWR_LMT_MEXICO, RTW_CHD_2G_02, TXPWR_LMT_MEXICO, RTW_CHD_5G_01), + /* 0x4E */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_42), + /* 0x4F */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_MKK, RTW_CHD_5G_43), + /* 0x50 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_16), + /* 0x51 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_09), + /* 0x52 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_17), + /* 0x53 */ CHPLAN_ENT(TXPWR_LMT_NCC, RTW_CHD_2G_03, TXPWR_LMT_NCC, RTW_CHD_5G_18), + /* 0x54 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_15), + /* 0x55 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_01), + /* 0x56 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_19), + /* 0x57 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_20), + /* 0x58 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_02, TXPWR_LMT_MKK, RTW_CHD_5G_14), + /* 0x59 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_21), + /* 0x5A */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_FCC, RTW_CHD_5G_44), + /* 0x5B */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_FCC, RTW_CHD_5G_45), + /* 0x5C */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_FCC, RTW_CHD_5G_43), + /* 0x5D */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_08), + /* 0x5E */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_03), + /* 0x5F */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_02, TXPWR_LMT_MKK, RTW_CHD_5G_47), + /* 0x60 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_09), + /* 0x61 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_01), + /* 0x62 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_03), + /* 0x63 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_23), + /* 0x64 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_02, TXPWR_LMT_MKK, RTW_CHD_5G_24), + /* 0x65 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_24), + /* 0x66 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_27), + /* 0x67 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_25), + /* 0x68 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_27), + /* 0x69 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_25), + /* 0x6A */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_25), + /* 0x6B */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_29), + /* 0x6C */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_26), + /* 0x6D */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_28), + /* 0x6E */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_25), + /* 0x6F */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_ETSI, RTW_CHD_5G_06), + /* 0x70 */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_ETSI, RTW_CHD_5G_30), + /* 0x71 */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_ETSI, RTW_CHD_5G_25), + /* 0x72 */ CHPLAN_ENT(TXPWR_LMT_NONE, RTW_CHD_2G_00, TXPWR_LMT_ETSI, RTW_CHD_5G_31), + /* 0x73 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_01), + /* 0x74 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_19), + /* 0x75 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_32), + /* 0x76 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_02, TXPWR_LMT_FCC, RTW_CHD_5G_22), + /* 0x77 */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_01, TXPWR_LMT_ETSI, RTW_CHD_5G_34), + /* 0x78 */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_03, TXPWR_LMT_FCC, RTW_CHD_5G_35), + /* 0x79 */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_02, TXPWR_LMT_MKK, RTW_CHD_5G_02), + /* 0x7A */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_28), + /* 0x7B */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_46), + /* 0x7C */ CHPLAN_ENT(TXPWR_LMT_ETSI, RTW_CHD_2G_02, TXPWR_LMT_ETSI, RTW_CHD_5G_47), + /* 0x7D */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_04, TXPWR_LMT_MKK, RTW_CHD_5G_48), + /* 0x7E */ CHPLAN_ENT(TXPWR_LMT_MKK, RTW_CHD_2G_02, TXPWR_LMT_MKK, RTW_CHD_5G_48), + /* 0x7F */ CHPLAN_ENT(TXPWR_LMT_FCC, RTW_CHD_2G_01, TXPWR_LMT_FCC, RTW_CHD_5G_03), +}; + +const int RTW_ChannelPlanMap_size = sizeof(RTW_ChannelPlanMap) / sizeof(RTW_ChannelPlanMap[0]); + +u8 rtw_chplan_get_default_regd_2g(u8 id) +{ + return RTW_ChannelPlanMap[id].regd_2g; +} + +u8 rtw_chplan_get_default_regd_5g(u8 id) +{ +#if CONFIG_IEEE80211_BAND_5GHZ + return RTW_ChannelPlanMap[id].regd_5g; +#else + return TXPWR_LMT_NONE; +#endif +} + +u8 rtw_chplan_get_default_regd(u8 id) +{ + u8 regd_2g = rtw_chplan_get_default_regd_2g(id); + u8 regd_5g = rtw_chplan_get_default_regd_5g(id); + + if (regd_2g != TXPWR_LMT_NONE && regd_5g != TXPWR_LMT_NONE) { + if (regd_2g != regd_5g) + RTW_WARN("channel_plan:0x%02x, regd_2g:%u, regd_5g:%u not the same\n", id, regd_2g, regd_5g); + return regd_5g; + } + return regd_2g != TXPWR_LMT_NONE ? regd_2g : regd_5g; +} + +bool rtw_chplan_is_empty(u8 id) +{ + const struct chplan_ent_t *chplan_map = &RTW_ChannelPlanMap[id]; + + if (chplan_map->chd_2g == RTW_CHD_2G_NULL + #if CONFIG_IEEE80211_BAND_5GHZ + && chplan_map->chd_5g == RTW_CHD_5G_NULL + #endif + ) + return _TRUE; + + return _FALSE; +} + +bool rtw_is_channel_plan_valid(u8 id) +{ + return id < RTW_ChannelPlanMap_size && !rtw_chplan_is_empty(id); +} + +bool rtw_regsty_is_excl_chs(struct registry_priv *regsty, u8 ch) +{ + int i; + + for (i = 0; i < MAX_CHANNEL_NUM; i++) { + if (regsty->excl_chs[i] == 0) + break; + if (regsty->excl_chs[i] == ch) + return _TRUE; + } + return _FALSE; +} + +const char *_regd_src_str[] = { + [REGD_SRC_RTK_PRIV] = "RTK_PRIV", + [REGD_SRC_OS] = "OS", + [REGD_SRC_NUM] = "UNKNOWN", +}; + +static u8 init_channel_set_from_rtk_priv(_adapter *padapter, RT_CHANNEL_INFO *channel_set) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct registry_priv *regsty = adapter_to_regsty(padapter); + u8 ChannelPlan = rfctl->ChannelPlan; + u8 index, chanset_size = 0; + u8 b5GBand = _FALSE, b2_4GBand = _FALSE; + u8 ch, attrib; +#ifdef CONFIG_DFS_MASTER + int i; +#endif + + if (!rtw_is_channel_plan_valid(ChannelPlan)) { + RTW_ERR("ChannelPlan ID 0x%02X error !!!!!\n", ChannelPlan); + return chanset_size; + } + + _rtw_memset(channel_set, 0, sizeof(RT_CHANNEL_INFO) * MAX_CHANNEL_NUM); + + if (IsSupported24G(regsty->wireless_mode) && hal_chk_band_cap(padapter, BAND_CAP_2G)) + b2_4GBand = _TRUE; + + if (is_supported_5g(regsty->wireless_mode) && hal_chk_band_cap(padapter, BAND_CAP_5G)) + b5GBand = _TRUE; + + if (b2_4GBand == _FALSE && b5GBand == _FALSE) { + RTW_WARN("HW band_cap has no intersection with SW wireless_mode setting\n"); + return chanset_size; + } + + if (b2_4GBand) { + u8 chd_2g = RTW_ChannelPlanMap[ChannelPlan].chd_2g; + + attrib = CH_LIST_ATTRIB(rtw_channel_def_2g[chd_2g]); + + for (index = 0; index < CH_LIST_LEN(rtw_channel_def_2g[chd_2g]); index++) { + ch = CH_LIST_CH(rtw_channel_def_2g[chd_2g], index); + if (rtw_regsty_is_excl_chs(regsty, ch) == _TRUE) + continue; + + if (chanset_size >= MAX_CHANNEL_NUM) { + RTW_WARN("chset size can't exceed MAX_CHANNEL_NUM(%u)\n", MAX_CHANNEL_NUM); + break; + } + + channel_set[chanset_size].ChannelNum = ch; + + if (ch >= 12 && ch <= 14 && (attrib & CLA_2G_12_14_PASSIVE)) + channel_set[chanset_size].flags |= RTW_CHF_NO_IR; + + if (channel_set[chanset_size].flags & RTW_CHF_NO_IR) { + if (rfctl->country_ent || ch <= 11) + RTW_INFO("ch%u is PASSIVE\n", ch); + } + + chanset_size++; + } + } + +#if CONFIG_IEEE80211_BAND_5GHZ + if (b5GBand) { + bool dfs; + u8 chd_5g = RTW_ChannelPlanMap[ChannelPlan].chd_5g; + + attrib = CH_LIST_ATTRIB(rtw_channel_def_5g[chd_5g]); + + for (index = 0; index < CH_LIST_LEN(rtw_channel_def_5g[chd_5g]); index++) { + ch = CH_LIST_CH(rtw_channel_def_5g[chd_5g], index); + if (rtw_regsty_is_excl_chs(regsty, ch) == _TRUE) + continue; + dfs = (rtw_is_5g_band2(ch) && (attrib & CLA_5G_B2_DFS)) + || (rtw_is_5g_band3(ch) && (attrib & CLA_5G_B3_DFS)) + || (rtw_is_5g_band4(ch) && (attrib & CLA_5G_B4_DFS)); + #if !CONFIG_DFS + if (dfs) + continue; + #endif + + if (chanset_size >= MAX_CHANNEL_NUM) { + RTW_WARN("chset size can't exceed MAX_CHANNEL_NUM(%u)\n", MAX_CHANNEL_NUM); + break; + } + + channel_set[chanset_size].ChannelNum = ch; + + if ((rtw_is_5g_band1(ch) && (attrib & CLA_5G_B1_PASSIVE)) /* band1 passive */ + || (rtw_is_5g_band2(ch) && (attrib & CLA_5G_B2_PASSIVE)) /* band2 passive */ + || (rtw_is_5g_band3(ch) && (attrib & CLA_5G_B3_PASSIVE)) /* band3 passive */ + || (rtw_is_5g_band4(ch) && (attrib & CLA_5G_B4_PASSIVE)) /* band4 passive */ + ) + channel_set[chanset_size].flags |= RTW_CHF_NO_IR; + + if (dfs) + channel_set[chanset_size].flags |= RTW_CHF_DFS; + + if (channel_set[chanset_size].flags & RTW_CHF_NO_IR) { + if (rfctl->country_ent || (channel_set[chanset_size].flags & RTW_CHF_DFS)) + RTW_INFO("ch%u is PASSIVE%s\n", ch, dfs ? " DFS" : ""); + } + + chanset_size++; + } + } + + #ifdef CONFIG_DFS_MASTER + for (i = 0; i < chanset_size; i++) + channel_set[i].non_ocp_end_time = rtw_get_current_time(); + #endif +#endif /* CONFIG_IEEE80211_BAND_5GHZ */ + + if (chanset_size) + RTW_INFO(FUNC_ADPT_FMT" ChannelPlan ID:0x%02x, ch num:%d\n" + , FUNC_ADPT_ARG(padapter), ChannelPlan, chanset_size); + else + RTW_WARN(FUNC_ADPT_FMT" ChannelPlan ID:0x%02x, final chset has no channel\n" + , FUNC_ADPT_ARG(padapter), ChannelPlan); + + return chanset_size; +} + +u8 init_channel_set(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + + if (rfctl->regd_src == REGD_SRC_RTK_PRIV) + return init_channel_set_from_rtk_priv(adapter, rfctl->channel_set); +#ifdef CONFIG_REGD_SRC_FROM_OS + else if (rfctl->regd_src == REGD_SRC_OS) + return rtw_os_init_channel_set(adapter, rfctl->channel_set); +#endif + else + rtw_warn_on(1); + + return 0; +} + +bool rtw_chset_is_dfs_range(struct _RT_CHANNEL_INFO *chset, u32 hi, u32 lo) +{ + u8 hi_ch = rtw_freq2ch(hi); + u8 lo_ch = rtw_freq2ch(lo); + int i; + + for (i = 0; i < MAX_CHANNEL_NUM && chset[i].ChannelNum != 0; i++){ + if (!(chset[i].flags & RTW_CHF_DFS)) + continue; + if (hi_ch > chset[i].ChannelNum && lo_ch < chset[i].ChannelNum) + return 1; + } + + return 0; +} + +bool rtw_chset_is_dfs_ch(struct _RT_CHANNEL_INFO *chset, u8 ch) +{ + int i; + + for (i = 0; i < MAX_CHANNEL_NUM && chset[i].ChannelNum != 0; i++){ + if (chset[i].ChannelNum == ch) + return chset[i].flags & RTW_CHF_DFS ? 1 : 0; + } + + return 0; +} + +bool rtw_chset_is_dfs_chbw(struct _RT_CHANNEL_INFO *chset, u8 ch, u8 bw, u8 offset) +{ + u32 hi, lo; + + if (!rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo)) + return 0; + + return rtw_chset_is_dfs_range(chset, hi, lo); +} + +u8 rtw_process_beacon_hint(_adapter *adapter, WLAN_BSSID_EX *bss) +{ +#ifndef RTW_CHPLAN_BEACON_HINT_NON_WORLD_WIDE +#define RTW_CHPLAN_BEACON_HINT_NON_WORLD_WIDE 0 +#endif + +#ifndef RTW_CHPLAN_BEACON_HINT_ON_2G_CH_1_11 +#define RTW_CHPLAN_BEACON_HINT_ON_2G_CH_1_11 0 +#endif + +#ifndef RTW_CHPLAN_BEACON_HINT_ON_DFS_CH +#define RTW_CHPLAN_BEACON_HINT_ON_DFS_CH 0 +#endif + + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + u8 ch = bss->Configuration.DSConfig; + int chset_idx = rtw_chset_search_ch(chset, ch); + u8 act_cnt = 0; + + if (chset_idx < 0) + goto exit; + + if ((chset[chset_idx].flags & RTW_CHF_NO_IR) + && (RTW_CHPLAN_BEACON_HINT_NON_WORLD_WIDE || !rfctl->country_ent || IS_ALPHA2_WORLDWIDE(rfctl->country_ent->alpha2)) + && (RTW_CHPLAN_BEACON_HINT_ON_2G_CH_1_11 || !(ch <= 11)) + && (RTW_CHPLAN_BEACON_HINT_ON_DFS_CH || !(chset[chset_idx].flags & RTW_CHF_DFS)) + ) { + RTW_INFO("%s: change ch:%d to active\n", __func__, ch); + chset[chset_idx].flags &= ~RTW_CHF_NO_IR; + act_cnt++; + } + +exit: + return act_cnt; +} + +const char *_rtw_dfs_regd_str[] = { + [RTW_DFS_REGD_NONE] = "NONE", + [RTW_DFS_REGD_FCC] = "FCC", + [RTW_DFS_REGD_MKK] = "MKK", + [RTW_DFS_REGD_ETSI] = "ETSI", +}; + +#ifdef CONFIG_80211AC_VHT +#define COUNTRY_CHPLAN_ASSIGN_EN_11AC(_val) , .en_11ac = (_val) +#else +#define COUNTRY_CHPLAN_ASSIGN_EN_11AC(_val) +#endif + +#define COUNTRY_CHPLAN_ENT(_alpha2, _chplan, _en_11ac) \ + {.alpha2 = (_alpha2), .chplan = (_chplan) \ + COUNTRY_CHPLAN_ASSIGN_EN_11AC(_en_11ac) \ + } + +#ifdef CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP + +#include "../platform/custom_country_chplan.h" + +#elif RTW_DEF_MODULE_REGULATORY_CERT + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8821AE_HMC_M2) /* 2013 certify */ +static const struct country_chplan RTL8821AE_HMC_M2_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x34, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x30, 1), + COUNTRY_CHPLAN_ENT("CN", 0x51, 1), + COUNTRY_CHPLAN_ENT("CO", 0x34, 1), + COUNTRY_CHPLAN_ENT("CR", 0x34, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x34, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x34, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x34, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x3D, 0), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x34, 1), + COUNTRY_CHPLAN_ENT("MY", 0x47, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x34, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x34, 1), + COUNTRY_CHPLAN_ENT("PE", 0x34, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x34, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x34, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("UA", 0x36, 0), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8821AU) /* 2014 certify */ +static const struct country_chplan RTL8821AU_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x34, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x3D, 0), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 0), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("UA", 0x36, 0), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8812AENF_NGFF) /* 2014 certify */ +static const struct country_chplan RTL8812AENF_NGFF_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8812AEBT_HMC) /* 2013 certify */ +static const struct country_chplan RTL8812AEBT_HMC_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x34, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 0), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("UA", 0x36, 0), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8188EE_HMC_M2) /* 2012 certify */ +static const struct country_chplan RTL8188EE_HMC_M2_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AW", 0x34, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BB", 0x34, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BN", 0x47, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x20, 1), + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x34, 1), + COUNTRY_CHPLAN_ENT("CR", 0x34, 1), + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x34, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x34, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("ET", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GD", 0x76, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), + COUNTRY_CHPLAN_ENT("GN", 0x26, 1), + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x34, 1), + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HT", 0x34, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x5D, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x34, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x34, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x34, 1), + COUNTRY_CHPLAN_ENT("PE", 0x34, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x34, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x34, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("RW", 0x26, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SC", 0x34, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VC", 0x34, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8723BE_HMC_M2) /* 2013 certify */ +static const struct country_chplan RTL8723BE_HMC_M2_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AW", 0x34, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BS", 0x34, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x20, 1), + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x34, 1), + COUNTRY_CHPLAN_ENT("CR", 0x34, 1), + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x34, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x34, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("ET", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GD", 0x76, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x34, 1), + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x5D, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MG", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), + COUNTRY_CHPLAN_ENT("MR", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x34, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x34, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x34, 1), + COUNTRY_CHPLAN_ENT("PE", 0x34, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x34, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x34, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("RW", 0x26, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("SZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8723BS_NGFF1216) /* 2014 certify */ +static const struct country_chplan RTL8723BS_NGFF1216_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BB", 0x34, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x20, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x34, 1), + COUNTRY_CHPLAN_ENT("CR", 0x34, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x34, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x34, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x34, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HT", 0x34, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x5D, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("MQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x34, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x34, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x34, 1), + COUNTRY_CHPLAN_ENT("PE", 0x34, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x34, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x34, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("YE", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8192EEBT_HMC_M2) /* 2013 certify */ +static const struct country_chplan RTL8192EEBT_HMC_M2_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AW", 0x34, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x20, 1), + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x34, 1), + COUNTRY_CHPLAN_ENT("CR", 0x34, 1), + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x34, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x34, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("EH", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("ET", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GD", 0x76, 1), + COUNTRY_CHPLAN_ENT("GF", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x34, 1), + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x5D, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), + COUNTRY_CHPLAN_ENT("MR", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x34, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x34, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x34, 1), + COUNTRY_CHPLAN_ENT("PE", 0x34, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x34, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x34, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("RW", 0x26, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SC", 0x34, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("ST", 0x34, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), + COUNTRY_CHPLAN_ENT("TF", 0x26, 1), + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x39, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x34, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("YT", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8723DE_NGFF1630) /* 2016 certify */ +static const struct country_chplan RTL8723DE_NGFF1630_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x2A, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x76, 1), + COUNTRY_CHPLAN_ENT("CR", 0x76, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x76, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x76, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x61, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x5D, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("KZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x34, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NA", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x76, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x76, 1), + COUNTRY_CHPLAN_ENT("PE", 0x76, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x76, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x76, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x76, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("US", 0x76, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8822BE) /* 2016 certify */ +static const struct country_chplan RTL8822BE_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BB", 0x76, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BM", 0x76, 1), + COUNTRY_CHPLAN_ENT("BN", 0x47, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BS", 0x76, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x2B, 1), + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x76, 1), + COUNTRY_CHPLAN_ENT("CR", 0x76, 1), + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x76, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x76, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("EH", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FJ", 0x76, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GE", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GI", 0x26, 1), + COUNTRY_CHPLAN_ENT("GL", 0x26, 1), + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), + COUNTRY_CHPLAN_ENT("GN", 0x26, 1), + COUNTRY_CHPLAN_ENT("GP", 0x26, 1), + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x61, 1), + COUNTRY_CHPLAN_ENT("GU", 0x76, 1), + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HT", 0x76, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x3D, 0), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("KZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MG", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), + COUNTRY_CHPLAN_ENT("MO", 0x35, 1), + COUNTRY_CHPLAN_ENT("MQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("MR", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x4D, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NA", 0x26, 1), + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x76, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x76, 1), + COUNTRY_CHPLAN_ENT("PE", 0x76, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x76, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x76, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SC", 0x76, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("ST", 0x76, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), + COUNTRY_CHPLAN_ENT("TF", 0x26, 1), + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), + COUNTRY_CHPLAN_ENT("TW", 0x76, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x76, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("YT", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8821CE) /* 2016 certify */ +static const struct country_chplan RTL8821CE_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BB", 0x76, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BM", 0x76, 1), + COUNTRY_CHPLAN_ENT("BN", 0x47, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BS", 0x76, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x2B, 1), + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x76, 1), + COUNTRY_CHPLAN_ENT("CR", 0x76, 1), + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x76, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x76, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("EH", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("ET", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FJ", 0x76, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GE", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GI", 0x26, 1), + COUNTRY_CHPLAN_ENT("GL", 0x26, 1), + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), + COUNTRY_CHPLAN_ENT("GN", 0x26, 1), + COUNTRY_CHPLAN_ENT("GP", 0x26, 1), + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x61, 1), + COUNTRY_CHPLAN_ENT("GU", 0x76, 1), + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HT", 0x76, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("ID", 0x3D, 0), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x28, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("KZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MG", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), + COUNTRY_CHPLAN_ENT("MO", 0x35, 1), + COUNTRY_CHPLAN_ENT("MQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("MR", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x4D, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NA", 0x26, 1), + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x76, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x76, 1), + COUNTRY_CHPLAN_ENT("PE", 0x76, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x76, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x76, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SC", 0x76, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("ST", 0x76, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), + COUNTRY_CHPLAN_ENT("TF", 0x26, 1), + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TW", 0x76, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x76, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("YT", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8822CE) /* 2018 certify */ +static const struct country_chplan RTL8822CE_country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), + COUNTRY_CHPLAN_ENT("AW", 0x76, 1), + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), + COUNTRY_CHPLAN_ENT("BB", 0x76, 1), + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("BM", 0x76, 1), + COUNTRY_CHPLAN_ENT("BN", 0x47, 1), + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), + COUNTRY_CHPLAN_ENT("BS", 0x76, 1), + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CA", 0x2B, 1), + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), + COUNTRY_CHPLAN_ENT("CO", 0x76, 1), + COUNTRY_CHPLAN_ENT("CR", 0x76, 1), + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), + COUNTRY_CHPLAN_ENT("DJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), + COUNTRY_CHPLAN_ENT("DO", 0x76, 1), + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), + COUNTRY_CHPLAN_ENT("EC", 0x76, 1), + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), + COUNTRY_CHPLAN_ENT("EH", 0x47, 1), + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), + COUNTRY_CHPLAN_ENT("ET", 0x26, 1), + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), + COUNTRY_CHPLAN_ENT("FJ", 0x76, 1), + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), + COUNTRY_CHPLAN_ENT("GE", 0x26, 1), + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), + COUNTRY_CHPLAN_ENT("GI", 0x26, 1), + COUNTRY_CHPLAN_ENT("GL", 0x26, 1), + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), + COUNTRY_CHPLAN_ENT("GN", 0x26, 1), + COUNTRY_CHPLAN_ENT("GP", 0x26, 1), + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), + COUNTRY_CHPLAN_ENT("GT", 0x61, 1), + COUNTRY_CHPLAN_ENT("GU", 0x76, 1), + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), + COUNTRY_CHPLAN_ENT("HT", 0x76, 1), + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), + COUNTRY_CHPLAN_ENT("KM", 0x26, 1), + COUNTRY_CHPLAN_ENT("KR", 0x4B, 1), + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), + COUNTRY_CHPLAN_ENT("KZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), + COUNTRY_CHPLAN_ENT("MG", 0x26, 1), + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), + COUNTRY_CHPLAN_ENT("MO", 0x35, 1), + COUNTRY_CHPLAN_ENT("MQ", 0x26, 1), + COUNTRY_CHPLAN_ENT("MR", 0x26, 1), + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), + COUNTRY_CHPLAN_ENT("MX", 0x4D, 1), + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("NA", 0x26, 1), + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), + COUNTRY_CHPLAN_ENT("NI", 0x76, 1), + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), + COUNTRY_CHPLAN_ENT("PA", 0x76, 1), + COUNTRY_CHPLAN_ENT("PE", 0x76, 1), + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), + COUNTRY_CHPLAN_ENT("PR", 0x76, 1), + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), + COUNTRY_CHPLAN_ENT("PY", 0x76, 1), + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), + COUNTRY_CHPLAN_ENT("SC", 0x76, 1), + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), + COUNTRY_CHPLAN_ENT("ST", 0x76, 1), + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), + COUNTRY_CHPLAN_ENT("SZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), + COUNTRY_CHPLAN_ENT("TF", 0x26, 1), + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), + COUNTRY_CHPLAN_ENT("TJ", 0x26, 1), + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), + COUNTRY_CHPLAN_ENT("TW", 0x76, 1), + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), + COUNTRY_CHPLAN_ENT("US", 0x76, 1), + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), + COUNTRY_CHPLAN_ENT("YT", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), +}; +#endif + +/** + * rtw_def_module_country_chplan_map - + * @hal_map: returned map + * @return: size of map + */ +static u16 rtw_def_module_country_chplan_map(const struct country_chplan **hal_map) +{ + u16 hal_map_sz = 0; + + /* TODO: runtime selection for multi driver */ +#if (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8821AE_HMC_M2) + *hal_map = RTL8821AE_HMC_M2_country_chplan_map; + hal_map_sz = sizeof(RTL8821AE_HMC_M2_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8821AU) + *hal_map = RTL8821AU_country_chplan_map; + hal_map_sz = sizeof(RTL8821AU_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8812AENF_NGFF) + *hal_map = RTL8812AENF_NGFF_country_chplan_map; + hal_map_sz = sizeof(RTL8812AENF_NGFF_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8812AEBT_HMC) + *hal_map = RTL8812AEBT_HMC_country_chplan_map; + hal_map_sz = sizeof(RTL8812AEBT_HMC_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8188EE_HMC_M2) + *hal_map = RTL8188EE_HMC_M2_country_chplan_map; + hal_map_sz = sizeof(RTL8188EE_HMC_M2_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8723BE_HMC_M2) + *hal_map = RTL8723BE_HMC_M2_country_chplan_map; + hal_map_sz = sizeof(RTL8723BE_HMC_M2_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8723BS_NGFF1216) + *hal_map = RTL8723BS_NGFF1216_country_chplan_map; + hal_map_sz = sizeof(RTL8723BS_NGFF1216_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8192EEBT_HMC_M2) + *hal_map = RTL8192EEBT_HMC_M2_country_chplan_map; + hal_map_sz = sizeof(RTL8192EEBT_HMC_M2_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8723DE_NGFF1630) + *hal_map = RTL8723DE_NGFF1630_country_chplan_map; + hal_map_sz = sizeof(RTL8723DE_NGFF1630_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8822BE) + *hal_map = RTL8822BE_country_chplan_map; + hal_map_sz = sizeof(RTL8822BE_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8821CE) + *hal_map = RTL8821CE_country_chplan_map; + hal_map_sz = sizeof(RTL8821CE_country_chplan_map) / sizeof(struct country_chplan); +#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8822CE) + *hal_map = RTL8822CE_country_chplan_map; + hal_map_sz = sizeof(RTL8822CE_country_chplan_map) / sizeof(struct country_chplan); +#endif + + return hal_map_sz; +} +#else + +static const struct country_chplan country_chplan_map[] = { + COUNTRY_CHPLAN_ENT("AD", 0x26, 1), /* Andorra */ + COUNTRY_CHPLAN_ENT("AE", 0x35, 1), /* United Arab Emirates */ + COUNTRY_CHPLAN_ENT("AF", 0x42, 1), /* Afghanistan */ + COUNTRY_CHPLAN_ENT("AG", 0x76, 1), /* Antigua & Barbuda */ + COUNTRY_CHPLAN_ENT("AI", 0x26, 1), /* Anguilla(UK) */ + COUNTRY_CHPLAN_ENT("AL", 0x26, 1), /* Albania */ + COUNTRY_CHPLAN_ENT("AM", 0x26, 1), /* Armenia */ + COUNTRY_CHPLAN_ENT("AN", 0x76, 1), /* Netherlands Antilles */ + COUNTRY_CHPLAN_ENT("AO", 0x47, 1), /* Angola */ + COUNTRY_CHPLAN_ENT("AQ", 0x26, 1), /* Antarctica */ + COUNTRY_CHPLAN_ENT("AR", 0x61, 1), /* Argentina */ + COUNTRY_CHPLAN_ENT("AS", 0x76, 1), /* American Samoa */ + COUNTRY_CHPLAN_ENT("AT", 0x26, 1), /* Austria */ + COUNTRY_CHPLAN_ENT("AU", 0x45, 1), /* Australia */ + COUNTRY_CHPLAN_ENT("AW", 0x76, 1), /* Aruba */ + COUNTRY_CHPLAN_ENT("AZ", 0x26, 1), /* Azerbaijan */ + COUNTRY_CHPLAN_ENT("BA", 0x26, 1), /* Bosnia & Herzegovina */ + COUNTRY_CHPLAN_ENT("BB", 0x76, 1), /* Barbados */ + COUNTRY_CHPLAN_ENT("BD", 0x26, 1), /* Bangladesh */ + COUNTRY_CHPLAN_ENT("BE", 0x26, 1), /* Belgium */ + COUNTRY_CHPLAN_ENT("BF", 0x26, 1), /* Burkina Faso */ + COUNTRY_CHPLAN_ENT("BG", 0x26, 1), /* Bulgaria */ + COUNTRY_CHPLAN_ENT("BH", 0x48, 1), /* Bahrain */ + COUNTRY_CHPLAN_ENT("BI", 0x26, 1), /* Burundi */ + COUNTRY_CHPLAN_ENT("BJ", 0x26, 1), /* Benin */ + COUNTRY_CHPLAN_ENT("BM", 0x76, 1), /* Bermuda (UK) */ + COUNTRY_CHPLAN_ENT("BN", 0x47, 1), /* Brunei */ + COUNTRY_CHPLAN_ENT("BO", 0x73, 1), /* Bolivia */ + COUNTRY_CHPLAN_ENT("BR", 0x62, 1), /* Brazil */ + COUNTRY_CHPLAN_ENT("BS", 0x76, 1), /* Bahamas */ + COUNTRY_CHPLAN_ENT("BT", 0x26, 1), /* Bhutan */ + COUNTRY_CHPLAN_ENT("BV", 0x26, 1), /* Bouvet Island (Norway) */ + COUNTRY_CHPLAN_ENT("BW", 0x35, 1), /* Botswana */ + COUNTRY_CHPLAN_ENT("BY", 0x26, 1), /* Belarus */ + COUNTRY_CHPLAN_ENT("BZ", 0x76, 1), /* Belize */ + COUNTRY_CHPLAN_ENT("CA", 0x2B, 1), /* Canada */ + COUNTRY_CHPLAN_ENT("CC", 0x26, 1), /* Cocos (Keeling) Islands (Australia) */ + COUNTRY_CHPLAN_ENT("CD", 0x26, 1), /* Congo, Republic of the */ + COUNTRY_CHPLAN_ENT("CF", 0x26, 1), /* Central African Republic */ + COUNTRY_CHPLAN_ENT("CG", 0x26, 1), /* Congo, Democratic Republic of the. Zaire */ + COUNTRY_CHPLAN_ENT("CH", 0x26, 1), /* Switzerland */ + COUNTRY_CHPLAN_ENT("CI", 0x42, 1), /* Cote d'Ivoire */ + COUNTRY_CHPLAN_ENT("CK", 0x26, 1), /* Cook Islands */ + COUNTRY_CHPLAN_ENT("CL", 0x2D, 1), /* Chile */ + COUNTRY_CHPLAN_ENT("CM", 0x26, 1), /* Cameroon */ + COUNTRY_CHPLAN_ENT("CN", 0x48, 1), /* China */ + COUNTRY_CHPLAN_ENT("CO", 0x76, 1), /* Colombia */ + COUNTRY_CHPLAN_ENT("CR", 0x76, 1), /* Costa Rica */ + COUNTRY_CHPLAN_ENT("CV", 0x26, 1), /* Cape Verde */ + COUNTRY_CHPLAN_ENT("CX", 0x45, 1), /* Christmas Island (Australia) */ + COUNTRY_CHPLAN_ENT("CY", 0x26, 1), /* Cyprus */ + COUNTRY_CHPLAN_ENT("CZ", 0x26, 1), /* Czech Republic */ + COUNTRY_CHPLAN_ENT("DE", 0x26, 1), /* Germany */ + COUNTRY_CHPLAN_ENT("DJ", 0x26, 1), /* Djibouti */ + COUNTRY_CHPLAN_ENT("DK", 0x26, 1), /* Denmark */ + COUNTRY_CHPLAN_ENT("DM", 0x76, 1), /* Dominica */ + COUNTRY_CHPLAN_ENT("DO", 0x76, 1), /* Dominican Republic */ + COUNTRY_CHPLAN_ENT("DZ", 0x00, 1), /* Algeria */ + COUNTRY_CHPLAN_ENT("EC", 0x76, 1), /* Ecuador */ + COUNTRY_CHPLAN_ENT("EE", 0x26, 1), /* Estonia */ + COUNTRY_CHPLAN_ENT("EG", 0x47, 1), /* Egypt */ + COUNTRY_CHPLAN_ENT("EH", 0x47, 1), /* Western Sahara */ + COUNTRY_CHPLAN_ENT("ER", 0x26, 1), /* Eritrea */ + COUNTRY_CHPLAN_ENT("ES", 0x26, 1), /* Spain, Canary Islands, Ceuta, Melilla */ + COUNTRY_CHPLAN_ENT("ET", 0x26, 1), /* Ethiopia */ + COUNTRY_CHPLAN_ENT("FI", 0x26, 1), /* Finland */ + COUNTRY_CHPLAN_ENT("FJ", 0x76, 1), /* Fiji */ + COUNTRY_CHPLAN_ENT("FK", 0x26, 1), /* Falkland Islands (Islas Malvinas) (UK) */ + COUNTRY_CHPLAN_ENT("FM", 0x76, 1), /* Micronesia, Federated States of (USA) */ + COUNTRY_CHPLAN_ENT("FO", 0x26, 1), /* Faroe Islands (Denmark) */ + COUNTRY_CHPLAN_ENT("FR", 0x26, 1), /* France */ + COUNTRY_CHPLAN_ENT("GA", 0x26, 1), /* Gabon */ + COUNTRY_CHPLAN_ENT("GB", 0x26, 1), /* Great Britain (United Kingdom; England) */ + COUNTRY_CHPLAN_ENT("GD", 0x76, 1), /* Grenada */ + COUNTRY_CHPLAN_ENT("GE", 0x26, 1), /* Georgia */ + COUNTRY_CHPLAN_ENT("GF", 0x26, 1), /* French Guiana */ + COUNTRY_CHPLAN_ENT("GG", 0x26, 1), /* Guernsey (UK) */ + COUNTRY_CHPLAN_ENT("GH", 0x26, 1), /* Ghana */ + COUNTRY_CHPLAN_ENT("GI", 0x26, 1), /* Gibraltar (UK) */ + COUNTRY_CHPLAN_ENT("GL", 0x26, 1), /* Greenland (Denmark) */ + COUNTRY_CHPLAN_ENT("GM", 0x26, 1), /* Gambia */ + COUNTRY_CHPLAN_ENT("GN", 0x26, 1), /* Guinea */ + COUNTRY_CHPLAN_ENT("GP", 0x26, 1), /* Guadeloupe (France) */ + COUNTRY_CHPLAN_ENT("GQ", 0x26, 1), /* Equatorial Guinea */ + COUNTRY_CHPLAN_ENT("GR", 0x26, 1), /* Greece */ + COUNTRY_CHPLAN_ENT("GS", 0x26, 1), /* South Georgia and the Sandwich Islands (UK) */ + COUNTRY_CHPLAN_ENT("GT", 0x61, 1), /* Guatemala */ + COUNTRY_CHPLAN_ENT("GU", 0x76, 1), /* Guam (USA) */ + COUNTRY_CHPLAN_ENT("GW", 0x26, 1), /* Guinea-Bissau */ + COUNTRY_CHPLAN_ENT("GY", 0x44, 1), /* Guyana */ + COUNTRY_CHPLAN_ENT("HK", 0x35, 1), /* Hong Kong */ + COUNTRY_CHPLAN_ENT("HM", 0x45, 1), /* Heard and McDonald Islands (Australia) */ + COUNTRY_CHPLAN_ENT("HN", 0x32, 1), /* Honduras */ + COUNTRY_CHPLAN_ENT("HR", 0x26, 1), /* Croatia */ + COUNTRY_CHPLAN_ENT("HT", 0x76, 1), /* Haiti */ + COUNTRY_CHPLAN_ENT("HU", 0x26, 1), /* Hungary */ + COUNTRY_CHPLAN_ENT("ID", 0x5D, 1), /* Indonesia */ + COUNTRY_CHPLAN_ENT("IE", 0x26, 1), /* Ireland */ + COUNTRY_CHPLAN_ENT("IL", 0x47, 1), /* Israel */ + COUNTRY_CHPLAN_ENT("IM", 0x26, 1), /* Isle of Man (UK) */ + COUNTRY_CHPLAN_ENT("IN", 0x48, 1), /* India */ + COUNTRY_CHPLAN_ENT("IO", 0x26, 1), /* British Indian Ocean Territory (UK) */ + COUNTRY_CHPLAN_ENT("IQ", 0x26, 1), /* Iraq */ + COUNTRY_CHPLAN_ENT("IR", 0x26, 0), /* Iran */ + COUNTRY_CHPLAN_ENT("IS", 0x26, 1), /* Iceland */ + COUNTRY_CHPLAN_ENT("IT", 0x26, 1), /* Italy */ + COUNTRY_CHPLAN_ENT("JE", 0x26, 1), /* Jersey (UK) */ + COUNTRY_CHPLAN_ENT("JM", 0x32, 1), /* Jamaica */ + COUNTRY_CHPLAN_ENT("JO", 0x49, 1), /* Jordan */ + COUNTRY_CHPLAN_ENT("JP", 0x27, 1), /* Japan- Telec */ + COUNTRY_CHPLAN_ENT("KE", 0x47, 1), /* Kenya */ + COUNTRY_CHPLAN_ENT("KG", 0x26, 1), /* Kyrgyzstan */ + COUNTRY_CHPLAN_ENT("KH", 0x26, 1), /* Cambodia */ + COUNTRY_CHPLAN_ENT("KI", 0x26, 1), /* Kiribati */ + COUNTRY_CHPLAN_ENT("KM", 0x26, 1), /* Comoros */ + COUNTRY_CHPLAN_ENT("KN", 0x76, 1), /* Saint Kitts and Nevis */ + COUNTRY_CHPLAN_ENT("KR", 0x4B, 1), /* South Korea */ + COUNTRY_CHPLAN_ENT("KW", 0x26, 1), /* Kuwait */ + COUNTRY_CHPLAN_ENT("KY", 0x76, 1), /* Cayman Islands (UK) */ + COUNTRY_CHPLAN_ENT("KZ", 0x26, 1), /* Kazakhstan */ + COUNTRY_CHPLAN_ENT("LA", 0x26, 1), /* Laos */ + COUNTRY_CHPLAN_ENT("LB", 0x26, 1), /* Lebanon */ + COUNTRY_CHPLAN_ENT("LC", 0x76, 1), /* Saint Lucia */ + COUNTRY_CHPLAN_ENT("LI", 0x26, 1), /* Liechtenstein */ + COUNTRY_CHPLAN_ENT("LK", 0x26, 1), /* Sri Lanka */ + COUNTRY_CHPLAN_ENT("LR", 0x26, 1), /* Liberia */ + COUNTRY_CHPLAN_ENT("LS", 0x26, 1), /* Lesotho */ + COUNTRY_CHPLAN_ENT("LT", 0x26, 1), /* Lithuania */ + COUNTRY_CHPLAN_ENT("LU", 0x26, 1), /* Luxembourg */ + COUNTRY_CHPLAN_ENT("LV", 0x26, 1), /* Latvia */ + COUNTRY_CHPLAN_ENT("LY", 0x26, 1), /* Libya */ + COUNTRY_CHPLAN_ENT("MA", 0x47, 1), /* Morocco */ + COUNTRY_CHPLAN_ENT("MC", 0x26, 1), /* Monaco */ + COUNTRY_CHPLAN_ENT("MD", 0x26, 1), /* Moldova */ + COUNTRY_CHPLAN_ENT("ME", 0x26, 1), /* Montenegro */ + COUNTRY_CHPLAN_ENT("MF", 0x76, 1), /* Saint Martin */ + COUNTRY_CHPLAN_ENT("MG", 0x26, 1), /* Madagascar */ + COUNTRY_CHPLAN_ENT("MH", 0x76, 1), /* Marshall Islands (USA) */ + COUNTRY_CHPLAN_ENT("MK", 0x26, 1), /* Republic of Macedonia (FYROM) */ + COUNTRY_CHPLAN_ENT("ML", 0x26, 1), /* Mali */ + COUNTRY_CHPLAN_ENT("MM", 0x26, 1), /* Burma (Myanmar) */ + COUNTRY_CHPLAN_ENT("MN", 0x26, 1), /* Mongolia */ + COUNTRY_CHPLAN_ENT("MO", 0x35, 1), /* Macau */ + COUNTRY_CHPLAN_ENT("MP", 0x76, 1), /* Northern Mariana Islands (USA) */ + COUNTRY_CHPLAN_ENT("MQ", 0x26, 1), /* Martinique (France) */ + COUNTRY_CHPLAN_ENT("MR", 0x26, 1), /* Mauritania */ + COUNTRY_CHPLAN_ENT("MS", 0x26, 1), /* Montserrat (UK) */ + COUNTRY_CHPLAN_ENT("MT", 0x26, 1), /* Malta */ + COUNTRY_CHPLAN_ENT("MU", 0x26, 1), /* Mauritius */ + COUNTRY_CHPLAN_ENT("MV", 0x47, 1), /* Maldives */ + COUNTRY_CHPLAN_ENT("MW", 0x26, 1), /* Malawi */ + COUNTRY_CHPLAN_ENT("MX", 0x4D, 1), /* Mexico */ + COUNTRY_CHPLAN_ENT("MY", 0x63, 1), /* Malaysia */ + COUNTRY_CHPLAN_ENT("MZ", 0x26, 1), /* Mozambique */ + COUNTRY_CHPLAN_ENT("NA", 0x26, 1), /* Namibia */ + COUNTRY_CHPLAN_ENT("NC", 0x26, 1), /* New Caledonia */ + COUNTRY_CHPLAN_ENT("NE", 0x26, 1), /* Niger */ + COUNTRY_CHPLAN_ENT("NF", 0x45, 1), /* Norfolk Island (Australia) */ + COUNTRY_CHPLAN_ENT("NG", 0x75, 1), /* Nigeria */ + COUNTRY_CHPLAN_ENT("NI", 0x76, 1), /* Nicaragua */ + COUNTRY_CHPLAN_ENT("NL", 0x26, 1), /* Netherlands */ + COUNTRY_CHPLAN_ENT("NO", 0x26, 1), /* Norway */ + COUNTRY_CHPLAN_ENT("NP", 0x48, 1), /* Nepal */ + COUNTRY_CHPLAN_ENT("NR", 0x26, 1), /* Nauru */ + COUNTRY_CHPLAN_ENT("NU", 0x45, 1), /* Niue */ + COUNTRY_CHPLAN_ENT("NZ", 0x45, 1), /* New Zealand */ + COUNTRY_CHPLAN_ENT("OM", 0x26, 1), /* Oman */ + COUNTRY_CHPLAN_ENT("PA", 0x76, 1), /* Panama */ + COUNTRY_CHPLAN_ENT("PE", 0x76, 1), /* Peru */ + COUNTRY_CHPLAN_ENT("PF", 0x26, 1), /* French Polynesia (France) */ + COUNTRY_CHPLAN_ENT("PG", 0x35, 1), /* Papua New Guinea */ + COUNTRY_CHPLAN_ENT("PH", 0x35, 1), /* Philippines */ + COUNTRY_CHPLAN_ENT("PK", 0x51, 1), /* Pakistan */ + COUNTRY_CHPLAN_ENT("PL", 0x26, 1), /* Poland */ + COUNTRY_CHPLAN_ENT("PM", 0x26, 1), /* Saint Pierre and Miquelon (France) */ + COUNTRY_CHPLAN_ENT("PR", 0x76, 1), /* Puerto Rico */ + COUNTRY_CHPLAN_ENT("PT", 0x26, 1), /* Portugal */ + COUNTRY_CHPLAN_ENT("PW", 0x76, 1), /* Palau */ + COUNTRY_CHPLAN_ENT("PY", 0x76, 1), /* Paraguay */ + COUNTRY_CHPLAN_ENT("QA", 0x35, 1), /* Qatar */ + COUNTRY_CHPLAN_ENT("RE", 0x26, 1), /* Reunion (France) */ + COUNTRY_CHPLAN_ENT("RO", 0x26, 1), /* Romania */ + COUNTRY_CHPLAN_ENT("RS", 0x26, 1), /* Serbia, Kosovo */ + COUNTRY_CHPLAN_ENT("RU", 0x59, 1), /* Russia(fac/gost), Kaliningrad */ + COUNTRY_CHPLAN_ENT("RW", 0x26, 1), /* Rwanda */ + COUNTRY_CHPLAN_ENT("SA", 0x35, 1), /* Saudi Arabia */ + COUNTRY_CHPLAN_ENT("SB", 0x26, 1), /* Solomon Islands */ + COUNTRY_CHPLAN_ENT("SC", 0x76, 1), /* Seychelles */ + COUNTRY_CHPLAN_ENT("SE", 0x26, 1), /* Sweden */ + COUNTRY_CHPLAN_ENT("SG", 0x35, 1), /* Singapore */ + COUNTRY_CHPLAN_ENT("SH", 0x26, 1), /* Saint Helena (UK) */ + COUNTRY_CHPLAN_ENT("SI", 0x26, 1), /* Slovenia */ + COUNTRY_CHPLAN_ENT("SJ", 0x26, 1), /* Svalbard (Norway) */ + COUNTRY_CHPLAN_ENT("SK", 0x26, 1), /* Slovakia */ + COUNTRY_CHPLAN_ENT("SL", 0x26, 1), /* Sierra Leone */ + COUNTRY_CHPLAN_ENT("SM", 0x26, 1), /* San Marino */ + COUNTRY_CHPLAN_ENT("SN", 0x26, 1), /* Senegal */ + COUNTRY_CHPLAN_ENT("SO", 0x26, 1), /* Somalia */ + COUNTRY_CHPLAN_ENT("SR", 0x74, 1), /* Suriname */ + COUNTRY_CHPLAN_ENT("ST", 0x76, 1), /* Sao Tome and Principe */ + COUNTRY_CHPLAN_ENT("SV", 0x30, 1), /* El Salvador */ + COUNTRY_CHPLAN_ENT("SX", 0x76, 1), /* Sint Marteen */ + COUNTRY_CHPLAN_ENT("SZ", 0x26, 1), /* Swaziland */ + COUNTRY_CHPLAN_ENT("TC", 0x26, 1), /* Turks and Caicos Islands (UK) */ + COUNTRY_CHPLAN_ENT("TD", 0x26, 1), /* Chad */ + COUNTRY_CHPLAN_ENT("TF", 0x26, 1), /* French Southern and Antarctic Lands (FR Southern Territories) */ + COUNTRY_CHPLAN_ENT("TG", 0x26, 1), /* Togo */ + COUNTRY_CHPLAN_ENT("TH", 0x35, 1), /* Thailand */ + COUNTRY_CHPLAN_ENT("TJ", 0x26, 1), /* Tajikistan */ + COUNTRY_CHPLAN_ENT("TK", 0x45, 1), /* Tokelau */ + COUNTRY_CHPLAN_ENT("TM", 0x26, 1), /* Turkmenistan */ + COUNTRY_CHPLAN_ENT("TN", 0x47, 1), /* Tunisia */ + COUNTRY_CHPLAN_ENT("TO", 0x26, 1), /* Tonga */ + COUNTRY_CHPLAN_ENT("TR", 0x26, 1), /* Turkey, Northern Cyprus */ + COUNTRY_CHPLAN_ENT("TT", 0x76, 1), /* Trinidad & Tobago */ + COUNTRY_CHPLAN_ENT("TV", 0x21, 0), /* Tuvalu */ + COUNTRY_CHPLAN_ENT("TW", 0x76, 1), /* Taiwan */ + COUNTRY_CHPLAN_ENT("TZ", 0x26, 1), /* Tanzania */ + COUNTRY_CHPLAN_ENT("UA", 0x35, 1), /* Ukraine */ + COUNTRY_CHPLAN_ENT("UG", 0x26, 1), /* Uganda */ + COUNTRY_CHPLAN_ENT("US", 0x76, 1), /* United States of America (USA) */ + COUNTRY_CHPLAN_ENT("UY", 0x30, 1), /* Uruguay */ + COUNTRY_CHPLAN_ENT("UZ", 0x47, 1), /* Uzbekistan */ + COUNTRY_CHPLAN_ENT("VA", 0x26, 1), /* Holy See (Vatican City) */ + COUNTRY_CHPLAN_ENT("VC", 0x76, 1), /* Saint Vincent and the Grenadines */ + COUNTRY_CHPLAN_ENT("VE", 0x30, 1), /* Venezuela */ + COUNTRY_CHPLAN_ENT("VG", 0x76, 1), /* British Virgin Islands (UK) */ + COUNTRY_CHPLAN_ENT("VI", 0x76, 1), /* United States Virgin Islands (USA) */ + COUNTRY_CHPLAN_ENT("VN", 0x35, 1), /* Vietnam */ + COUNTRY_CHPLAN_ENT("VU", 0x26, 1), /* Vanuatu */ + COUNTRY_CHPLAN_ENT("WF", 0x26, 1), /* Wallis and Futuna (France) */ + COUNTRY_CHPLAN_ENT("WS", 0x76, 1), /* Samoa */ + COUNTRY_CHPLAN_ENT("YE", 0x26, 1), /* Yemen */ + COUNTRY_CHPLAN_ENT("YT", 0x26, 1), /* Mayotte (France) */ + COUNTRY_CHPLAN_ENT("ZA", 0x35, 1), /* South Africa */ + COUNTRY_CHPLAN_ENT("ZM", 0x26, 1), /* Zambia */ + COUNTRY_CHPLAN_ENT("ZW", 0x26, 1), /* Zimbabwe */ +}; +#endif /* CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP or RTW_DEF_MODULE_REGULATORY_CERT or newest */ + +/* +* rtw_get_chplan_from_country - +* @country_code: string of country code +* +* Return pointer of struct country_chplan entry or NULL when unsupported country_code is given +*/ +const struct country_chplan *rtw_get_chplan_from_country(const char *country_code) +{ + const struct country_chplan *ent = NULL; + const struct country_chplan *map = NULL; + u16 map_sz = 0; + char code[2]; + int i; + + code[0] = alpha_to_upper(country_code[0]); + code[1] = alpha_to_upper(country_code[1]); + +#ifdef CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP + map = CUSTOMIZED_country_chplan_map; + map_sz = sizeof(CUSTOMIZED_country_chplan_map) / sizeof(struct country_chplan); +#elif RTW_DEF_MODULE_REGULATORY_CERT + map_sz = rtw_def_module_country_chplan_map(&map); +#else + map = country_chplan_map; + map_sz = sizeof(country_chplan_map) / sizeof(struct country_chplan); +#endif + + for (i = 0; i < map_sz; i++) { + if (strncmp(code, map[i].alpha2, 2) == 0) { + ent = &map[i]; + break; + } + } + + return ent; +} + +void dump_country_chplan(void *sel, const struct country_chplan *ent) +{ + char buf[16]; + + if (ent->chplan == RTW_CHPLAN_UNSPECIFIED) + sprintf(buf, "NA"); + else + sprintf(buf, "0x%02X", ent->chplan); + + RTW_PRINT_SEL(sel, "\"%c%c\", %s%s\n" + , ent->alpha2[0], ent->alpha2[1], buf + , COUNTRY_CHPLAN_EN_11AC(ent) ? " ac" : "" + ); +} + +void dump_country_chplan_map(void *sel) +{ + const struct country_chplan *ent; + u8 code[2]; + +#if RTW_DEF_MODULE_REGULATORY_CERT + RTW_PRINT_SEL(sel, "RTW_DEF_MODULE_REGULATORY_CERT:0x%x\n", RTW_DEF_MODULE_REGULATORY_CERT); +#endif +#ifdef CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP + RTW_PRINT_SEL(sel, "CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP\n"); +#endif + + for (code[0] = 'A'; code[0] <= 'Z'; code[0]++) { + for (code[1] = 'A'; code[1] <= 'Z'; code[1]++) { + ent = rtw_get_chplan_from_country(code); + if (!ent) + continue; + + dump_country_chplan(sel, ent); + } + } +} + +void dump_chplan_id_list(void *sel) +{ + u8 first = 1; + int i; + + for (i = 0; i < RTW_ChannelPlanMap_size; i++) { + if (!rtw_is_channel_plan_valid(i)) + continue; + + if (first) { + RTW_PRINT_SEL(sel, "0x%02X ", i); + first = 0; + } else + _RTW_PRINT_SEL(sel, "0x%02X ", i); + } +} + +#ifdef CONFIG_RTW_DEBUG +void dump_chplan_test(void *sel) +{ + int i, j; + + /* check redundent */ + for (i = 0; i < RTW_CHD_2G_MAX; i++) { + for (j = 0; j < i; j++) { + if (CH_LIST_LEN(rtw_channel_def_2g[i]) == CH_LIST_LEN(rtw_channel_def_2g[j]) + && _rtw_memcmp(&CH_LIST_CH(rtw_channel_def_2g[i], 0), &CH_LIST_CH(rtw_channel_def_2g[j], 0), CH_LIST_LEN(rtw_channel_def_2g[i]) + 1) == _TRUE) + RTW_PRINT_SEL(sel, "2G chd:%u and %u is the same\n", i, j); + } + } + + /* check invalid channel */ + for (i = 0; i < RTW_CHD_2G_MAX; i++) { + for (j = 0; j < CH_LIST_LEN(rtw_channel_def_2g[i]); j++) { + if (rtw_ch2freq(CH_LIST_CH(rtw_channel_def_2g[i], j)) == 0) + RTW_PRINT_SEL(sel, "2G invalid ch:%u at (%d,%d)\n", CH_LIST_CH(rtw_channel_def_2g[i], j), i, j); + } + } + +#if CONFIG_IEEE80211_BAND_5GHZ + /* check redundent */ + for (i = 0; i < RTW_CHD_5G_MAX; i++) { + for (j = 0; j < i; j++) { + if (CH_LIST_LEN(rtw_channel_def_5g[i]) == CH_LIST_LEN(rtw_channel_def_5g[j]) + && _rtw_memcmp(&CH_LIST_CH(rtw_channel_def_5g[i], 0), &CH_LIST_CH(rtw_channel_def_5g[j], 0), CH_LIST_LEN(rtw_channel_def_5g[i]) + 1) == _TRUE) + RTW_PRINT_SEL(sel, "5G chd:%u and %u is the same\n", i, j); + } + } + + /* check invalid channel */ + for (i = 0; i < RTW_CHD_5G_MAX; i++) { + for (j = 0; j < CH_LIST_LEN(rtw_channel_def_5g[i]); j++) { + if (rtw_ch2freq(CH_LIST_CH(rtw_channel_def_5g[i], j)) == 0) + RTW_PRINT_SEL(sel, "5G invalid ch:%u at (%d,%d)\n", CH_LIST_CH(rtw_channel_def_5g[i], j), i, j); + } + } +#endif + + /* check redundent */ + for (i = 0; i < RTW_ChannelPlanMap_size; i++) { + if (!rtw_is_channel_plan_valid(i)) + continue; + for (j = 0; j < i; j++) { + if (!rtw_is_channel_plan_valid(j)) + continue; + if (_rtw_memcmp(&RTW_ChannelPlanMap[i], &RTW_ChannelPlanMap[j], sizeof(RTW_ChannelPlanMap[i])) == _TRUE) + RTW_PRINT_SEL(sel, "channel plan 0x%02x and 0x%02x is the same\n", i, j); + } + } +} +#endif /* CONFIG_RTW_DEBUG */ + +void dump_chplan_ver(void *sel) +{ + RTW_PRINT_SEL(sel, "%s%s-%s\n", RTW_DOMAIN_MAP_VER, RTW_DOMAIN_MAP_M_VER, RTW_COUNTRY_MAP_VER); +} diff --git a/core/rtw_chplan.h b/core/rtw_chplan.h new file mode 100644 index 0000000..bfe1ab2 --- /dev/null +++ b/core/rtw_chplan.h @@ -0,0 +1,95 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2018 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifndef __RTW_CHPLAN_H__ +#define __RTW_CHPLAN_H__ + +#define RTW_CHPLAN_UNSPECIFIED 0xFF + +u8 rtw_chplan_get_default_regd(u8 id); +bool rtw_chplan_is_empty(u8 id); +bool rtw_is_channel_plan_valid(u8 id); +bool rtw_regsty_is_excl_chs(struct registry_priv *regsty, u8 ch); + +enum regd_src_t { + REGD_SRC_RTK_PRIV = 0, /* Regulatory settings from Realtek framework (Realtek defined or customized) */ + REGD_SRC_OS = 1, /* Regulatory settings from OS */ + REGD_SRC_NUM, +}; + +#define regd_src_is_valid(src) ((src) < REGD_SRC_NUM) + +extern const char *_regd_src_str[]; +#define regd_src_str(src) ((src) >= REGD_SRC_NUM ? _regd_src_str[REGD_SRC_NUM] : _regd_src_str[src]) + +struct _RT_CHANNEL_INFO; +u8 init_channel_set(_adapter *adapter); +bool rtw_chset_is_dfs_range(struct _RT_CHANNEL_INFO *chset, u32 hi, u32 lo); +bool rtw_chset_is_dfs_ch(struct _RT_CHANNEL_INFO *chset, u8 ch); +bool rtw_chset_is_dfs_chbw(struct _RT_CHANNEL_INFO *chset, u8 ch, u8 bw, u8 offset); +u8 rtw_process_beacon_hint(_adapter *adapter, WLAN_BSSID_EX *bss); + +#define IS_ALPHA2_NO_SPECIFIED(_alpha2) ((*((u16 *)(_alpha2))) == 0xFFFF) +#define IS_ALPHA2_WORLDWIDE(_alpha2) (strncmp(_alpha2, "00", 2) == 0) + +#define RTW_MODULE_RTL8821AE_HMC_M2 BIT0 /* RTL8821AE(HMC + M.2) */ +#define RTW_MODULE_RTL8821AU BIT1 /* RTL8821AU */ +#define RTW_MODULE_RTL8812AENF_NGFF BIT2 /* RTL8812AENF(8812AE+8761)_NGFF */ +#define RTW_MODULE_RTL8812AEBT_HMC BIT3 /* RTL8812AEBT(8812AE+8761)_HMC */ +#define RTW_MODULE_RTL8188EE_HMC_M2 BIT4 /* RTL8188EE(HMC + M.2) */ +#define RTW_MODULE_RTL8723BE_HMC_M2 BIT5 /* RTL8723BE(HMC + M.2) */ +#define RTW_MODULE_RTL8723BS_NGFF1216 BIT6 /* RTL8723BS(NGFF1216) */ +#define RTW_MODULE_RTL8192EEBT_HMC_M2 BIT7 /* RTL8192EEBT(8192EE+8761AU)_(HMC + M.2) */ +#define RTW_MODULE_RTL8723DE_NGFF1630 BIT8 /* RTL8723DE(NGFF1630) */ +#define RTW_MODULE_RTL8822BE BIT9 /* RTL8822BE */ +#define RTW_MODULE_RTL8821CE BIT10 /* RTL8821CE */ +#define RTW_MODULE_RTL8822CE BIT11 /* RTL8822CE */ + +enum rtw_dfs_regd { + RTW_DFS_REGD_NONE = 0, + RTW_DFS_REGD_FCC = 1, + RTW_DFS_REGD_MKK = 2, + RTW_DFS_REGD_ETSI = 3, + RTW_DFS_REGD_NUM, + RTW_DFS_REGD_AUTO = 0xFF, /* follow channel plan */ +}; + +extern const char *_rtw_dfs_regd_str[]; +#define rtw_dfs_regd_str(region) (((region) >= RTW_DFS_REGD_NUM) ? _rtw_dfs_regd_str[RTW_DFS_REGD_NONE] : _rtw_dfs_regd_str[(region)]) + +struct country_chplan { + char alpha2[2]; /* "00" means worldwide */ + u8 chplan; +#ifdef CONFIG_80211AC_VHT + u8 en_11ac; +#endif +}; + +#ifdef CONFIG_80211AC_VHT +#define COUNTRY_CHPLAN_EN_11AC(_ent) ((_ent)->en_11ac) +#else +#define COUNTRY_CHPLAN_EN_11AC(_ent) 0 +#endif + +const struct country_chplan *rtw_get_chplan_from_country(const char *country_code); + +void dump_country_chplan(void *sel, const struct country_chplan *ent); +void dump_country_chplan_map(void *sel); +void dump_chplan_id_list(void *sel); +#ifdef CONFIG_RTW_DEBUG +void dump_chplan_test(void *sel); +#endif +void dump_chplan_ver(void *sel); + +#endif /* __RTW_CHPLAN_H__ */ diff --git a/core/rtw_cmd.c b/core/rtw_cmd.c new file mode 100644 index 0000000..7bcc6f4 --- /dev/null +++ b/core/rtw_cmd.c @@ -0,0 +1,5710 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_CMD_C_ + +#include +#include + +#ifndef DBG_CMD_EXECUTE + #define DBG_CMD_EXECUTE 0 +#endif + +/* +Caller and the rtw_cmd_thread can protect cmd_q by spin_lock. +No irqsave is necessary. +*/ + +sint _rtw_init_cmd_priv(struct cmd_priv *pcmdpriv) +{ + sint res = _SUCCESS; + + + _rtw_init_sema(&(pcmdpriv->cmd_queue_sema), 0); + /* _rtw_init_sema(&(pcmdpriv->cmd_done_sema), 0); */ + _rtw_init_sema(&(pcmdpriv->start_cmdthread_sema), 0); + + _rtw_init_queue(&(pcmdpriv->cmd_queue)); + + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + + pcmdpriv->cmd_seq = 1; + + pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ); + + if (pcmdpriv->cmd_allocated_buf == NULL) { + res = _FAIL; + goto exit; + } + + pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf + CMDBUFF_ALIGN_SZ - ((SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ - 1)); + + pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4); + + if (pcmdpriv->rsp_allocated_buf == NULL) { + res = _FAIL; + goto exit; + } + + pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf + 4 - ((SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3); + + pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0; + + _rtw_mutex_init(&pcmdpriv->sctx_mutex); +exit: + + + return res; + +} + +#ifdef CONFIG_C2H_WK +static void c2h_wk_callback(_workitem *work) +{ + struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk); + _adapter *adapter = container_of(evtpriv, _adapter, evtpriv); + u8 *c2h_evt; + c2h_id_filter direct_hdl_filter = rtw_hal_c2h_id_handle_directly; + u8 id, seq, plen; + u8 *payload; + + evtpriv->c2h_wk_alive = _TRUE; + + while (!rtw_cbuf_empty(evtpriv->c2h_queue)) { + c2h_evt = (u8 *)rtw_cbuf_pop(evtpriv->c2h_queue); + if (c2h_evt != NULL) { + /* This C2H event is read, clear it */ + c2h_evt_clear(adapter); + } else { + c2h_evt = (u8 *)rtw_malloc(C2H_REG_LEN); + if (c2h_evt == NULL) { + rtw_warn_on(1); + continue; + } + + /* This C2H event is not read, read & clear now */ + if (rtw_hal_c2h_evt_read(adapter, c2h_evt) != _SUCCESS) { + rtw_mfree(c2h_evt, C2H_REG_LEN); + continue; + } + } + + /* Special pointer to trigger c2h_evt_clear only */ + if ((void *)c2h_evt == (void *)evtpriv) + continue; + + if (!rtw_hal_c2h_valid(adapter, c2h_evt) + || rtw_hal_c2h_reg_hdr_parse(adapter, c2h_evt, &id, &seq, &plen, &payload) != _SUCCESS + ) { + rtw_mfree(c2h_evt, C2H_REG_LEN); + continue; + } + + if (direct_hdl_filter(adapter, id, seq, plen, payload) == _TRUE) { + /* Handle directly */ + rtw_hal_c2h_handler(adapter, id, seq, plen, payload); + rtw_mfree(c2h_evt, C2H_REG_LEN); + } else { + /* Enqueue into cmd_thread for others */ + rtw_c2h_reg_wk_cmd(adapter, c2h_evt); + rtw_mfree(c2h_evt, C2H_REG_LEN); + } + } + + evtpriv->c2h_wk_alive = _FALSE; +} +#endif /* CONFIG_C2H_WK */ + +sint _rtw_init_evt_priv(struct evt_priv *pevtpriv) +{ + sint res = _SUCCESS; + + +#ifdef CONFIG_H2CLBK + _rtw_init_sema(&(pevtpriv->lbkevt_done), 0); + pevtpriv->lbkevt_limit = 0; + pevtpriv->lbkevt_num = 0; + pevtpriv->cmdevt_parm = NULL; +#endif + + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + ATOMIC_SET(&pevtpriv->event_seq, 0); + pevtpriv->evt_done_cnt = 0; + +#ifdef CONFIG_EVENT_THREAD_MODE + + _rtw_init_sema(&(pevtpriv->evt_notify), 0); + + pevtpriv->evt_allocated_buf = rtw_zmalloc(MAX_EVTSZ + 4); + if (pevtpriv->evt_allocated_buf == NULL) { + res = _FAIL; + goto exit; + } + pevtpriv->evt_buf = pevtpriv->evt_allocated_buf + 4 - ((unsigned int)(pevtpriv->evt_allocated_buf) & 3); + + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pevtpriv->allocated_c2h_mem = rtw_zmalloc(C2H_MEM_SZ + 4); + + if (pevtpriv->allocated_c2h_mem == NULL) { + res = _FAIL; + goto exit; + } + + pevtpriv->c2h_mem = pevtpriv->allocated_c2h_mem + 4\ + - ((u32)(pevtpriv->allocated_c2h_mem) & 3); +#endif /* end of CONFIG_SDIO_HCI */ + + _rtw_init_queue(&(pevtpriv->evt_queue)); + +exit: + +#endif /* end of CONFIG_EVENT_THREAD_MODE */ + +#ifdef CONFIG_C2H_WK + _init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL); + pevtpriv->c2h_wk_alive = _FALSE; + pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN + 1); +#endif + + + return res; +} + +void _rtw_free_evt_priv(struct evt_priv *pevtpriv) +{ + + +#ifdef CONFIG_EVENT_THREAD_MODE + _rtw_free_sema(&(pevtpriv->evt_notify)); + + if (pevtpriv->evt_allocated_buf) + rtw_mfree(pevtpriv->evt_allocated_buf, MAX_EVTSZ + 4); +#endif + +#ifdef CONFIG_C2H_WK + _cancel_workitem_sync(&pevtpriv->c2h_wk); + while (pevtpriv->c2h_wk_alive) + rtw_msleep_os(10); + + while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) { + void *c2h; + c2h = rtw_cbuf_pop(pevtpriv->c2h_queue); + if (c2h != NULL && c2h != (void *)pevtpriv) + rtw_mfree(c2h, 16); + } + rtw_cbuf_free(pevtpriv->c2h_queue); +#endif + + + +} + +void _rtw_free_cmd_priv(struct cmd_priv *pcmdpriv) +{ + + if (pcmdpriv) { + _rtw_spinlock_free(&(pcmdpriv->cmd_queue.lock)); + _rtw_free_sema(&(pcmdpriv->cmd_queue_sema)); + /* _rtw_free_sema(&(pcmdpriv->cmd_done_sema)); */ + _rtw_free_sema(&(pcmdpriv->start_cmdthread_sema)); + + if (pcmdpriv->cmd_allocated_buf) + rtw_mfree(pcmdpriv->cmd_allocated_buf, MAX_CMDSZ + CMDBUFF_ALIGN_SZ); + + if (pcmdpriv->rsp_allocated_buf) + rtw_mfree(pcmdpriv->rsp_allocated_buf, MAX_RSPSZ + 4); + + _rtw_mutex_free(&pcmdpriv->sctx_mutex); + } +} + +/* +Calling Context: + +rtw_enqueue_cmd can only be called between kernel thread, +since only spin_lock is used. + +ISR/Call-Back functions can't call this sub-function. + +*/ +#ifdef DBG_CMD_QUEUE +extern u8 dump_cmd_id; +#endif + +sint _rtw_enqueue_cmd(_queue *queue, struct cmd_obj *obj, bool to_head) +{ + _irqL irqL; + + + if (obj == NULL) + goto exit; + + /* _enter_critical_bh(&queue->lock, &irqL); */ + _enter_critical(&queue->lock, &irqL); + + if (to_head) + rtw_list_insert_head(&obj->list, &queue->queue); + else + rtw_list_insert_tail(&obj->list, &queue->queue); + +#ifdef DBG_CMD_QUEUE + if (dump_cmd_id) { + RTW_INFO("%s===> cmdcode:0x%02x\n", __FUNCTION__, obj->cmdcode); + if (obj->cmdcode == CMD_SET_MLME_EVT) { + if (obj->parmbuf) { + struct rtw_evt_header *evt_hdr = (struct rtw_evt_header *)(obj->parmbuf); + RTW_INFO("evt_hdr->id:%d\n", evt_hdr->id); + } + } + if (obj->cmdcode == CMD_SET_DRV_EXTRA) { + if (obj->parmbuf) { + struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)(obj->parmbuf); + RTW_INFO("pdrvextra_cmd_parm->ec_id:0x%02x\n", pdrvextra_cmd_parm->ec_id); + } + } + } + + if (queue->queue.prev->next != &queue->queue) { + RTW_INFO("[%d] head %p, tail %p, tail->prev->next %p[tail], tail->next %p[head]\n", __LINE__, + &queue->queue, queue->queue.prev, queue->queue.prev->prev->next, queue->queue.prev->next); + + RTW_INFO("==========%s============\n", __FUNCTION__); + RTW_INFO("head:%p,obj_addr:%p\n", &queue->queue, obj); + RTW_INFO("padapter: %p\n", obj->padapter); + RTW_INFO("cmdcode: 0x%02x\n", obj->cmdcode); + RTW_INFO("res: %d\n", obj->res); + RTW_INFO("parmbuf: %p\n", obj->parmbuf); + RTW_INFO("cmdsz: %d\n", obj->cmdsz); + RTW_INFO("rsp: %p\n", obj->rsp); + RTW_INFO("rspsz: %d\n", obj->rspsz); + RTW_INFO("sctx: %p\n", obj->sctx); + RTW_INFO("list->next: %p\n", obj->list.next); + RTW_INFO("list->prev: %p\n", obj->list.prev); + } +#endif /* DBG_CMD_QUEUE */ + + /* _exit_critical_bh(&queue->lock, &irqL); */ + _exit_critical(&queue->lock, &irqL); + +exit: + + + return _SUCCESS; +} + +struct cmd_obj *_rtw_dequeue_cmd(_queue *queue) +{ + _irqL irqL; + struct cmd_obj *obj; + + + /* _enter_critical_bh(&(queue->lock), &irqL); */ + _enter_critical(&queue->lock, &irqL); + +#ifdef DBG_CMD_QUEUE + if (queue->queue.prev->next != &queue->queue) { + RTW_INFO("[%d] head %p, tail %p, tail->prev->next %p[tail], tail->next %p[head]\n", __LINE__, + &queue->queue, queue->queue.prev, queue->queue.prev->prev->next, queue->queue.prev->next); + } +#endif /* DBG_CMD_QUEUE */ + + + if (rtw_is_list_empty(&(queue->queue))) + obj = NULL; + else { + obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list); + +#ifdef DBG_CMD_QUEUE + if (queue->queue.prev->next != &queue->queue) { + RTW_INFO("==========%s============\n", __FUNCTION__); + RTW_INFO("head:%p,obj_addr:%p\n", &queue->queue, obj); + RTW_INFO("padapter: %p\n", obj->padapter); + RTW_INFO("cmdcode: 0x%02x\n", obj->cmdcode); + RTW_INFO("res: %d\n", obj->res); + RTW_INFO("parmbuf: %p\n", obj->parmbuf); + RTW_INFO("cmdsz: %d\n", obj->cmdsz); + RTW_INFO("rsp: %p\n", obj->rsp); + RTW_INFO("rspsz: %d\n", obj->rspsz); + RTW_INFO("sctx: %p\n", obj->sctx); + RTW_INFO("list->next: %p\n", obj->list.next); + RTW_INFO("list->prev: %p\n", obj->list.prev); + } + + if (dump_cmd_id) { + RTW_INFO("%s===> cmdcode:0x%02x\n", __FUNCTION__, obj->cmdcode); + if (obj->cmdcode == CMD_SET_DRV_EXTRA) { + if (obj->parmbuf) { + struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)(obj->parmbuf); + printk("pdrvextra_cmd_parm->ec_id:0x%02x\n", pdrvextra_cmd_parm->ec_id); + } + } + + } +#endif /* DBG_CMD_QUEUE */ + + rtw_list_delete(&obj->list); + } + + /* _exit_critical_bh(&(queue->lock), &irqL); */ + _exit_critical(&queue->lock, &irqL); + + + return obj; +} + +u32 rtw_init_cmd_priv(struct cmd_priv *pcmdpriv) +{ + u32 res; + res = _rtw_init_cmd_priv(pcmdpriv); + return res; +} + +u32 rtw_init_evt_priv(struct evt_priv *pevtpriv) +{ + int res; + res = _rtw_init_evt_priv(pevtpriv); + return res; +} + +void rtw_free_evt_priv(struct evt_priv *pevtpriv) +{ + _rtw_free_evt_priv(pevtpriv); +} + +void rtw_free_cmd_priv(struct cmd_priv *pcmdpriv) +{ + _rtw_free_cmd_priv(pcmdpriv); +} + +int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj); +int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj) +{ +#ifndef CONFIG_MAC_LOOPBACK_DRIVER + u8 bAllow = _FALSE; /* set to _TRUE to allow enqueuing cmd when hw_init_completed is _FALSE */ +#else + u8 bAllow = _TRUE; /* hw_init_completed is _FALSE in the case of MAC loopback*/ +#endif + +#ifdef SUPPORT_HW_RFOFF_DETECTED + /* To decide allow or not */ + if ((adapter_to_pwrctl(pcmdpriv->padapter)->bHWPwrPindetect) + && (!pcmdpriv->padapter->registrypriv.usbss_enable) + ) { + if (cmd_obj->cmdcode == CMD_SET_DRV_EXTRA) { + struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf; + if (pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID) { + /* RTW_INFO("==>enqueue POWER_SAVING_CTRL_WK_CID\n"); */ + bAllow = _TRUE; + } + } + } +#endif + + if (cmd_obj->cmdcode == CMD_SET_CHANPLAN) + bAllow = _TRUE; + + if (cmd_obj->no_io) + bAllow = _TRUE; + + if ((!rtw_is_hw_init_completed(pcmdpriv->padapter) && (bAllow == _FALSE)) + || ATOMIC_READ(&(pcmdpriv->cmdthd_running)) == _FALSE /* com_thread not running */ + ) { + if (DBG_CMD_EXECUTE) + RTW_INFO(ADPT_FMT" drop "CMD_FMT" hw_init_completed:%u, cmdthd_running:%u\n", ADPT_ARG(cmd_obj->padapter) + , CMD_ARG(cmd_obj), rtw_get_hw_init_completed(cmd_obj->padapter), ATOMIC_READ(&pcmdpriv->cmdthd_running)); + if (0) + rtw_warn_on(1); + + return _FAIL; + } + return _SUCCESS; +} + + + +u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj) +{ + int res = _FAIL; + PADAPTER padapter = pcmdpriv->padapter; + + + if (cmd_obj == NULL) + goto exit; + + cmd_obj->padapter = padapter; + +#ifdef CONFIG_CONCURRENT_MODE + /* change pcmdpriv to primary's pcmdpriv */ + if (!is_primary_adapter(padapter)) + pcmdpriv = &(GET_PRIMARY_ADAPTER(padapter)->cmdpriv); +#endif + + res = rtw_cmd_filter(pcmdpriv, cmd_obj); + if ((_FAIL == res) || (cmd_obj->cmdsz > MAX_CMDSZ)) { + if (cmd_obj->cmdsz > MAX_CMDSZ) { + RTW_INFO("%s failed due to obj->cmdsz(%d) > MAX_CMDSZ(%d)\n", __func__, cmd_obj->cmdsz, MAX_CMDSZ); + rtw_warn_on(1); + } + + if (cmd_obj->cmdcode == CMD_SET_DRV_EXTRA) { + struct drvextra_cmd_parm *extra_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf; + + if (extra_parm->pbuf && extra_parm->size > 0) + rtw_mfree(extra_parm->pbuf, extra_parm->size); + } + rtw_free_cmd_obj(cmd_obj); + goto exit; + } + + res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj, 0); + + if (res == _SUCCESS) + _rtw_up_sema(&pcmdpriv->cmd_queue_sema); + +exit: + + + return res; +} + +struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv) +{ + struct cmd_obj *cmd_obj; + + + cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue); + + return cmd_obj; +} + +void rtw_cmd_clr_isr(struct cmd_priv *pcmdpriv) +{ + pcmdpriv->cmd_done_cnt++; + /* _rtw_up_sema(&(pcmdpriv->cmd_done_sema)); */ +} + +void rtw_free_cmd_obj(struct cmd_obj *pcmd) +{ + if (pcmd->parmbuf != NULL) { + /* free parmbuf in cmd_obj */ + rtw_mfree((unsigned char *)pcmd->parmbuf, pcmd->cmdsz); + } + if (pcmd->rsp != NULL) { + if (pcmd->rspsz != 0) { + /* free rsp in cmd_obj */ + rtw_mfree((unsigned char *)pcmd->rsp, pcmd->rspsz); + } + } + + /* free cmd_obj */ + rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj)); +} + + +void rtw_stop_cmd_thread(_adapter *adapter) +{ + if (adapter->cmdThread) { + _rtw_up_sema(&adapter->cmdpriv.cmd_queue_sema); + rtw_thread_stop(adapter->cmdThread); + adapter->cmdThread = NULL; + } +} + +thread_return rtw_cmd_thread(thread_context context) +{ + u8 ret; + struct cmd_obj *pcmd; + u8 *pcmdbuf, *prspbuf; + systime cmd_start_time; + u32 cmd_process_time; + u8(*cmd_hdl)(_adapter *padapter, u8 *pbuf); + void (*pcmd_callback)(_adapter *dev, struct cmd_obj *pcmd); + PADAPTER padapter = (PADAPTER)context; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + struct drvextra_cmd_parm *extra_parm = NULL; + _irqL irqL; + + thread_enter("RTW_CMD_THREAD"); + + pcmdbuf = pcmdpriv->cmd_buf; + prspbuf = pcmdpriv->rsp_buf; + ATOMIC_SET(&(pcmdpriv->cmdthd_running), _TRUE); + _rtw_up_sema(&pcmdpriv->start_cmdthread_sema); + + + while (1) { + if (_rtw_down_sema(&pcmdpriv->cmd_queue_sema) == _FAIL) { + RTW_PRINT(FUNC_ADPT_FMT" _rtw_down_sema(&pcmdpriv->cmd_queue_sema) return _FAIL, break\n", FUNC_ADPT_ARG(padapter)); + break; + } + + if (RTW_CANNOT_RUN(padapter)) { + RTW_DBG(FUNC_ADPT_FMT "- bDriverStopped(%s) bSurpriseRemoved(%s)\n", + FUNC_ADPT_ARG(padapter), + rtw_is_drv_stopped(padapter) ? "True" : "False", + rtw_is_surprise_removed(padapter) ? "True" : "False"); + break; + } + + _enter_critical(&pcmdpriv->cmd_queue.lock, &irqL); + if (rtw_is_list_empty(&(pcmdpriv->cmd_queue.queue))) { + /* RTW_INFO("%s: cmd queue is empty!\n", __func__); */ + _exit_critical(&pcmdpriv->cmd_queue.lock, &irqL); + continue; + } + _exit_critical(&pcmdpriv->cmd_queue.lock, &irqL); + +_next: + if (RTW_CANNOT_RUN(padapter)) { + RTW_PRINT("%s: DriverStopped(%s) SurpriseRemoved(%s) break at line %d\n", + __func__ + , rtw_is_drv_stopped(padapter) ? "True" : "False" + , rtw_is_surprise_removed(padapter) ? "True" : "False" + , __LINE__); + break; + } + + pcmd = rtw_dequeue_cmd(pcmdpriv); + if (!pcmd) { +#ifdef CONFIG_LPS_LCLK + rtw_unregister_cmd_alive(padapter); +#endif + continue; + } + + cmd_start_time = rtw_get_current_time(); + pcmdpriv->cmd_issued_cnt++; + + if (pcmd->cmdsz > MAX_CMDSZ) { + RTW_ERR("%s cmdsz:%d > MAX_CMDSZ:%d\n", __func__, pcmd->cmdsz, MAX_CMDSZ); + pcmd->res = H2C_PARAMETERS_ERROR; + goto post_process; + } + + if (pcmd->cmdcode >= (sizeof(wlancmds) / sizeof(struct rtw_cmd))) { + RTW_ERR("%s undefined cmdcode:%d\n", __func__, pcmd->cmdcode); + pcmd->res = H2C_PARAMETERS_ERROR; + goto post_process; + } + + cmd_hdl = wlancmds[pcmd->cmdcode].cmd_hdl; + if (!cmd_hdl) { + RTW_ERR("%s no cmd_hdl for cmdcode:%d\n", __func__, pcmd->cmdcode); + pcmd->res = H2C_PARAMETERS_ERROR; + goto post_process; + } + + if (_FAIL == rtw_cmd_filter(pcmdpriv, pcmd)) { + pcmd->res = H2C_DROPPED; + if (pcmd->cmdcode == CMD_SET_DRV_EXTRA) { + extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf; + if (extra_parm && extra_parm->pbuf && extra_parm->size > 0) + rtw_mfree(extra_parm->pbuf, extra_parm->size); + } + #if CONFIG_DFS + else if (pcmd->cmdcode == CMD_SET_CHANSWITCH) + adapter_to_rfctl(padapter)->csa_ch = 0; + #endif + goto post_process; + } + +#ifdef CONFIG_LPS_LCLK + if (pcmd->no_io) + rtw_unregister_cmd_alive(padapter); + else { + if (rtw_register_cmd_alive(padapter) != _SUCCESS) { + if (DBG_CMD_EXECUTE) + RTW_PRINT("%s: wait to leave LPS_LCLK\n", __func__); + + pcmd->res = H2C_ENQ_HEAD; + ret = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, pcmd, 1); + if (ret == _SUCCESS) { + if (DBG_CMD_EXECUTE) + RTW_INFO(ADPT_FMT" "CMD_FMT" ENQ_HEAD\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd)); + continue; + } + + RTW_INFO(ADPT_FMT" "CMD_FMT" ENQ_HEAD_FAIL\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd)); + pcmd->res = H2C_ENQ_HEAD_FAIL; + rtw_warn_on(1); + } + } +#endif /* CONFIG_LPS_LCLK */ + + if (DBG_CMD_EXECUTE) + RTW_INFO(ADPT_FMT" "CMD_FMT" %sexecute\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd) + , pcmd->res == H2C_ENQ_HEAD ? "ENQ_HEAD " : (pcmd->res == H2C_ENQ_HEAD_FAIL ? "ENQ_HEAD_FAIL " : "")); + + _rtw_memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz); + ret = cmd_hdl(pcmd->padapter, pcmdbuf); + pcmd->res = ret; + + pcmdpriv->cmd_seq++; + +post_process: + + _enter_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL); + if (pcmd->sctx) { + if (0) + RTW_PRINT(FUNC_ADPT_FMT" pcmd->sctx\n", FUNC_ADPT_ARG(pcmd->padapter)); + if (pcmd->res == H2C_SUCCESS) + rtw_sctx_done(&pcmd->sctx); + else + rtw_sctx_done_err(&pcmd->sctx, RTW_SCTX_DONE_CMD_ERROR); + } + _exit_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL); + + cmd_process_time = rtw_get_passing_time_ms(cmd_start_time); + if (cmd_process_time > 1000) { + RTW_INFO(ADPT_FMT" "CMD_FMT" process_time=%d\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd), cmd_process_time); + if (0) + rtw_warn_on(1); + } + + /* call callback function for post-processed */ + if (pcmd->cmdcode < (sizeof(wlancmds) / sizeof(struct rtw_cmd))) + pcmd_callback = wlancmds[pcmd->cmdcode].callback; + else + pcmd_callback = NULL; + + if (pcmd_callback == NULL) { + rtw_free_cmd_obj(pcmd); + } else { + /* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!=NULL) */ + pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */ + } + + flush_signals_thread(); + + goto _next; + + } + +#ifdef CONFIG_LPS_LCLK + rtw_unregister_cmd_alive(padapter); +#endif + + /* to avoid enqueue cmd after free all cmd_obj */ + ATOMIC_SET(&(pcmdpriv->cmdthd_running), _FALSE); + + /* free all cmd_obj resources */ + do { + pcmd = rtw_dequeue_cmd(pcmdpriv); + if (pcmd == NULL) + break; + + if (0) + RTW_INFO("%s: leaving... drop "CMD_FMT"\n", __func__, CMD_ARG(pcmd)); + + if (pcmd->cmdcode == CMD_SET_DRV_EXTRA) { + extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf; + if (extra_parm->pbuf && extra_parm->size > 0) + rtw_mfree(extra_parm->pbuf, extra_parm->size); + } + #if CONFIG_DFS + else if (pcmd->cmdcode == CMD_SET_CHANSWITCH) + adapter_to_rfctl(padapter)->csa_ch = 0; + #endif + + _enter_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL); + if (pcmd->sctx) { + if (0) + RTW_PRINT(FUNC_ADPT_FMT" pcmd->sctx\n", FUNC_ADPT_ARG(pcmd->padapter)); + rtw_sctx_done_err(&pcmd->sctx, RTW_SCTX_DONE_CMD_DROP); + } + _exit_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL); + + rtw_free_cmd_obj(pcmd); + } while (1); + + RTW_INFO(FUNC_ADPT_FMT " Exit\n", FUNC_ADPT_ARG(padapter)); + + rtw_thread_wait_stop(); + + return 0; +} + + +#ifdef CONFIG_EVENT_THREAD_MODE +u32 rtw_enqueue_evt(struct evt_priv *pevtpriv, struct evt_obj *obj) +{ + _irqL irqL; + int res; + _queue *queue = &pevtpriv->evt_queue; + + + res = _SUCCESS; + + if (obj == NULL) { + res = _FAIL; + goto exit; + } + + _enter_critical_bh(&queue->lock, &irqL); + + rtw_list_insert_tail(&obj->list, &queue->queue); + + _exit_critical_bh(&queue->lock, &irqL); + + /* rtw_evt_notify_isr(pevtpriv); */ + +exit: + + + return res; +} + +struct evt_obj *rtw_dequeue_evt(_queue *queue) +{ + _irqL irqL; + struct evt_obj *pevtobj; + + + _enter_critical_bh(&queue->lock, &irqL); + + if (rtw_is_list_empty(&(queue->queue))) + pevtobj = NULL; + else { + pevtobj = LIST_CONTAINOR(get_next(&(queue->queue)), struct evt_obj, list); + rtw_list_delete(&pevtobj->list); + } + + _exit_critical_bh(&queue->lock, &irqL); + + + return pevtobj; +} + +void rtw_free_evt_obj(struct evt_obj *pevtobj) +{ + + if (pevtobj->parmbuf) + rtw_mfree((unsigned char *)pevtobj->parmbuf, pevtobj->evtsz); + + rtw_mfree((unsigned char *)pevtobj, sizeof(struct evt_obj)); + +} + +void rtw_evt_notify_isr(struct evt_priv *pevtpriv) +{ + pevtpriv->evt_done_cnt++; + _rtw_up_sema(&(pevtpriv->evt_notify)); +} +#endif + +void rtw_init_sitesurvey_parm(_adapter *padapter, struct sitesurvey_parm *pparm) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + _rtw_memset(pparm, 0, sizeof(struct sitesurvey_parm)); + pparm->scan_mode = pmlmepriv->scan_mode; +} + +/* +rtw_sitesurvey_cmd(~) + ### NOTE:#### (!!!!) + MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock +*/ +u8 rtw_sitesurvey_cmd(_adapter *padapter, struct sitesurvey_parm *pparm) +{ + u8 res = _FAIL; + struct cmd_obj *ph2c; + struct sitesurvey_parm *psurveyPara; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + +#ifdef CONFIG_LPS + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 0); +#endif + +#ifdef CONFIG_P2P_PS + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1); +#endif /* CONFIG_P2P_PS */ + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) + return _FAIL; + + psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm)); + if (psurveyPara == NULL) { + rtw_mfree((unsigned char *) ph2c, sizeof(struct cmd_obj)); + return _FAIL; + } + + if (pparm) + _rtw_memcpy(psurveyPara, pparm, sizeof(struct sitesurvey_parm)); + else + psurveyPara->scan_mode = pmlmepriv->scan_mode; + + rtw_free_network_queue(padapter, _FALSE); + + init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, CMD_SITE_SURVEY); + + set_fwstate(pmlmepriv, WIFI_UNDER_SURVEY); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + + if (res == _SUCCESS) { + u32 scan_timeout_ms; + + pmlmepriv->scan_start_time = rtw_get_current_time(); + scan_timeout_ms = rtw_scan_timeout_decision(padapter); + mlme_set_scan_to_timer(pmlmepriv,scan_timeout_ms); + + rtw_led_control(padapter, LED_CTL_SITE_SURVEY); + } else + _clr_fwstate_(pmlmepriv, WIFI_UNDER_SURVEY); + + + return res; +} + +void rtw_readtssi_cmdrsp_callback(_adapter *padapter, struct cmd_obj *pcmd) +{ + + rtw_mfree((unsigned char *) pcmd->parmbuf, pcmd->cmdsz); + rtw_mfree((unsigned char *) pcmd, sizeof(struct cmd_obj)); + +#ifdef CONFIG_MP_INCLUDED + if (padapter->registrypriv.mp_mode == 1) + padapter->mppriv.workparam.bcompleted = _TRUE; +#endif + +} + +#ifdef CONFIG_AP_MODE +static u8 rtw_createbss_cmd(_adapter *adapter, int flags, bool adhoc + , u8 ifbmp, u8 excl_ifbmp, s16 req_ch, s8 req_bw, s8 req_offset) +{ + struct cmd_obj *cmdobj; + struct createbss_parm *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (req_ch > 0 && req_bw >= 0 && req_offset >= 0) { + if (!rtw_chset_is_chbw_valid(adapter_to_chset(adapter), req_ch, req_bw, req_offset, 0, 0)) { + res = _FAIL; + goto exit; + } + } + + /* prepare cmd parameter */ + parm = (struct createbss_parm *)rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + + if (adhoc) { + /* for now, adhoc doesn't support ch,bw,offset request */ + parm->adhoc = 1; + } else { + parm->adhoc = 0; + parm->ifbmp = ifbmp; + parm->excl_ifbmp = excl_ifbmp; + parm->req_ch = req_ch; + parm->req_bw = req_bw; + parm->req_offset = req_offset; + } + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != createbss_hdl(adapter, (u8 *)parm)) + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_CREATE_BSS); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } + +exit: + return res; +} + +inline u8 rtw_create_ibss_cmd(_adapter *adapter, int flags) +{ + return rtw_createbss_cmd(adapter, flags + , 1 + , 0, 0 + , 0, REQ_BW_NONE, REQ_OFFSET_NONE /* for now, adhoc doesn't support ch,bw,offset request */ + ); +} + +inline u8 rtw_startbss_cmd(_adapter *adapter, int flags) +{ + return rtw_createbss_cmd(adapter, flags + , 0 + , BIT(adapter->iface_id), 0 + , 0, REQ_BW_NONE, REQ_OFFSET_NONE /* excute entire AP setup cmd */ + ); +} + +inline u8 rtw_change_bss_chbw_cmd(_adapter *adapter, int flags + , u8 ifbmp, u8 excl_ifbmp, s16 req_ch, s8 req_bw, s8 req_offset) +{ + return rtw_createbss_cmd(adapter, flags + , 0 + , ifbmp, excl_ifbmp + , req_ch, req_bw, req_offset + ); +} +#endif /* CONFIG_AP_MODE */ + +u8 rtw_joinbss_cmd(_adapter *padapter, struct wlan_network *pnetwork) +{ + u8 *auth, res = _SUCCESS; + uint t_len = 0; + WLAN_BSSID_EX *psecnetwork; + struct cmd_obj *pcmd; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; +#ifdef CONFIG_80211N_HT + struct ht_priv *phtpriv = &pmlmepriv->htpriv; +#endif /* CONFIG_80211N_HT */ +#ifdef CONFIG_80211AC_VHT + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; +#endif /* CONFIG_80211AC_VHT */ + NDIS_802_11_NETWORK_INFRASTRUCTURE ndis_network_mode = pnetwork->network.InfrastructureMode; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + u32 tmp_len; + u8 *ptmp = NULL; + + rtw_led_control(padapter, LED_CTL_START_TO_LINK); + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } +#if 0 + /* for IEs is pointer */ + t_len = sizeof(u32) + sizeof(NDIS_802_11_MAC_ADDRESS) + 2 + + sizeof(NDIS_802_11_SSID) + sizeof(u32) + + sizeof(NDIS_802_11_RSSI) + sizeof(NDIS_802_11_NETWORK_TYPE) + + sizeof(NDIS_802_11_CONFIGURATION) + + sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE) + + sizeof(NDIS_802_11_RATES_EX) + sizeof(WLAN_PHY_INFO) + sizeof(u32) + MAX_IE_SZ; +#endif + /* for IEs is fix buf size */ + t_len = sizeof(WLAN_BSSID_EX); + + + /* for hidden ap to set fw_state here */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE) != _TRUE) { + switch (ndis_network_mode) { + case Ndis802_11IBSS: + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + break; + + case Ndis802_11Infrastructure: + set_fwstate(pmlmepriv, WIFI_STATION_STATE); + break; + + default: + rtw_warn_on(1); + break; + } + } + + pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength); + +#ifdef CONFIG_80211AC_VHT + /* save AP beamform_cap info for BCM IOT issue */ + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_BROADCOM) + get_vht_bf_cap(pnetwork->network.IEs, + pnetwork->network.IELength, + &pvhtpriv->ap_bf_cap); +#endif + /* + Modified by Arvin 2015/05/13 + Solution for allocating a new WLAN_BSSID_EX to avoid race condition issue between disconnect and joinbss + */ + psecnetwork = (WLAN_BSSID_EX *)rtw_zmalloc(sizeof(WLAN_BSSID_EX)); + if (psecnetwork == NULL) { + if (pcmd != NULL) + rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj)); + + res = _FAIL; + + + goto exit; + } + + _rtw_memset(psecnetwork, 0, t_len); + + _rtw_memcpy(psecnetwork, &pnetwork->network, get_WLAN_BSSID_EX_sz(&pnetwork->network)); + + auth = &psecuritypriv->authenticator_ie[0]; + psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->IELength; + + if ((psecnetwork->IELength - 12) < (256 - 1)) + _rtw_memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength - 12); + else + _rtw_memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256 - 1)); + + psecnetwork->IELength = 0; + /* Added by Albert 2009/02/18 */ + /* If the the driver wants to use the bssid to create the connection. */ + /* If not, we have to copy the connecting AP's MAC address to it so that */ + /* the driver just has the bssid information for PMKIDList searching. */ + + if (pmlmepriv->assoc_by_bssid == _FALSE) + _rtw_memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN); + + /* copy fixed ie */ + _rtw_memcpy(psecnetwork->IEs, pnetwork->network.IEs, 12); + psecnetwork->IELength = 12; + + psecnetwork->IELength += rtw_restruct_sec_ie(padapter, psecnetwork->IEs + psecnetwork->IELength); + + + pqospriv->qos_option = 0; + + if (pregistrypriv->wmm_enable) { +#ifdef CONFIG_WMMPS_STA + rtw_uapsd_use_default_setting(padapter); +#endif /* CONFIG_WMMPS_STA */ + tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength); + + if (psecnetwork->IELength != tmp_len) { + psecnetwork->IELength = tmp_len; + pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */ + } else { + pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */ + } + } + +#ifdef CONFIG_80211N_HT + phtpriv->ht_option = _FALSE; + if (pregistrypriv->ht_enable && is_supported_ht(pregistrypriv->wireless_mode)) { + ptmp = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &tmp_len, pnetwork->network.IELength - 12); + if (ptmp && tmp_len > 0) { + /* Added by Albert 2010/06/23 */ + /* For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */ + /* Especially for Realtek 8192u SoftAP. */ + if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) && + (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) && + (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) { + rtw_ht_use_default_setting(padapter); + + /* rtw_restructure_ht_ie */ + rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[12], &psecnetwork->IEs[0], + pnetwork->network.IELength - 12, &psecnetwork->IELength, + pnetwork->network.Configuration.DSConfig); + } + } + } + +#ifdef CONFIG_80211AC_VHT + pvhtpriv->vht_option = _FALSE; + if (phtpriv->ht_option + && REGSTY_IS_11AC_ENABLE(pregistrypriv) + && is_supported_vht(pregistrypriv->wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + u8 vht_enable = 0; + + if (pnetwork->network.Configuration.DSConfig > 14) + vht_enable = 1; + else if ((REGSTY_IS_11AC_24G_ENABLE(pregistrypriv)) && (padapter->registrypriv.wifi_spec == 0)) + vht_enable = 1; + + if (vht_enable == 1) + rtw_restructure_vht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], + pnetwork->network.IELength, &psecnetwork->IELength); + } +#endif +#endif /* CONFIG_80211N_HT */ + + rtw_append_exented_cap(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength); + +#ifdef CONFIG_RTW_80211R + rtw_ft_validate_akm_type(padapter, pnetwork); +#endif + +#if 0 + psecuritypriv->supplicant_ie[0] = (u8)psecnetwork->IELength; + + if (psecnetwork->IELength < (256 - 1)) + _rtw_memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0], psecnetwork->IELength); + else + _rtw_memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0], (256 - 1)); +#endif + + pcmd->cmdsz = sizeof(WLAN_BSSID_EX); + + _rtw_init_listhead(&pcmd->list); + pcmd->cmdcode = CMD_JOINBSS;/* _JoinBss_CMD_ */ + pcmd->parmbuf = (unsigned char *)psecnetwork; + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + + + return res; +} + +u8 rtw_disassoc_cmd(_adapter *padapter, u32 deauth_timeout_ms, int flags) /* for sta_mode */ +{ + struct cmd_obj *cmdobj = NULL; + struct disconnect_parm *param = NULL; + struct cmd_priv *cmdpriv = &padapter->cmdpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + /* prepare cmd parameter */ + param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param)); + if (param == NULL) { + res = _FAIL; + goto exit; + } + param->deauth_timeout_ms = deauth_timeout_ms; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (disconnect_hdl(padapter, (u8 *)param) != H2C_SUCCESS) + res = _FAIL; + rtw_mfree((u8 *)param, sizeof(*param)); + + } else { + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)param, sizeof(*param)); + goto exit; + } + init_h2fwcmd_w_parm_no_rsp(cmdobj, param, CMD_DISCONNECT); + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } + +exit: + + + return res; +} + +#ifdef CONFIG_AP_MODE +u8 rtw_stop_ap_cmd(_adapter *adapter, u8 flags) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != stop_ap_hdl(adapter)) + res = _FAIL; + } else { + parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + + parm->ec_id = STOP_AP_WK_CID; + parm->type = 0; + parm->size = 0; + parm->pbuf = NULL; + + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } +exit: + return res; +} +#endif /* CONFIG_AP_MODE */ + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +u8 rtw_tx_control_cmd(_adapter *adapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL){ + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = TBTX_CONTROL_TX_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} +#endif + +u8 rtw_setopmode_cmd(_adapter *adapter, NDIS_802_11_NETWORK_INFRASTRUCTURE networktype, u8 flags) +{ + struct cmd_obj *cmdobj; + struct setopmode_parm *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + /* prepare cmd parameter */ + parm = (struct setopmode_parm *)rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + parm->mode = (u8)networktype; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != setopmode_hdl(adapter, (u8 *)parm)) + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_OPMODE); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } + +exit: + return res; +} + +u8 rtw_setstakey_cmd(_adapter *padapter, struct sta_info *sta, u8 key_type, bool enqueue) +{ + struct cmd_obj *ph2c; + struct set_stakey_parm *psetstakey_para; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct set_stakey_rsp *psetstakey_rsp = NULL; + + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + u8 key_len =16; + u8 res = _SUCCESS; + + + psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm)); + if (psetstakey_para == NULL) { + res = _FAIL; + goto exit; + } + + _rtw_memcpy(psetstakey_para->addr, sta->cmn.mac_addr, ETH_ALEN); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) + psetstakey_para->algorithm = (unsigned char) psecuritypriv->dot11PrivacyAlgrthm; + else + GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, _FALSE); + + if ((psetstakey_para->algorithm == _GCMP_256_) || (psetstakey_para->algorithm == _CCMP_256_)) + key_len = 32; + + if (key_type == GROUP_KEY) { + _rtw_memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, key_len); + psetstakey_para->gk = 1; + } else if (key_type == UNICAST_KEY) + _rtw_memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, key_len); +#ifdef CONFIG_TDLS + else if (key_type == TDLS_KEY) { + _rtw_memcpy(&psetstakey_para->key, sta->tpk.tk, key_len); + psetstakey_para->algorithm = (u8)sta->dot118021XPrivacy; + } +#endif /* CONFIG_TDLS */ + + /* jeff: set this becasue at least sw key is ready */ + padapter->securitypriv.busetkipkey = _TRUE; + + if (enqueue) { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm)); + res = _FAIL; + goto exit; + } + + psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp)); + if (psetstakey_rsp == NULL) { + rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm)); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, CMD_SET_STAKEY); + ph2c->rsp = (u8 *) psetstakey_rsp; + ph2c->rspsz = sizeof(struct set_stakey_rsp); + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else { + set_stakey_hdl(padapter, (u8 *)psetstakey_para); + rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm)); + } +exit: + + + return res; +} + +u8 rtw_clearstakey_cmd(_adapter *padapter, struct sta_info *sta, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct set_stakey_parm *psetstakey_para; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct set_stakey_rsp *psetstakey_rsp = NULL; + s16 cam_id = 0; + u8 res = _SUCCESS; + + if (!sta) { + RTW_ERR("%s sta == NULL\n", __func__); + goto exit; + } + + if (!enqueue) { + while ((cam_id = rtw_camid_search(padapter, sta->cmn.mac_addr, -1, -1)) >= 0) { + RTW_PRINT("clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(sta->cmn.mac_addr), cam_id); + clear_cam_entry(padapter, cam_id); + rtw_camid_free(padapter, cam_id); + } + } else { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm)); + if (psetstakey_para == NULL) { + rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp)); + if (psetstakey_rsp == NULL) { + rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm)); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, CMD_SET_STAKEY); + ph2c->rsp = (u8 *) psetstakey_rsp; + ph2c->rspsz = sizeof(struct set_stakey_rsp); + + _rtw_memcpy(psetstakey_para->addr, sta->cmn.mac_addr, ETH_ALEN); + + psetstakey_para->algorithm = _NO_PRIVACY_; + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + + } + +exit: + + + return res; +} + +u8 rtw_addbareq_cmd(_adapter *padapter, u8 tid, u8 *addr) +{ + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct cmd_obj *ph2c; + struct addBaReq_parm *paddbareq_parm; + + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + paddbareq_parm = (struct addBaReq_parm *)rtw_zmalloc(sizeof(struct addBaReq_parm)); + if (paddbareq_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + paddbareq_parm->tid = tid; + _rtw_memcpy(paddbareq_parm->addr, addr, ETH_ALEN); + + init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, CMD_ADD_BAREQ); + + /* RTW_INFO("rtw_addbareq_cmd, tid=%d\n", tid); */ + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + + return res; +} + +u8 rtw_addbarsp_cmd(_adapter *padapter, u8 *addr, u16 tid, u8 status, u8 size, u16 start_seq) +{ + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct cmd_obj *ph2c; + struct addBaRsp_parm *paddBaRsp_parm; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + paddBaRsp_parm = (struct addBaRsp_parm *)rtw_zmalloc(sizeof(struct addBaRsp_parm)); + + if (paddBaRsp_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + _rtw_memcpy(paddBaRsp_parm->addr, addr, ETH_ALEN); + paddBaRsp_parm->tid = tid; + paddBaRsp_parm->status = status; + paddBaRsp_parm->size = size; + paddBaRsp_parm->start_seq = start_seq; + + init_h2fwcmd_w_parm_no_rsp(ph2c, paddBaRsp_parm, CMD_ADD_BARSP); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + + return res; +} +/* add for CONFIG_IEEE80211W, none 11w can use it */ +u8 rtw_reset_securitypriv_cmd(_adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = RESET_SECURITYPRIV; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + + return res; + +} + +void free_assoc_resources_hdl(_adapter *padapter, u8 lock_scanned_queue) +{ + rtw_free_assoc_resources(padapter, lock_scanned_queue); +} + +u8 rtw_free_assoc_resources_cmd(_adapter *padapter, u8 lock_scanned_queue, int flags) +{ + struct cmd_obj *cmd; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (flags & RTW_CMDF_DIRECTLY) { + free_assoc_resources_hdl(padapter, lock_scanned_queue); + } + else { + cmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmd == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)cmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = FREE_ASSOC_RESOURCES; + pdrvextra_cmd_parm->type = lock_scanned_queue; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(cmd, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + if (flags & RTW_CMDF_WAIT_ACK) { + cmd->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmd); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmd->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } +exit: + return res; + +} + +u8 rtw_dynamic_chk_wk_cmd(_adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + /* only primary padapter does this cmd */ + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + + /* rtw_enqueue_cmd(pcmdpriv, ph2c); */ + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + + return res; + +} + +u8 rtw_iqk_cmd(_adapter *padapter, u8 flags) +{ + struct cmd_obj *pcmdobj; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl */ + rtw_iqk_hdl(padapter, NULL); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_parm_rsp(pcmdobj, CMD_DO_IQK); + + if (flags & RTW_CMDF_WAIT_ACK) { + pcmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + pcmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } + +exit: + + return res; +} + +u8 rtw_set_chbw_cmd(_adapter *padapter, u8 ch, u8 bw, u8 ch_offset, u8 flags) +{ + struct cmd_obj *pcmdobj; + struct set_ch_parm *set_ch_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + + RTW_INFO(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n", + FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset); + + /* check input parameter */ + + /* prepare cmd parameter */ + set_ch_parm = (struct set_ch_parm *)rtw_zmalloc(sizeof(*set_ch_parm)); + if (set_ch_parm == NULL) { + res = _FAIL; + goto exit; + } + set_ch_parm->ch = ch; + set_ch_parm->bw = bw; + set_ch_parm->ch_offset = ch_offset; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != rtw_set_chbw_hdl(padapter, (u8 *)set_ch_parm)) + res = _FAIL; + + rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm)); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm, CMD_SET_CHANNEL); + + if (flags & RTW_CMDF_WAIT_ACK) { + pcmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + pcmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + } + + /* do something based on res... */ + +exit: + + RTW_INFO(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), res); + + + return res; +} + +static u8 _rtw_set_chplan_cmd(_adapter *adapter, int flags, u8 chplan, const struct country_chplan *country_ent, enum regd_src_t regd_src, u8 swconfig) +{ + struct cmd_obj *cmdobj; + struct SetChannelPlan_param *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + /* check if allow software config */ + if (swconfig && rtw_hal_is_disable_sw_channel_plan(adapter) == _TRUE) { + res = _FAIL; + goto exit; + } + + /* if country_entry is provided, replace chplan */ + if (country_ent) + chplan = country_ent->chplan; + + /* check input parameter */ + if (regd_src == REGD_SRC_RTK_PRIV && !rtw_is_channel_plan_valid(chplan)) { + res = _FAIL; + goto exit; + } + + /* prepare cmd parameter */ + parm = (struct SetChannelPlan_param *)rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + parm->regd_src = regd_src; + parm->country_ent = country_ent; + parm->channel_plan = chplan; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != rtw_set_chplan_hdl(adapter, (u8 *)parm)) + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_CHANPLAN); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + + /* allow set channel plan when cmd_thread is not running */ + if (res != _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + parm = (struct SetChannelPlan_param *)rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + parm->regd_src = regd_src; + parm->country_ent = country_ent; + parm->channel_plan = chplan; + + if (H2C_SUCCESS != rtw_set_chplan_hdl(adapter, (u8 *)parm)) + res = _FAIL; + else + res = _SUCCESS; + rtw_mfree((u8 *)parm, sizeof(*parm)); + } + } + +exit: + return res; +} + +inline u8 rtw_set_chplan_cmd(_adapter *adapter, int flags, u8 chplan, u8 swconfig) +{ + return _rtw_set_chplan_cmd(adapter, flags, chplan, NULL, REGD_SRC_RTK_PRIV, swconfig); +} + +inline u8 rtw_set_country_cmd(_adapter *adapter, int flags, const char *country_code, u8 swconfig) +{ + const struct country_chplan *ent; + + if (is_alpha(country_code[0]) == _FALSE + || is_alpha(country_code[1]) == _FALSE + ) { + RTW_PRINT("%s input country_code is not alpha2\n", __func__); + return _FAIL; + } + + ent = rtw_get_chplan_from_country(country_code); + + if (ent == NULL) { + RTW_PRINT("%s unsupported country_code:\"%c%c\"\n", __func__, country_code[0], country_code[1]); + return _FAIL; + } + + RTW_PRINT("%s country_code:\"%c%c\" mapping to chplan:0x%02x\n", __func__, country_code[0], country_code[1], ent->chplan); + + return _rtw_set_chplan_cmd(adapter, flags, RTW_CHPLAN_UNSPECIFIED, ent, REGD_SRC_RTK_PRIV, swconfig); +} + +#ifdef CONFIG_REGD_SRC_FROM_OS +inline u8 rtw_sync_os_regd_cmd(_adapter *adapter, int flags, const char *country_code, u8 dfs_region) +{ + struct country_chplan *ent; + const struct country_chplan *rtk_ent; + + /* allocate entry for regd source out of driver */ + ent = rtw_malloc(sizeof(*ent)); + if (ent == NULL) + return _FAIL; + + rtk_ent = rtw_get_chplan_from_country(country_code); + + _rtw_memcpy(ent->alpha2, country_code, 2); + + /* + * Regulation follows OS, the internal txpwr limit selection is searched by alpha2 + * "00" => WW, others use string mapping + * When no matching txpwr limit selection is found, use + * 1. txpwr lmit selection associated with alpha2 inside driver regulation database + * 2. WW when driver has no support of this alpha2 + */ + + ent->chplan = rtk_ent ? rtk_ent->chplan : RTW_CHPLAN_UNSPECIFIED; + #ifdef CONFIG_80211AC_VHT + ent->en_11ac = 1; + #endif + + /* TODO: dfs_region */ + + return _rtw_set_chplan_cmd(adapter, flags, RTW_CHPLAN_UNSPECIFIED, ent, REGD_SRC_OS, 1); +} +#endif /* CONFIG_REGD_SRC_FROM_OS */ + +u8 rtw_get_chplan_cmd(_adapter *adapter, int flags, struct get_chplan_resp **resp) +{ + struct cmd_obj *cmdobj; + struct get_channel_plan_param *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _FAIL; + + if (!(flags & (RTW_CMDF_DIRECTLY | RTW_CMDF_WAIT_ACK))) + goto exit; + + /* prepare cmd parameter */ + parm = rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) + goto exit; + parm->resp = resp; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS == rtw_get_chplan_hdl(adapter, (u8 *)parm)) + res = _SUCCESS; + rtw_mfree((u8 *)parm, sizeof(*parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_GET_CHANPLAN); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + + /* allow get channel plan when cmd_thread is not running */ + if (res != _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + parm = rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) + goto exit; + parm->resp = resp; + + if (H2C_SUCCESS == rtw_get_chplan_hdl(adapter, (u8 *)parm)) + res = _SUCCESS; + + rtw_mfree((u8 *)parm, sizeof(*parm)); + } + } + +exit: + return res; +} + +u8 rtw_led_blink_cmd(_adapter *padapter, void *pLed) +{ + struct cmd_obj *pcmdobj; + struct LedBlink_param *ledBlink_param; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + u8 res = _SUCCESS; + + + + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + res = _FAIL; + goto exit; + } + + ledBlink_param = (struct LedBlink_param *)rtw_zmalloc(sizeof(struct LedBlink_param)); + if (ledBlink_param == NULL) { + rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + ledBlink_param->pLed = pLed; + + init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, CMD_LEDBLINK); + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + +exit: + + + return res; +} + +u8 rtw_set_csa_cmd(_adapter *adapter) +{ + struct cmd_obj *cmdobj; + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + cmdobj = rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_parm_rsp(cmdobj, CMD_SET_CHANSWITCH); + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + +exit: + return res; +} + +u8 rtw_tdls_cmd(_adapter *padapter, u8 *addr, u8 option) +{ + u8 res = _SUCCESS; +#ifdef CONFIG_TDLS + struct cmd_obj *pcmdobj; + struct TDLSoption_param *TDLSoption; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + res = _FAIL; + goto exit; + } + + TDLSoption = (struct TDLSoption_param *)rtw_zmalloc(sizeof(struct TDLSoption_param)); + if (TDLSoption == NULL) { + rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + _rtw_spinlock(&(padapter->tdlsinfo.cmd_lock)); + if (addr != NULL) + _rtw_memcpy(TDLSoption->addr, addr, 6); + TDLSoption->option = option; + _rtw_spinunlock(&(padapter->tdlsinfo.cmd_lock)); + init_h2fwcmd_w_parm_no_rsp(pcmdobj, TDLSoption, CMD_TDLS); + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + +exit: +#endif /* CONFIG_TDLS */ + + return res; +} + +u8 rtw_enable_hw_update_tsf_cmd(_adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = EN_HW_UPDATE_TSF_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +u8 rtw_periodic_tsf_update_end_cmd(_adapter *adapter) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + rtw_mfree((unsigned char *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + parm->ec_id = PERIOD_TSF_UPDATE_END_WK_CID; + parm->type = 0; + parm->size = 0; + parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + +exit: + return res; +} +u8 rtw_ssmps_wk_hdl(_adapter *adapter, struct ssmps_cmd_parm *ssmp_param) +{ + u8 res = _SUCCESS; + struct sta_info *sta = ssmp_param->sta; + u8 smps = ssmp_param->smps; + + if (sta == NULL) + return _FALSE; + + if (smps) + rtw_ssmps_enter(adapter, sta); + else + rtw_ssmps_leave(adapter, sta); + return res; +} + +u8 rtw_ssmps_wk_cmd(_adapter *adapter, struct sta_info *sta, u8 smps, u8 enqueue) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *cmd_parm; + struct ssmps_cmd_parm *ssmp_param; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + if (enqueue) { + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (cmd_parm == NULL) { + rtw_mfree((unsigned char *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + ssmp_param = (struct ssmps_cmd_parm *)rtw_zmalloc(sizeof(struct ssmps_cmd_parm)); + if (ssmp_param == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + ssmp_param->smps = smps; + ssmp_param->sta = sta; + + cmd_parm->ec_id = SSMPS_WK_CID; + cmd_parm->type = 0; + cmd_parm->size = sizeof(struct ssmps_cmd_parm); + cmd_parm->pbuf = (u8 *)ssmp_param; + + init_h2fwcmd_w_parm_no_rsp(cmdobj, cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + } else { + struct ssmps_cmd_parm tmp_ssmp_param; + + tmp_ssmp_param.smps = smps; + tmp_ssmp_param.sta = sta; + rtw_ssmps_wk_hdl(adapter, &tmp_ssmp_param); + } + +exit: + return res; +} + +#ifdef CONFIG_SUPPORT_STATIC_SMPS +u8 _ssmps_chk_by_tp(_adapter *adapter, u8 from_timer) +{ + u8 enter_smps = _FALSE; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct sta_priv *pstapriv = &adapter->stapriv; + struct sta_info *psta; + u32 tx_tp_mbits, rx_tp_mbits; + + if (!MLME_IS_STA(adapter) || + !hal_is_mimo_support(adapter) || + !pmlmeext->ssmps_en || + (pmlmeext->cur_channel > 14) + ) + return enter_smps; + + psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv)); + if (psta == NULL) { + RTW_ERR(ADPT_FMT" sta == NULL\n", ADPT_ARG(adapter)); + rtw_warn_on(1); + return enter_smps; + } + + if (psta->cmn.mimo_type == RF_1T1R) + return enter_smps; + + tx_tp_mbits = psta->sta_stats.tx_tp_kbits >> 10; + rx_tp_mbits = psta->sta_stats.rx_tp_kbits >> 10; + + #ifdef DBG_STATIC_SMPS + if (pmlmeext->ssmps_test) { + enter_smps = (pmlmeext->ssmps_test_en == 1) ? _TRUE : _FALSE; + } + else + #endif + { + if ((tx_tp_mbits <= pmlmeext->ssmps_tx_tp_th) && + (rx_tp_mbits <= pmlmeext->ssmps_rx_tp_th)) + enter_smps = _TRUE; + else + enter_smps = _FALSE; + } + + if (1) { + RTW_INFO(FUNC_ADPT_FMT" tx_tp:%d [%d], rx_tp:%d [%d] , SSMPS enter :%s\n", + FUNC_ADPT_ARG(adapter), + tx_tp_mbits, pmlmeext->ssmps_tx_tp_th, + rx_tp_mbits, pmlmeext->ssmps_rx_tp_th, + (enter_smps == _TRUE) ? "True" : "False"); + #ifdef DBG_STATIC_SMPS + RTW_INFO(FUNC_ADPT_FMT" test:%d test_en:%d\n", + FUNC_ADPT_ARG(adapter), + pmlmeext->ssmps_test, + pmlmeext->ssmps_test_en); + #endif + } + + if (enter_smps) { + if (!from_timer && psta->cmn.sm_ps != SM_PS_STATIC) + rtw_ssmps_enter(adapter, psta); + } else { + if (!from_timer && psta->cmn.sm_ps != SM_PS_DISABLE) + rtw_ssmps_leave(adapter, psta); + else { + u8 ps_change = _FALSE; + + if (enter_smps && psta->cmn.sm_ps != SM_PS_STATIC) + ps_change = _TRUE; + else if (!enter_smps && psta->cmn.sm_ps != SM_PS_DISABLE) + ps_change = _TRUE; + + if (ps_change) + rtw_ssmps_wk_cmd(adapter, psta, enter_smps, 1); + } + } + + return enter_smps; +} +#endif /*CONFIG_SUPPORT_STATIC_SMPS*/ + +#ifdef CONFIG_CTRL_TXSS_BY_TP +void rtw_ctrl_txss_update_mimo_type(_adapter *adapter, struct sta_info *sta) +{ + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + pmlmeext->txss_momi_type_bk = sta->cmn.mimo_type; +} + +u8 rtw_ctrl_txss(_adapter *adapter, struct sta_info *sta, bool tx_1ss) +{ + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + u8 lps_changed = _FALSE; + u8 rst = _SUCCESS; + + if (pmlmeext->txss_1ss == tx_1ss) + return _FALSE; + + if (pwrpriv->bLeisurePs && pwrpriv->pwr_mode != PS_MODE_ACTIVE) { + lps_changed = _TRUE; + LPS_Leave(adapter, "LPS_CTRL_TXSS"); + } + + RTW_INFO(ADPT_FMT" STA [" MAC_FMT "] set tx to %d ss\n", + ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr), + (tx_1ss) ? 1 : rtw_get_sta_tx_nss(adapter, sta)); + + /*ra re-registed*/ + sta->cmn.mimo_type = (tx_1ss) ? RF_1T1R : pmlmeext->txss_momi_type_bk; + rtw_phydm_ra_registed(adapter, sta); + + /*configure trx mode*/ + rtw_phydm_trx_cfg(adapter, tx_1ss); + pmlmeext->txss_1ss = tx_1ss; + + if (lps_changed) + LPS_Enter(adapter, "LPS_CTRL_TXSS"); + + return rst; +} + +u8 rtw_ctrl_txss_wk_hdl(_adapter *adapter, struct txss_cmd_parm *txss_param) +{ + if (!txss_param->sta) + return _FALSE; + + return rtw_ctrl_txss(adapter, txss_param->sta, txss_param->tx_1ss); +} + +u8 rtw_ctrl_txss_wk_cmd(_adapter *adapter, struct sta_info *sta, bool tx_1ss, u8 flag) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *cmd_parm; + struct txss_cmd_parm *txss_param; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + txss_param = (struct txss_cmd_parm *)rtw_zmalloc(sizeof(struct txss_cmd_parm)); + if (txss_param == NULL) { + res = _FAIL; + goto exit; + } + + txss_param->tx_1ss = tx_1ss; + txss_param->sta = sta; + + if (flag & RTW_CMDF_DIRECTLY) { + res = rtw_ctrl_txss_wk_hdl(adapter, txss_param); + rtw_mfree((u8 *)txss_param, sizeof(*txss_param)); + } else { + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (cmd_parm == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + cmd_parm->ec_id = TXSS_WK_CID; + cmd_parm->type = 0; + cmd_parm->size = sizeof(struct txss_cmd_parm); + cmd_parm->pbuf = (u8 *)txss_param; + + init_h2fwcmd_w_parm_no_rsp(cmdobj, cmd_parm, CMD_SET_DRV_EXTRA); + + if (flag & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + if (res == _SUCCESS && (flag & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + } + +exit: + return res; +} + +void rtw_ctrl_tx_ss_by_tp(_adapter *adapter, u8 from_timer) +{ + bool tx_1ss = _FALSE; /*change tx from 2ss to 1ss*/ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct sta_priv *pstapriv = &adapter->stapriv; + struct sta_info *psta; + u32 tx_tp_mbits; + + if (!MLME_IS_STA(adapter) || + !hal_is_mimo_support(adapter) || + !pmlmeext->txss_ctrl_en + ) + return; + + psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv)); + if (psta == NULL) { + RTW_ERR(ADPT_FMT" sta == NULL\n", ADPT_ARG(adapter)); + rtw_warn_on(1); + return; + } + + tx_tp_mbits = psta->sta_stats.tx_tp_kbits >> 10; + if (tx_tp_mbits >= pmlmeext->txss_tp_th) { + tx_1ss = _FALSE; + } else { + if (pmlmeext->txss_tp_chk_cnt && --pmlmeext->txss_tp_chk_cnt) + tx_1ss = _FALSE; + else + tx_1ss = _TRUE; + } + + if (1) { + RTW_INFO(FUNC_ADPT_FMT" tx_tp:%d [%d] tx_1ss(%d):%s\n", + FUNC_ADPT_ARG(adapter), + tx_tp_mbits, pmlmeext->txss_tp_th, + pmlmeext->txss_tp_chk_cnt, + (tx_1ss == _TRUE) ? "True" : "False"); + } + + if (pmlmeext->txss_1ss != tx_1ss) { + if (from_timer) + rtw_ctrl_txss_wk_cmd(adapter, psta, tx_1ss, 0); + else + rtw_ctrl_txss(adapter, psta, tx_1ss); + } +} +#ifdef DBG_CTRL_TXSS +void dbg_ctrl_txss(_adapter *adapter, bool tx_1ss) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct sta_priv *pstapriv = &adapter->stapriv; + struct sta_info *psta; + + if (!MLME_IS_STA(adapter) || + !hal_is_mimo_support(adapter) + ) + return; + + psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv)); + if (psta == NULL) { + RTW_ERR(ADPT_FMT" sta == NULL\n", ADPT_ARG(adapter)); + rtw_warn_on(1); + return; + } + + rtw_ctrl_txss(adapter, psta, tx_1ss); +} +#endif +#endif /*CONFIG_CTRL_TXSS_BY_TP*/ + +#ifdef CONFIG_LPS +#ifdef CONFIG_LPS_CHK_BY_TP +#ifdef LPS_BCN_CNT_MONITOR +static u8 _bcn_cnt_expected(struct sta_info *psta) +{ + _adapter *adapter = psta->padapter; + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 dtim = rtw_get_bcn_dtim_period(adapter); + u8 bcn_cnt = 0; + + if ((pmlmeinfo->bcn_interval !=0) && (dtim != 0)) + bcn_cnt = 2000 / pmlmeinfo->bcn_interval / dtim * 4 / 5; /*2s*/ + if (0) + RTW_INFO("%s bcn_cnt:%d\n", __func__, bcn_cnt); + + if (bcn_cnt == 0) { + RTW_ERR(FUNC_ADPT_FMT" bcn_cnt == 0\n", FUNC_ADPT_ARG(adapter)); + rtw_warn_on(1); + } + + return bcn_cnt; +} +#endif +u8 _lps_chk_by_tp(_adapter *adapter, u8 from_timer) +{ + u8 enter_ps = _FALSE; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct sta_priv *pstapriv = &adapter->stapriv; + struct sta_info *psta; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + u32 tx_tp_mbits, rx_tp_mbits, bi_tp_mbits; + u8 rx_bcn_cnt; + + psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv)); + if (psta == NULL) { + RTW_ERR(ADPT_FMT" sta == NULL\n", ADPT_ARG(adapter)); + rtw_warn_on(1); + return enter_ps; + } + + rx_bcn_cnt = rtw_get_bcn_cnt(psta->padapter); + psta->sta_stats.acc_tx_bytes = psta->sta_stats.tx_bytes; + psta->sta_stats.acc_rx_bytes = psta->sta_stats.rx_bytes; + +#if 1 + tx_tp_mbits = psta->sta_stats.tx_tp_kbits >> 10; + rx_tp_mbits = psta->sta_stats.rx_tp_kbits >> 10; + bi_tp_mbits = tx_tp_mbits + rx_tp_mbits; +#else + tx_tp_mbits = psta->sta_stats.smooth_tx_tp_kbits >> 10; + rx_tp_mbits = psta->sta_stats.smooth_rx_tp_kbits >> 10; + bi_tp_mbits = tx_tp_mbits + rx_tp_mbits; +#endif + + if ((bi_tp_mbits >= pwrpriv->lps_bi_tp_th) || + (tx_tp_mbits >= pwrpriv->lps_tx_tp_th) || + (rx_tp_mbits >= pwrpriv->lps_rx_tp_th)) { + enter_ps = _FALSE; + pwrpriv->lps_chk_cnt = pwrpriv->lps_chk_cnt_th; + } + else { +#ifdef LPS_BCN_CNT_MONITOR + u8 bcn_cnt = _bcn_cnt_expected(psta); + + if (bcn_cnt && (rx_bcn_cnt < bcn_cnt)) { + pwrpriv->lps_chk_cnt = 2; + RTW_ERR(FUNC_ADPT_FMT" BCN_CNT:%d(%d) invalid\n", + FUNC_ADPT_ARG(adapter), rx_bcn_cnt, bcn_cnt); + } +#endif + + if (pwrpriv->lps_chk_cnt && --pwrpriv->lps_chk_cnt) + enter_ps = _FALSE; + else + enter_ps = _TRUE; + } + + if (1) { + RTW_INFO(FUNC_ADPT_FMT" tx_tp:%d [%d], rx_tp:%d [%d], bi_tp:%d [%d], enter_ps(%d):%s\n", + FUNC_ADPT_ARG(adapter), + tx_tp_mbits, pwrpriv->lps_tx_tp_th, + rx_tp_mbits, pwrpriv->lps_rx_tp_th, + bi_tp_mbits, pwrpriv->lps_bi_tp_th, + pwrpriv->lps_chk_cnt, + (enter_ps == _TRUE) ? "True" : "False"); + RTW_INFO(FUNC_ADPT_FMT" tx_pkt_cnt :%d [%d], rx_pkt_cnt :%d [%d]\n", + FUNC_ADPT_ARG(adapter), + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, + pwrpriv->lps_tx_pkts, + pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod, + pwrpriv->lps_rx_pkts); + if (!adapter->bsta_tp_dump) + RTW_INFO(FUNC_ADPT_FMT" bcn_cnt:%d (per-%d second)\n", + FUNC_ADPT_ARG(adapter), + rx_bcn_cnt, + 2); + } + + if (enter_ps) { + if (!from_timer) + LPS_Enter(adapter, "TRAFFIC_IDLE"); + } else { + if (!from_timer) + LPS_Leave(adapter, "TRAFFIC_BUSY"); + else { + #ifdef CONFIG_CONCURRENT_MODE + #ifndef CONFIG_FW_MULTI_PORT_SUPPORT + if (adapter->hw_port == HW_PORT0) + #endif + #endif + rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_TRAFFIC_BUSY, 0); + } + } + + return enter_ps; +} +#endif + +static u8 _lps_chk_by_pkt_cnts(_adapter *padapter, u8 from_timer, u8 bBusyTraffic) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u8 bEnterPS = _FALSE; + + /* check traffic for powersaving. */ + if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) || + #ifdef CONFIG_LPS_SLOW_TRANSITION + (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2) + #else /* CONFIG_LPS_SLOW_TRANSITION */ + (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4) + #endif /* CONFIG_LPS_SLOW_TRANSITION */ + ) { + #ifdef DBG_RX_COUNTER_DUMP + if (padapter->dump_rx_cnt_mode & DUMP_DRV_TRX_COUNTER_DATA) + RTW_INFO("(-)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); + #endif + + bEnterPS = _FALSE; + #ifdef CONFIG_LPS_SLOW_TRANSITION + if (bBusyTraffic == _TRUE) { + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4) + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4; + + pmlmepriv->LinkDetectInfo.TrafficTransitionCount++; + + /* RTW_INFO("Set TrafficTransitionCount to %d\n", pmlmepriv->LinkDetectInfo.TrafficTransitionCount); */ + + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/) + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30; + } + #endif /* CONFIG_LPS_SLOW_TRANSITION */ + } else { + #ifdef DBG_RX_COUNTER_DUMP + if (padapter->dump_rx_cnt_mode & DUMP_DRV_TRX_COUNTER_DATA) + RTW_INFO("(+)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); + #endif + + #ifdef CONFIG_LPS_SLOW_TRANSITION + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount >= 2) + pmlmepriv->LinkDetectInfo.TrafficTransitionCount -= 2; + else + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0; + + if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount == 0) + bEnterPS = _TRUE; + #else /* CONFIG_LPS_SLOW_TRANSITION */ + bEnterPS = _TRUE; + #endif /* CONFIG_LPS_SLOW_TRANSITION */ + } + + #ifdef CONFIG_DYNAMIC_DTIM + if (pmlmepriv->LinkDetectInfo.LowPowerTransitionCount == 8) + bEnterPS = _FALSE; + + RTW_INFO("LowPowerTransitionCount=%d\n", pmlmepriv->LinkDetectInfo.LowPowerTransitionCount); + #endif /* CONFIG_DYNAMIC_DTIM */ + + /* LeisurePS only work in infra mode. */ + if (bEnterPS) { + if (!from_timer) { + #ifdef CONFIG_DYNAMIC_DTIM + if (pmlmepriv->LinkDetectInfo.LowPowerTransitionCount < 8) + adapter_to_pwrctl(padapter)->dtim = 1; + else + adapter_to_pwrctl(padapter)->dtim = 3; + #endif /* CONFIG_DYNAMIC_DTIM */ + LPS_Enter(padapter, "TRAFFIC_IDLE"); + } else { + /* do this at caller */ + /* rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 0); */ + /* rtw_hal_dm_watchdog_in_lps(padapter); */ + } + + #ifdef CONFIG_DYNAMIC_DTIM + if (adapter_to_pwrctl(padapter)->bFwCurrentInPSMode == _TRUE) + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount++; + #endif /* CONFIG_DYNAMIC_DTIM */ + } else { + #ifdef CONFIG_DYNAMIC_DTIM + if (pmlmepriv->LinkDetectInfo.LowPowerTransitionCount != 8) + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0; + else + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount++; + #endif /* CONFIG_DYNAMIC_DTIM */ + + if (!from_timer) + LPS_Leave(padapter, "TRAFFIC_BUSY"); + else { + #ifdef CONFIG_CONCURRENT_MODE + #ifndef CONFIG_FW_MULTI_PORT_SUPPORT + if (padapter->hw_port == HW_PORT0) + #endif + #endif + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_TRAFFIC_BUSY, 0); + } + } + + return bEnterPS; +} +#endif /* CONFIG_LPS */ + +/* from_timer == 1 means driver is in LPS */ +u8 traffic_status_watchdog(_adapter *padapter, u8 from_timer) +{ + u8 bEnterPS = _FALSE; + u16 BusyThresholdHigh; + u16 BusyThresholdLow; + u16 BusyThreshold; + u8 bBusyTraffic = _FALSE, bTxBusyTraffic = _FALSE, bRxBusyTraffic = _FALSE; + u8 bHigherBusyTraffic = _FALSE, bHigherBusyRxTraffic = _FALSE, bHigherBusyTxTraffic = _FALSE; + + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); +#ifdef CONFIG_TDLS + struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo); + struct tdls_txmgmt txmgmt; + u8 baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; +#endif /* CONFIG_TDLS */ +#ifdef CONFIG_TRAFFIC_PROTECT + RT_LINK_DETECT_T *link_detect = &pmlmepriv->LinkDetectInfo; +#endif + +#ifdef CONFIG_BT_COEXIST + if (padapter->registrypriv.wifi_spec != 1) { + BusyThresholdHigh = 25; + BusyThresholdLow = 10; + } else +#endif /* CONFIG_BT_COEXIST */ + { + BusyThresholdHigh = 100; + BusyThresholdLow = 75; + } + BusyThreshold = BusyThresholdHigh; + + + /* */ + /* Determine if our traffic is busy now */ + /* */ + if ((check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + /*&& !MgntInitAdapterInProgress(pMgntInfo)*/) { + /* if we raise bBusyTraffic in last watchdog, using lower threshold. */ + if (pmlmepriv->LinkDetectInfo.bBusyTraffic) + BusyThreshold = BusyThresholdLow; + + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold || + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) { + bBusyTraffic = _TRUE; + + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) + bRxBusyTraffic = _TRUE; + else + bTxBusyTraffic = _TRUE; + } + + /* Higher Tx/Rx data. */ + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 || + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) { + bHigherBusyTraffic = _TRUE; + + if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) + bHigherBusyRxTraffic = _TRUE; + else + bHigherBusyTxTraffic = _TRUE; + } + +#ifdef CONFIG_TRAFFIC_PROTECT +#define TX_ACTIVE_TH 10 +#define RX_ACTIVE_TH 20 +#define TRAFFIC_PROTECT_PERIOD_MS 4500 + + if (link_detect->NumTxOkInPeriod > TX_ACTIVE_TH + || link_detect->NumRxUnicastOkInPeriod > RX_ACTIVE_TH) { + + RTW_INFO(FUNC_ADPT_FMT" acqiure wake_lock for %u ms(tx:%d,rx_unicast:%d)\n", + FUNC_ADPT_ARG(padapter), + TRAFFIC_PROTECT_PERIOD_MS, + link_detect->NumTxOkInPeriod, + link_detect->NumRxUnicastOkInPeriod); + + rtw_lock_traffic_suspend_timeout(TRAFFIC_PROTECT_PERIOD_MS); + } +#endif + +#ifdef CONFIG_TDLS +#ifdef CONFIG_TDLS_AUTOSETUP + /* TDLS_WATCHDOG_PERIOD * 2sec, periodically send */ + if (hal_chk_wl_func(padapter, WL_FUNC_TDLS) == _TRUE) { + if ((ptdlsinfo->watchdog_count % TDLS_WATCHDOG_PERIOD) == 0) { + _rtw_memcpy(txmgmt.peer, baddr, ETH_ALEN); + issue_tdls_dis_req(padapter, &txmgmt); + } + ptdlsinfo->watchdog_count++; + } +#endif /* CONFIG_TDLS_AUTOSETUP */ +#endif /* CONFIG_TDLS */ + +#ifdef CONFIG_SUPPORT_STATIC_SMPS + _ssmps_chk_by_tp(padapter, from_timer); +#endif +#ifdef CONFIG_CTRL_TXSS_BY_TP + rtw_ctrl_tx_ss_by_tp(padapter, from_timer); +#endif + +#ifdef CONFIG_LPS + if (adapter_to_pwrctl(padapter)->bLeisurePs && MLME_IS_STA(padapter)) { + #ifdef CONFIG_LPS_CHK_BY_TP + if (adapter_to_pwrctl(padapter)->lps_chk_by_tp) + bEnterPS = _lps_chk_by_tp(padapter, from_timer); + else + #endif /*CONFIG_LPS_CHK_BY_TP*/ + bEnterPS = _lps_chk_by_pkt_cnts(padapter, from_timer, bBusyTraffic); + } +#endif /* CONFIG_LPS */ + + } else { +#ifdef CONFIG_LPS + if (!from_timer && rtw_mi_get_assoc_if_num(padapter) == 0) + LPS_Leave(padapter, "NON_LINKED"); +#endif + } + + session_tracker_chk_cmd(padapter, NULL); + +#ifdef CONFIG_BEAMFORMING +#ifdef RTW_BEAMFORMING_VERSION_2 + rtw_bf_update_traffic(padapter); +#endif /* RTW_BEAMFORMING_VERSION_2 */ +#endif /* CONFIG_BEAMFORMING */ + + pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0; + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0; + pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0; + pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic; + pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic; + pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic; + pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic; + pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic; + pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic; + + return bEnterPS; + +} + +#ifdef CONFIG_AP_MODE +/* for 11n Logo 4.2.31/4.2.32 */ +static void dynamic_update_bcn_check(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (!padapter->registrypriv.wifi_spec) + return; + + if (!padapter->registrypriv.ht_enable || !is_supported_ht(padapter->registrypriv.wireless_mode)) + return; + + if (!MLME_IS_AP(padapter)) + return; + + if (pmlmeext->bstart_bss) { + /* In 10 * 2 = 20s, there are no legacy AP, update HT info */ + static u8 count = 1; + + if (count % 10 == 0) { + count = 1; +#ifdef CONFIG_80211N_HT + if (_FALSE == ATOMIC_READ(&pmlmepriv->olbc) + && _FALSE == ATOMIC_READ(&pmlmepriv->olbc_ht)) { + + if (rtw_ht_operation_update(padapter) > 0) { + update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, _FALSE, 0); + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _TRUE, 0); + } + } +#endif /* CONFIG_80211N_HT */ + } + +#ifdef CONFIG_80211N_HT + /* In 2s, there are any legacy AP, update HT info, and then reset count */ + + if (_FALSE != ATOMIC_READ(&pmlmepriv->olbc) + && _FALSE != ATOMIC_READ(&pmlmepriv->olbc_ht)) { + + if (rtw_ht_operation_update(padapter) > 0) { + update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, _FALSE, 0); + update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _TRUE, 0); + + } + ATOMIC_SET(&pmlmepriv->olbc, _FALSE); + ATOMIC_SET(&pmlmepriv->olbc_ht, _FALSE); + count = 0; + } +#endif /* CONFIG_80211N_HT */ + count ++; + } +} +#endif /* CONFIG_AP_MODE */ + +void rtw_iface_dynamic_chk_wk_hdl(_adapter *padapter) +{ + #ifdef CONFIG_AP_MODE + #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + if (MLME_IS_AP(padapter) || MLME_IS_MESH(padapter)) { + expire_timeout_chk(padapter); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter) && MLME_IS_ASOC(padapter)) + rtw_mesh_peer_status_chk(padapter); + #endif + } + #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */ + + dynamic_update_bcn_check(padapter); + + #endif /* CONFIG_AP_MODE */ + + linked_status_chk(padapter, 0); + traffic_status_watchdog(padapter, 0); + + /* for debug purpose */ + _linked_info_dump(padapter); + +#ifdef CONFIG_RTW_CFGVENDOR_RSSIMONITOR + rtw_cfgvendor_rssi_monitor_evt(padapter); +#endif + + +} +void rtw_dynamic_chk_wk_hdl(_adapter *padapter) +{ + rtw_mi_dynamic_chk_wk_hdl(padapter); +#ifdef CONFIG_MP_INCLUDED + if (rtw_mp_mode_check(padapter) == _FALSE) +#endif + { +#ifdef DBG_CONFIG_ERROR_DETECT + rtw_hal_sreset_xmit_status_check(padapter); + rtw_hal_sreset_linked_status_check(padapter); +#endif + } + + /* if(check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_SURVEY)==_FALSE) */ + { +#ifdef DBG_RX_COUNTER_DUMP + rtw_dump_rx_counters(padapter); +#endif + dm_DynamicUsbTxAgg(padapter, 0); + } + rtw_hal_dm_watchdog(padapter); + + /* check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type); */ + +#ifdef CONFIG_BT_COEXIST + /* BT-Coexist */ + rtw_btcoex_Handler(padapter); +#endif + +#ifdef CONFIG_RTW_MULTI_AP + rtw_ch_util_rpt(padapter); +#endif + +#ifdef CONFIG_DFS_MASTER + rtw_chset_chk_non_ocp_finish(adapter_to_rfctl(padapter)); +#endif + +#ifdef CONFIG_IPS_CHECK_IN_WD + /* always call rtw_ps_processor() at last one. */ + rtw_ps_processor(padapter); +#endif + +#ifdef CONFIG_MCC_MODE + rtw_hal_mcc_sw_status_check(padapter); +#endif /* CONFIG_MCC_MODE */ + + rtw_hal_periodic_tsf_update_chk(padapter); +} + +#ifdef CONFIG_LPS +struct lps_ctrl_wk_parm { + s8 lps_level; + #ifdef CONFIG_LPS_1T1R + s8 lps_1t1r; + #endif +}; + +void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type, u8 *buf) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct lps_ctrl_wk_parm *parm = (struct lps_ctrl_wk_parm *)buf; + u8 mstatus; + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) + || (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE)) + return; + + switch (lps_ctrl_type) { + case LPS_CTRL_SCAN: + /* RTW_INFO("LPS_CTRL_SCAN\n"); */ +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_ScanNotify(padapter, _TRUE); +#endif /* CONFIG_BT_COEXIST */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) { + /* connect */ + LPS_Leave(padapter, "LPS_CTRL_SCAN"); + } + break; + case LPS_CTRL_JOINBSS: + /* RTW_INFO("LPS_CTRL_JOINBSS\n"); */ + LPS_Leave(padapter, "LPS_CTRL_JOINBSS"); + break; + case LPS_CTRL_CONNECT: + /* RTW_INFO("LPS_CTRL_CONNECT\n"); */ + mstatus = 1;/* connect */ + /* Reset LPS Setting */ + pwrpriv->LpsIdleCount = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus)); +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_MediaStatusNotify(padapter, mstatus); +#endif /* CONFIG_BT_COEXIST */ + break; + case LPS_CTRL_DISCONNECT: + /* RTW_INFO("LPS_CTRL_DISCONNECT\n"); */ + mstatus = 0;/* disconnect */ +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_MediaStatusNotify(padapter, mstatus); +#endif /* CONFIG_BT_COEXIST */ + LPS_Leave(padapter, "LPS_CTRL_DISCONNECT"); + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus)); + break; + case LPS_CTRL_SPECIAL_PACKET: + /* RTW_INFO("LPS_CTRL_SPECIAL_PACKET\n"); */ + rtw_set_lps_deny(padapter, LPS_DELAY_MS); +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP); +#endif /* CONFIG_BT_COEXIST */ + LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET"); + break; + case LPS_CTRL_LEAVE: + LPS_Leave(padapter, "LPS_CTRL_LEAVE"); + break; + case LPS_CTRL_LEAVE_SET_OPTION: + LPS_Leave(padapter, "LPS_CTRL_LEAVE_SET_OPTION"); + if (parm) { + if (parm->lps_level >= 0) + pwrpriv->lps_level = parm->lps_level; + #ifdef CONFIG_LPS_1T1R + if (parm->lps_1t1r >= 0) + pwrpriv->lps_1t1r = parm->lps_1t1r; + #endif + } + break; + case LPS_CTRL_LEAVE_CFG80211_PWRMGMT: + LPS_Leave(padapter, "CFG80211_PWRMGMT"); + break; + case LPS_CTRL_TRAFFIC_BUSY: + LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY"); + break; + case LPS_CTRL_TX_TRAFFIC_LEAVE: + LPS_Leave(padapter, "LPS_CTRL_TX_TRAFFIC_LEAVE"); + break; + case LPS_CTRL_RX_TRAFFIC_LEAVE: + LPS_Leave(padapter, "LPS_CTRL_RX_TRAFFIC_LEAVE"); + break; + case LPS_CTRL_ENTER: + LPS_Enter(padapter, "TRAFFIC_IDLE_1"); + break; + default: + break; + } + +} + +static u8 _rtw_lps_ctrl_wk_cmd(_adapter *adapter, u8 lps_ctrl_type, s8 lps_level, s8 lps_1t1r, u8 flags) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct lps_ctrl_wk_parm *wk_parm = NULL; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (lps_ctrl_type == LPS_CTRL_LEAVE_SET_OPTION) { + wk_parm = rtw_zmalloc(sizeof(*wk_parm)); + if (wk_parm == NULL) { + res = _FAIL; + goto exit; + } + wk_parm->lps_level = lps_level; + #ifdef CONFIG_LPS_1T1R + wk_parm->lps_1t1r = lps_1t1r; + #endif + } + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly */ + lps_ctrl_wk_hdl(adapter, lps_ctrl_type, (u8 *)wk_parm); + if (wk_parm) + rtw_mfree(wk_parm, sizeof(*wk_parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + parm = rtw_zmalloc(sizeof(*parm)); + if (parm == NULL) { + if (wk_parm) + rtw_mfree(wk_parm, sizeof(*wk_parm)); + res = _FAIL; + goto exit; + } + + parm->ec_id = LPS_CTRL_WK_CID; + parm->type = lps_ctrl_type; + parm->size = wk_parm ? sizeof(*wk_parm) : 0; + parm->pbuf = (u8 *)wk_parm; + + cmdobj = rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + rtw_mfree(parm, sizeof(*parm)); + if (wk_parm) + rtw_mfree(wk_parm, sizeof(*wk_parm)); + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + } + +exit: + return res; +} + +u8 rtw_lps_ctrl_wk_cmd(_adapter *adapter, u8 lps_ctrl_type, u8 flags) +{ + return _rtw_lps_ctrl_wk_cmd(adapter, lps_ctrl_type, -1, -1, flags); +} + +u8 rtw_lps_ctrl_leave_set_level_cmd(_adapter *adapter, u8 lps_level, u8 flags) +{ + return _rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_LEAVE_SET_OPTION, lps_level, -1, flags); +} + +#ifdef CONFIG_LPS_1T1R +u8 rtw_lps_ctrl_leave_set_1t1r_cmd(_adapter *adapter, u8 lps_1t1r, u8 flags) +{ + return _rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_LEAVE_SET_OPTION, -1, lps_1t1r, flags); +} +#endif + +void rtw_dm_in_lps_hdl(_adapter *padapter) +{ + rtw_hal_set_hwreg(padapter, HW_VAR_DM_IN_LPS_LCLK, NULL); +} + +u8 rtw_dm_in_lps_wk_cmd(_adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = DM_IN_LPS_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +void rtw_lps_change_dtim_hdl(_adapter *padapter, u8 dtim) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + if (dtim <= 0 || dtim > 16) + return; + +#ifdef CONFIG_BT_COEXIST + if (rtw_btcoex_IsBtControlLps(padapter) == _TRUE) + return; +#endif + +#ifdef CONFIG_LPS_LCLK + _enter_pwrlock(&pwrpriv->lock); +#endif + + if (pwrpriv->dtim != dtim) { + RTW_INFO("change DTIM from %d to %d, bFwCurrentInPSMode=%d, ps_mode=%d\n", pwrpriv->dtim, dtim, + pwrpriv->bFwCurrentInPSMode, pwrpriv->pwr_mode); + + pwrpriv->dtim = dtim; + } + + if ((pwrpriv->bFwCurrentInPSMode == _TRUE) && (pwrpriv->pwr_mode > PS_MODE_ACTIVE)) { + u8 ps_mode = pwrpriv->pwr_mode; + + /* RTW_INFO("change DTIM from %d to %d, ps_mode=%d\n", pwrpriv->dtim, dtim, ps_mode); */ + + rtw_exec_lps(padapter, ps_mode); + } + +#ifdef CONFIG_LPS_LCLK + _exit_pwrlock(&pwrpriv->lock); +#endif + +} + +#endif + +u8 rtw_lps_change_dtim_cmd(_adapter *padapter, u8 dtim) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + /* + #ifdef CONFIG_CONCURRENT_MODE + if (padapter->hw_port != HW_PORT0) + return res; + #endif + */ + { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = LPS_CHANGE_DTIM_CID; + pdrvextra_cmd_parm->type = dtim; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } + +exit: + + return res; + +} + +#if (RATE_ADAPTIVE_SUPPORT == 1) +void rpt_timer_setting_wk_hdl(_adapter *padapter, u16 minRptTime) +{ + rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&minRptTime)); +} + +u8 rtw_rpt_timer_cfg_cmd(_adapter *padapter, u16 minRptTime) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID; + pdrvextra_cmd_parm->type = minRptTime; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + res = rtw_enqueue_cmd(pcmdpriv, ph2c); +exit: + + + return res; + +} + +#endif + +#ifdef CONFIG_ANTENNA_DIVERSITY +void antenna_select_wk_hdl(_adapter *padapter, u8 antenna) +{ + rtw_hal_set_odm_var(padapter, HAL_ODM_ANTDIV_SELECT, &antenna, _TRUE); +} + +u8 rtw_antenna_select_cmd(_adapter *padapter, u8 antenna, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + u8 bSupportAntDiv = _FALSE; + u8 res = _SUCCESS; + int i; + + rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv)); + if (_FALSE == bSupportAntDiv) + return _FAIL; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (rtw_linked_check(dvobj->padapters[i])) + return _FAIL; + } + + if (_TRUE == enqueue) { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID; + pdrvextra_cmd_parm->type = antenna; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else + antenna_select_wk_hdl(padapter, antenna); +exit: + + + return res; + +} +#endif + +void rtw_dm_ra_mask_hdl(_adapter *padapter, struct sta_info *psta) +{ + if (psta) + set_sta_rate(padapter, psta); +} + +u8 rtw_dm_ra_mask_wk_cmd(_adapter *padapter, u8 *psta) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = DM_RA_MSK_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = psta; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +void power_saving_wk_hdl(_adapter *padapter) +{ + rtw_ps_processor(padapter); +} + +/* add for CONFIG_IEEE80211W, none 11w can use it */ +void reset_securitypriv_hdl(_adapter *padapter) +{ + rtw_reset_securitypriv(padapter); +} + +#ifdef CONFIG_P2P +u8 p2p_protocol_wk_cmd(_adapter *padapter, int intCmdType) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return res; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID; + pdrvextra_cmd_parm->type = intCmdType; /* As the command tppe. */ + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; /* Must be NULL here */ + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + + return res; + +} +#endif /* CONFIG_P2P */ + +#ifdef CONFIG_IOCTL_CFG80211 +static struct rtw_roch_parm *rtw_alloc_roch_parm(_adapter *adapter + , u64 cookie, struct wireless_dev *wdev + , struct ieee80211_channel *ch, enum nl80211_channel_type ch_type + , unsigned int duration + , u8 flags +) +{ + struct rtw_roch_parm *roch_parm; + bool cancel = duration ? 0 : 1; + + roch_parm = (struct rtw_roch_parm *)rtw_zmalloc(sizeof(struct rtw_roch_parm)); + if (!roch_parm) + return NULL; + + roch_parm->cookie = cookie; + roch_parm->wdev = wdev; + if (!cancel) { + _rtw_memcpy(&roch_parm->ch, ch, sizeof(struct ieee80211_channel)); + roch_parm->ch_type = ch_type; + roch_parm->duration = duration; + } + + return roch_parm; +} + +inline u8 rtw_roch_cmd(_adapter *adapter + , u64 cookie, struct wireless_dev *wdev + , struct ieee80211_channel *ch, enum nl80211_channel_type ch_type + , unsigned int duration + , u8 flags +) +{ + struct rtw_roch_parm *roch_parm; + + roch_parm = rtw_alloc_roch_parm(adapter, cookie, wdev, ch, ch_type, duration, flags); + if (!roch_parm) + return _FAIL; + + return rtw_roch_wk_cmd(adapter, ROCH_RO_CH_WK, roch_parm, flags); +} + +inline u8 rtw_cancel_roch_cmd(_adapter *adapter, u64 cookie, struct wireless_dev *wdev, u8 flags) +{ + struct rtw_roch_parm *roch_parm; + + roch_parm = rtw_alloc_roch_parm(adapter, cookie, wdev, NULL, 0, 0, flags); + if (!roch_parm) + return _FAIL; + + return rtw_roch_wk_cmd(adapter, ROCH_CANCEL_RO_CH_WK, roch_parm, flags); +} + +inline u8 rtw_mgnt_tx_cmd(_adapter *adapter, u8 tx_ch, u8 no_cck, const u8 *buf, size_t len, int wait_ack, u8 flags) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct mgnt_tx_parm *mgnt_parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + mgnt_parm = (struct mgnt_tx_parm *)rtw_zmalloc(sizeof(struct mgnt_tx_parm)); + if (mgnt_parm == NULL) { + res = _FAIL; + goto exit; + } + + mgnt_parm->tx_ch = tx_ch; + mgnt_parm->no_cck = no_cck; + mgnt_parm->buf = buf; + mgnt_parm->len = len; + mgnt_parm->wait_ack = wait_ack; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != rtw_mgnt_tx_handler(adapter, (u8 *)mgnt_parm)) + res = _FAIL; + rtw_mfree((u8 *)mgnt_parm, sizeof(*mgnt_parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + rtw_mfree((u8 *)mgnt_parm, sizeof(*mgnt_parm)); + res = _FAIL; + goto exit; + } + + parm->ec_id = MGNT_TX_WK_CID; + parm->type = 0; + parm->size = sizeof(*mgnt_parm); + parm->pbuf = (u8 *)mgnt_parm; + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)mgnt_parm, sizeof(*mgnt_parm)); + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + } + +exit: + return res; +} +#endif + +u8 rtw_ps_cmd(_adapter *padapter) +{ + struct cmd_obj *ppscmd; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + u8 res = _SUCCESS; + +#ifdef CONFIG_CONCURRENT_MODE + if (!is_primary_adapter(padapter)) + goto exit; +#endif + + ppscmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ppscmd == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ppscmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ppscmd); + +exit: + + + return res; + +} + +#if CONFIG_DFS +void rtw_dfs_ch_switch_hdl(struct dvobj_priv *dvobj) +{ + struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj); + _adapter *pri_adapter = dvobj_get_primary_adapter(dvobj); + struct mlme_ext_priv *pmlmeext = &pri_adapter->mlmeextpriv; + u8 ifbmp_m = rtw_mi_get_ap_mesh_ifbmp(pri_adapter); + u8 ifbmp_s = rtw_mi_get_ld_sta_ifbmp(pri_adapter); + s16 req_ch; + u8 req_bw = CHANNEL_WIDTH_20, req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + rtw_hal_macid_sleep_all_used(pri_adapter); + + if (rtw_chset_search_ch(rfctl->channel_set, rfctl->csa_ch) >= 0 + && !rtw_chset_is_ch_non_ocp(rfctl->channel_set, rfctl->csa_ch) + ) { + /* CSA channel available and valid */ + req_ch = rfctl->csa_ch; + RTW_INFO("%s valid CSA ch%u\n", __func__, rfctl->csa_ch); + } else if (ifbmp_m) { + /* no available or valid CSA channel, having AP/MESH ifaces */ + req_ch = REQ_CH_NONE; + RTW_INFO("%s ch sel by AP/MESH ifaces\n", __func__); + } else { + /* no available or valid CSA channel and no AP/MESH ifaces */ + if (!IsSupported24G(dvobj_to_regsty(dvobj)->wireless_mode) + #ifdef CONFIG_DFS_MASTER + || rfctl->radar_detected + #endif + ) + req_ch = 36; + else + req_ch = 1; + RTW_INFO("%s switch to ch%d\n", __func__, req_ch); + } + + /* only support 80 Mhz so far */ + if(rfctl->csa_ch_width == 1 || rfctl->csa_ch_width == 2 || rfctl->csa_ch_width == 3) { + if (rtw_get_offset_by_chbw(req_ch, CHANNEL_WIDTH_80, &req_offset)) { + req_bw = CHANNEL_WIDTH_80; + } else { + req_bw = CHANNEL_WIDTH_20; + req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + } else if(rfctl->csa_ch_offset == 1) { + req_bw = CHANNEL_WIDTH_40; + req_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + } else if(rfctl->csa_ch_offset == 3) { + req_bw = CHANNEL_WIDTH_40; + req_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + } else{ + req_bw = CHANNEL_WIDTH_20; + req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + RTW_INFO("req_ch=%d, req_bw=%d, req_offset=%d, ifbmp_m=0x%02x, ifbmp_s=0x%02x\n" + , req_ch, req_bw, req_offset, ifbmp_m, ifbmp_s); + + /* update ch, bw, offset for all asoc STA ifaces */ + if (ifbmp_s) { + _adapter *iface; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface || !(ifbmp_s & BIT(iface->iface_id))) + continue; + + /* update STA mode ch/bw/offset */ + iface->mlmeextpriv.cur_channel = req_ch; + iface->mlmeextpriv.cur_bwmode = req_bw; + iface->mlmeextpriv.cur_ch_offset = req_offset; + /* updaet STA mode DSConfig , ap mode will update in rtw_change_bss_chbw_cmd */ + iface->mlmepriv.cur_network.network.Configuration.DSConfig = req_ch; + set_fwstate(&iface->mlmepriv, WIFI_CSA_UPDATE_BEACON); + + } + } + + if (rfctl->csa_ch > 0) { + RTW_INFO("pmlmeext->csa_timer 70 seconds\n"); + /* wait 70 seconds for receiving beacons */ + _set_timer(&pmlmeext->csa_timer, CAC_TIME_MS + 10000); + } + +#ifdef CONFIG_AP_MODE + if (ifbmp_m) { + /* trigger channel selection with consideraton of asoc STA ifaces */ + rtw_change_bss_chbw_cmd(dvobj_get_primary_adapter(dvobj), RTW_CMDF_DIRECTLY + , ifbmp_m, 0, req_ch, REQ_BW_ORI, REQ_OFFSET_NONE); + } else +#endif + { + /* no AP/MESH iface, switch DFS status and channel directly */ + rtw_warn_on(req_ch <= 0); + #ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(pri_adapter, MLME_OPCH_SWITCH, 0); + #endif + LeaveAllPowerSaveModeDirect(pri_adapter); + set_channel_bwmode(pri_adapter, req_ch, req_offset, req_bw); + /* update union ch/bw/offset for STA only */ + rtw_mi_update_union_chan_inf(pri_adapter, req_ch, req_offset, req_bw); + rtw_rfctl_update_op_mode(rfctl, 0, 0); + } + + rfctl->csa_ch = 0; + rfctl->csa_switch_cnt = 0; + rfctl->csa_ch_offset = 0; + rfctl->csa_ch_width = 0; + rfctl->csa_ch_freq_seg0 = 0; + rfctl->csa_ch_freq_seg1 = 0; + + rtw_hal_macid_wakeup_all_used(pri_adapter); + rtw_mi_os_xmit_schedule(pri_adapter); +} +#endif /* CONFIG_DFS */ + +#ifdef CONFIG_AP_MODE + +static void rtw_chk_hi_queue_hdl(_adapter *padapter) +{ + struct sta_info *psta_bmc; + struct sta_priv *pstapriv = &padapter->stapriv; + systime start = rtw_get_current_time(); + u8 empty = _FALSE; + + psta_bmc = rtw_get_bcmc_stainfo(padapter); + if (!psta_bmc) + return; + + rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty); + + while (_FALSE == empty && rtw_get_passing_time_ms(start) < rtw_get_wait_hiq_empty_ms()) { + rtw_msleep_os(100); + rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty); + } + + if (psta_bmc->sleepq_len == 0) { + if (empty == _SUCCESS) { + bool update_tim = _FALSE; + + if (rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, 0)) + update_tim = _TRUE; + + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, 0); + rtw_tim_map_clear(padapter, pstapriv->sta_dz_bitmap, 0); + + if (update_tim == _TRUE) + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0,"bmc sleepq and HIQ empty"); + } else /* re check again */ + rtw_chk_hi_queue_cmd(padapter); + + } + +} + +u8 rtw_chk_hi_queue_cmd(_adapter *padapter) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + return res; + +} + +#ifdef CONFIG_DFS_MASTER +u8 rtw_dfs_rd_hdl(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + u8 cch; + + if (!rfctl->radar_detect_enabled) + goto exit; + + cch = rtw_get_center_ch(rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset); + + if (dvobj->oper_channel != rfctl->radar_detect_ch + || rtw_get_passing_time_ms(rtw_get_on_oper_ch_time(adapter)) < 300 + ) { + /* offchannel, bypass radar detect */ + goto cac_status_chk; + } + + if (IS_CH_WAITING(rfctl) && !IS_UNDER_CAC(rfctl)) { + /* non_ocp, bypass radar detect */ + goto cac_status_chk; + } + + if (!rfctl->dbg_dfs_fake_radar_detect_cnt + && rtw_odm_radar_detect(adapter) != _TRUE) + goto cac_status_chk; + + if (!rfctl->dbg_dfs_fake_radar_detect_cnt + && rfctl->dbg_dfs_radar_detect_trigger_non + ) { + /* radar detect debug mode, trigger no mlme flow */ + RTW_INFO("%s radar detected on test mode, trigger no mlme flow\n", __func__); + goto cac_status_chk; + } + + if (rfctl->dbg_dfs_fake_radar_detect_cnt != 0) { + RTW_INFO("%s fake radar detected, cnt:%d\n", __func__ + , rfctl->dbg_dfs_fake_radar_detect_cnt); + rfctl->dbg_dfs_fake_radar_detect_cnt--; + } else + RTW_INFO("%s radar detected\n", __func__); + + rfctl->radar_detected = 1; + + rtw_chset_update_non_ocp(rfctl->channel_set + , rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset); + + if (IS_UNDER_CAC(rfctl)) + rtw_nlrtw_cac_abort_event(adapter, cch, rfctl->radar_detect_bw); + rtw_nlrtw_radar_detect_event(adapter, cch, rfctl->radar_detect_bw); + + rtw_dfs_ch_switch_hdl(dvobj); + + if (rfctl->radar_detect_enabled) + goto set_timer; + goto exit; + +cac_status_chk: + + if (!IS_CAC_STOPPED(rfctl) + && ((IS_UNDER_CAC(rfctl) && rfctl->cac_force_stop) + || !IS_CH_WAITING(rfctl) + ) + ) { + u8 pause = 0x00; + + rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &pause); + rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED; + rtw_nlrtw_cac_finish_event(adapter, cch, rfctl->radar_detect_bw); + + if (rtw_mi_check_fwstate(adapter, WIFI_UNDER_LINKING|WIFI_UNDER_SURVEY) == _FALSE) { + u8 doiqk = _TRUE; + u8 u_ch, u_bw, u_offset; + + rtw_hal_set_hwreg(adapter , HW_VAR_DO_IQK , &doiqk); + + if (rtw_mi_get_ch_setting_union(adapter, &u_ch, &u_bw, &u_offset)) + set_channel_bwmode(adapter, u_ch, u_offset, u_bw); + else + rtw_warn_on(1); + + doiqk = _FALSE; + rtw_hal_set_hwreg(adapter , HW_VAR_DO_IQK , &doiqk); + + #ifdef CONFIG_AP_MODE + ResumeTxBeacon(adapter); + rtw_mi_tx_beacon_hdl(adapter); + #endif + } + } + +set_timer: + _set_timer(&rfctl->radar_detect_timer + , rtw_odm_radar_detect_polling_int_ms(dvobj)); + +exit: + return H2C_SUCCESS; +} + +u8 rtw_dfs_rd_cmd(_adapter *adapter, bool enqueue) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + u8 res = _FAIL; + + if (enqueue) { + cmdobj = rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) + goto exit; + + parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + rtw_mfree(cmdobj, sizeof(struct cmd_obj)); + goto exit; + } + + parm->ec_id = DFS_RADAR_DETECT_WK_CID; + parm->type = 0; + parm->size = 0; + parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + } else { + rtw_dfs_rd_hdl(adapter); + res = _SUCCESS; + } + +exit: + return res; +} + +void rtw_dfs_rd_timer_hdl(void *ctx) +{ + struct rf_ctl_t *rfctl = (struct rf_ctl_t *)ctx; + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + + rtw_dfs_rd_cmd(dvobj_get_primary_adapter(dvobj), _TRUE); +} + +static void rtw_dfs_rd_enable(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset, bool bypass_cac) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + + RTW_INFO("%s on %u,%u,%u\n", __func__, ch, bw, offset); + + if (bypass_cac) + rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED; + else if (rtw_is_cac_reset_needed(rfctl, ch, bw, offset) == _TRUE) + rtw_reset_cac(rfctl, ch, bw, offset); + + rfctl->radar_detect_by_others = _FALSE; + rfctl->radar_detect_ch = ch; + rfctl->radar_detect_bw = bw; + rfctl->radar_detect_offset = offset; + + rfctl->radar_detected = 0; + + if (IS_CH_WAITING(rfctl)) + StopTxBeacon(adapter); + + if (!rfctl->radar_detect_enabled) { + RTW_INFO("%s set radar_detect_enabled\n", __func__); + rfctl->radar_detect_enabled = 1; + #ifdef CONFIG_LPS + LPS_Leave(adapter, "RADAR_DETECT_EN"); + #endif + _set_timer(&rfctl->radar_detect_timer + , rtw_odm_radar_detect_polling_int_ms(dvobj)); + + if (rtw_rfctl_overlap_radar_detect_ch(rfctl)) { + if (IS_CH_WAITING(rfctl)) { + u8 pause = 0xFF; + + rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &pause); + } + rtw_odm_radar_detect_enable(adapter); + } + } +} + +static void rtw_dfs_rd_disable(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset, bool by_others) +{ + _adapter *adapter = dvobj_get_primary_adapter(rfctl_to_dvobj(rfctl)); + + rfctl->radar_detect_by_others = by_others; + + if (rfctl->radar_detect_enabled) { + bool overlap_radar_detect_ch = rtw_rfctl_overlap_radar_detect_ch(rfctl); + + RTW_INFO("%s clear radar_detect_enabled\n", __func__); + + rfctl->radar_detect_enabled = 0; + rfctl->radar_detected = 0; + rfctl->radar_detect_ch = 0; + rfctl->radar_detect_bw = 0; + rfctl->radar_detect_offset = 0; + rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED; + _cancel_timer_ex(&rfctl->radar_detect_timer); + + if (rtw_mi_check_fwstate(adapter, WIFI_UNDER_LINKING|WIFI_UNDER_SURVEY) == _FALSE) { + ResumeTxBeacon(adapter); + rtw_mi_tx_beacon_hdl(adapter); + } + + if (overlap_radar_detect_ch) { + u8 pause = 0x00; + + rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &pause); + rtw_odm_radar_detect_disable(adapter); + } + } + + if (by_others) { + rfctl->radar_detect_ch = ch; + rfctl->radar_detect_bw = bw; + rfctl->radar_detect_offset = offset; + } else { + rfctl->radar_detect_ch = 0; + rfctl->radar_detect_bw = 0; + rfctl->radar_detect_offset = 0; + } +} + +void rtw_dfs_rd_en_decision(_adapter *adapter, u8 mlme_act, u8 excl_ifbmp) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct mi_state mstate; + u8 ifbmp; + u8 u_ch, u_bw, u_offset; + bool ld_sta_in_dfs = _FALSE; + bool sync_ch = _FALSE; /* _FALSE: asign channel directly */ + bool needed = _FALSE; + + if (mlme_act == MLME_OPCH_SWITCH + || mlme_act == MLME_ACTION_NONE + ) { + ifbmp = ~excl_ifbmp; + rtw_mi_status_by_ifbmp(dvobj, ifbmp, &mstate); + rtw_mi_get_ch_setting_union_by_ifbmp(dvobj, ifbmp, &u_ch, &u_bw, &u_offset); + } else { + ifbmp = ~excl_ifbmp & ~BIT(adapter->iface_id); + rtw_mi_status_by_ifbmp(dvobj, ifbmp, &mstate); + rtw_mi_get_ch_setting_union_by_ifbmp(dvobj, ifbmp, &u_ch, &u_bw, &u_offset); + if (u_ch != 0) + sync_ch = _TRUE; + + switch (mlme_act) { + case MLME_STA_CONNECTING: + MSTATE_STA_LG_NUM(&mstate)++; + break; + case MLME_STA_CONNECTED: + MSTATE_STA_LD_NUM(&mstate)++; + break; + case MLME_STA_DISCONNECTED: + break; +#ifdef CONFIG_AP_MODE + case MLME_AP_STARTED: + MSTATE_AP_NUM(&mstate)++; + break; + case MLME_AP_STOPPED: + break; +#endif +#ifdef CONFIG_RTW_MESH + case MLME_MESH_STARTED: + MSTATE_MESH_NUM(&mstate)++; + break; + case MLME_MESH_STOPPED: + break; +#endif + default: + rtw_warn_on(1); + break; + } + + if (sync_ch == _TRUE) { + if (!MLME_IS_OPCH_SW(adapter)) { + if (!rtw_is_chbw_grouped(mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset, u_ch, u_bw, u_offset)) { + RTW_INFO(FUNC_ADPT_FMT" can't sync %u,%u,%u with %u,%u,%u\n", FUNC_ADPT_ARG(adapter) + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset, u_ch, u_bw, u_offset); + goto apply; + } + + rtw_sync_chbw(&mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset + , &u_ch, &u_bw, &u_offset); + } + } else { + u_ch = mlmeext->cur_channel; + u_bw = mlmeext->cur_bwmode; + u_offset = mlmeext->cur_ch_offset; + } + } + + if (MSTATE_STA_LG_NUM(&mstate) > 0) { + /* STA mode is linking */ + goto apply; + } + + if (MSTATE_STA_LD_NUM(&mstate) > 0) { + if (rtw_chset_is_dfs_chbw(rfctl->channel_set, u_ch, u_bw, u_offset)) { + /* + * if operate as slave w/o radar detect, + * rely on AP on which STA mode connects + */ + if (IS_DFS_SLAVE_WITH_RD(rfctl) && !rtw_rfctl_dfs_domain_unknown(rfctl)) + needed = _TRUE; + ld_sta_in_dfs = _TRUE; + } + goto apply; + } + + if (!MSTATE_AP_NUM(&mstate) && !MSTATE_MESH_NUM(&mstate)) { + /* No working AP/Mesh mode */ + goto apply; + } + + if (rtw_chset_is_dfs_chbw(rfctl->channel_set, u_ch, u_bw, u_offset)) + needed = _TRUE; + +apply: + + RTW_INFO(FUNC_ADPT_FMT" needed:%d, mlme_act:%u, excl_ifbmp:0x%02x\n" + , FUNC_ADPT_ARG(adapter), needed, mlme_act, excl_ifbmp); + RTW_INFO(FUNC_ADPT_FMT" ld_sta_num:%u, lg_sta_num:%u, ap_num:%u, mesh_num:%u, %u,%u,%u\n" + , FUNC_ADPT_ARG(adapter), MSTATE_STA_LD_NUM(&mstate), MSTATE_STA_LG_NUM(&mstate) + , MSTATE_AP_NUM(&mstate), MSTATE_MESH_NUM(&mstate) + , u_ch, u_bw, u_offset); + + if (needed == _TRUE) + rtw_dfs_rd_enable(rfctl, u_ch, u_bw, u_offset, ld_sta_in_dfs); + else + rtw_dfs_rd_disable(rfctl, u_ch, u_bw, u_offset, ld_sta_in_dfs); +} + +u8 rtw_dfs_rd_en_decision_cmd(_adapter *adapter) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + u8 res = _FAIL; + + cmdobj = rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) + goto exit; + + parm = rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + rtw_mfree(cmdobj, sizeof(struct cmd_obj)); + goto exit; + } + + parm->ec_id = DFS_RADAR_DETECT_EN_DEC_WK_CID; + parm->type = 0; + parm->size = 0; + parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + +exit: + return res; +} +#endif /* CONFIG_DFS_MASTER */ + +#endif /* CONFIG_AP_MODE */ + +#ifdef CONFIG_BT_COEXIST +struct btinfo { + u8 cid; + u8 len; + + u8 bConnection:1; + u8 bSCOeSCO:1; + u8 bInQPage:1; + u8 bACLBusy:1; + u8 bSCOBusy:1; + u8 bHID:1; + u8 bA2DP:1; + u8 bFTP:1; + + u8 retry_cnt:4; + u8 rsvd_34:1; + u8 rsvd_35:1; + u8 rsvd_36:1; + u8 rsvd_37:1; + + u8 rssi; + + u8 rsvd_50:1; + u8 rsvd_51:1; + u8 rsvd_52:1; + u8 rsvd_53:1; + u8 rsvd_54:1; + u8 rsvd_55:1; + u8 eSCO_SCO:1; + u8 Master_Slave:1; + + u8 rsvd_6; + u8 rsvd_7; +}; + +void btinfo_evt_dump(void *sel, void *buf) +{ + struct btinfo *info = (struct btinfo *)buf; + + RTW_PRINT_SEL(sel, "cid:0x%02x, len:%u\n", info->cid, info->len); + + if (info->len > 2) + RTW_PRINT_SEL(sel, "byte2:%s%s%s%s%s%s%s%s\n" + , info->bConnection ? "bConnection " : "" + , info->bSCOeSCO ? "bSCOeSCO " : "" + , info->bInQPage ? "bInQPage " : "" + , info->bACLBusy ? "bACLBusy " : "" + , info->bSCOBusy ? "bSCOBusy " : "" + , info->bHID ? "bHID " : "" + , info->bA2DP ? "bA2DP " : "" + , info->bFTP ? "bFTP" : "" + ); + + if (info->len > 3) + RTW_PRINT_SEL(sel, "retry_cnt:%u\n", info->retry_cnt); + + if (info->len > 4) + RTW_PRINT_SEL(sel, "rssi:%u\n", info->rssi); + + if (info->len > 5) + RTW_PRINT_SEL(sel, "byte5:%s%s\n" + , info->eSCO_SCO ? "eSCO_SCO " : "" + , info->Master_Slave ? "Master_Slave " : "" + ); +} + +static void rtw_btinfo_hdl(_adapter *adapter, u8 *buf, u16 buf_len) +{ +#define BTINFO_WIFI_FETCH 0x23 +#define BTINFO_BT_AUTO_RPT 0x27 +#ifdef CONFIG_BT_COEXIST_SOCKET_TRX + struct btinfo_8761ATV *info = (struct btinfo_8761ATV *)buf; +#else /* !CONFIG_BT_COEXIST_SOCKET_TRX */ + struct btinfo *info = (struct btinfo *)buf; +#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */ + u8 cmd_idx; + u8 len; + + cmd_idx = info->cid; + + if (info->len > buf_len - 2) { + rtw_warn_on(1); + len = buf_len - 2; + } else + len = info->len; + + /* #define DBG_PROC_SET_BTINFO_EVT */ +#ifdef DBG_PROC_SET_BTINFO_EVT +#ifdef CONFIG_BT_COEXIST_SOCKET_TRX + RTW_INFO("%s: btinfo[0]=%x,btinfo[1]=%x,btinfo[2]=%x,btinfo[3]=%x btinfo[4]=%x,btinfo[5]=%x,btinfo[6]=%x,btinfo[7]=%x\n" + , __func__, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]); +#else/* !CONFIG_BT_COEXIST_SOCKET_TRX */ + btinfo_evt_dump(RTW_DBGDUMP, info); +#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */ +#endif /* DBG_PROC_SET_BTINFO_EVT */ + + /* transform BT-FW btinfo to WiFI-FW C2H format and notify */ + if (cmd_idx == BTINFO_WIFI_FETCH) + buf[1] = 0; + else if (cmd_idx == BTINFO_BT_AUTO_RPT) + buf[1] = 2; +#ifdef CONFIG_BT_COEXIST_SOCKET_TRX + else if (0x01 == cmd_idx || 0x02 == cmd_idx) + buf[1] = buf[0]; +#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */ + rtw_btcoex_BtInfoNotify(adapter , len + 1, &buf[1]); +} + +u8 rtw_btinfo_cmd(_adapter *adapter, u8 *buf, u16 len) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + u8 *btinfo; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + btinfo = rtw_zmalloc(len); + if (btinfo == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = BTINFO_WK_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = len; + pdrvextra_cmd_parm->pbuf = btinfo; + + _rtw_memcpy(btinfo, buf, len); + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +static void rtw_btc_reduce_wl_txpwr_hdl(_adapter *adapter, u32 pwr_lvl) +{ + rtw_btcoex_set_reduced_wl_pwr_lvl(adapter, pwr_lvl); + rtw_btcoex_do_reduce_wl_pwr_lvl(adapter); + + RTW_INFO(FUNC_ADPT_FMT ": BTC reduce WL TxPwr %d dB!\n", + FUNC_ADPT_ARG(adapter), pwr_lvl); +} + +u8 rtw_btc_reduce_wl_txpwr_cmd(_adapter *adapter, u32 val) +{ + struct cmd_obj *pcmdobj; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = BTC_REDUCE_WL_TXPWR_CID; + pdrvextra_cmd_parm->type = val; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(pcmdobj, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + +exit: + return res; +} +#endif /* CONFIG_BT_COEXIST */ + +u8 rtw_test_h2c_cmd(_adapter *adapter, u8 *buf, u8 len) +{ + struct cmd_obj *pcmdobj; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + u8 *ph2c_content; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 res = _SUCCESS; + + pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmdobj == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + ph2c_content = rtw_zmalloc(len); + if (ph2c_content == NULL) { + rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = TEST_H2C_CID; + pdrvextra_cmd_parm->type = 0; + pdrvextra_cmd_parm->size = len; + pdrvextra_cmd_parm->pbuf = ph2c_content; + + _rtw_memcpy(ph2c_content, buf, len); + + init_h2fwcmd_w_parm_no_rsp(pcmdobj, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, pcmdobj); + +exit: + return res; +} + +#ifdef CONFIG_MP_INCLUDED +static s32 rtw_mp_cmd_hdl(_adapter *padapter, u8 mp_cmd_id) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + int ret = H2C_SUCCESS; + uint status = _SUCCESS; + + if (mp_cmd_id == MP_START) { + if (padapter->registrypriv.mp_mode == 0) { + rtw_intf_stop(padapter); + rtw_hal_deinit(padapter); + padapter->registrypriv.mp_mode = 1; +#ifdef CONFIG_BT_COEXIST + padapter->mppriv.CureFuseBTCoex = pHalData->EEPROMBluetoothCoexist; + pHalData->EEPROMBluetoothCoexist = _FALSE; +#endif +#ifdef CONFIG_RF_POWER_TRIM + if (!IS_HARDWARE_TYPE_8814A(padapter) && !IS_HARDWARE_TYPE_8822B(padapter) && !IS_HARDWARE_TYPE_8822C(padapter)) { + padapter->registrypriv.RegPwrTrimEnable = 1; + rtw_hal_read_chip_info(padapter); + } +#endif /*CONFIG_RF_POWER_TRIM*/ + rtw_reset_drv_sw(padapter); +#ifdef CONFIG_NEW_NETDEV_HDL + if (!rtw_is_hw_init_completed(padapter)) { + status = rtw_hal_init(padapter); + if (status == _FAIL) { + ret = H2C_REJECTED; + goto exit; + } + rtw_hal_iface_init(padapter); + } +#else + status = rtw_hal_init(padapter); + if (status == _FAIL) { + ret = H2C_REJECTED; + goto exit; + } +#endif /*CONFIG_NEW_NETDEV_HDL*/ +#ifndef RTW_HALMAC + rtw_intf_start(padapter); +#endif /* !RTW_HALMAC */ +#ifdef RTW_HALMAC /*for New IC*/ + MPT_InitializeAdapter(padapter, 1); +#endif /* CONFIG_MP_INCLUDED */ + } + + if (padapter->registrypriv.mp_mode == 0) { + ret = H2C_REJECTED; + goto exit; + } + + if (padapter->mppriv.mode == MP_OFF) { + if (mp_start_test(padapter) == _FAIL) { + ret = H2C_REJECTED; + goto exit; + } + padapter->mppriv.mode = MP_ON; + MPT_PwrCtlDM(padapter, 0); + } + padapter->mppriv.bmac_filter = _FALSE; +#ifdef CONFIG_RTL8723B +#ifdef CONFIG_USB_HCI + rtw_write32(padapter, 0x765, 0x0000); + rtw_write32(padapter, 0x948, 0x0280); +#else + rtw_write32(padapter, 0x765, 0x0000); + rtw_write32(padapter, 0x948, 0x0000); +#endif +#ifdef CONFIG_FOR_RTL8723BS_VQ0 + rtw_write32(padapter, 0x765, 0x0000); + rtw_write32(padapter, 0x948, 0x0280); +#endif + rtw_write8(padapter, 0x66, 0x27); /*Open BT uart Log*/ + rtw_write8(padapter, 0xc50, 0x20); /*for RX init Gain*/ +#endif + odm_write_dig(&pHalData->odmpriv, 0x20); + + } else if (mp_cmd_id == MP_STOP) { + if (padapter->registrypriv.mp_mode == 1) { + MPT_DeInitAdapter(padapter); + rtw_intf_stop(padapter); + rtw_hal_deinit(padapter); + padapter->registrypriv.mp_mode = 0; +#ifdef CONFIG_BT_COEXIST + pHalData->EEPROMBluetoothCoexist = padapter->mppriv.CureFuseBTCoex; +#endif + rtw_reset_drv_sw(padapter); +#ifdef CONFIG_NEW_NETDEV_HDL + if (!rtw_is_hw_init_completed(padapter)) { + status = rtw_hal_init(padapter); + if (status == _FAIL) { + ret = H2C_REJECTED; + goto exit; + } + rtw_hal_iface_init(padapter); + } +#else + status = rtw_hal_init(padapter); + if (status == _FAIL) { + ret = H2C_REJECTED; + goto exit; + } +#endif /*CONFIG_NEW_NETDEV_HDL*/ +#ifndef RTW_HALMAC + rtw_intf_start(padapter); +#endif /* !RTW_HALMAC */ + } + + if (padapter->mppriv.mode != MP_OFF) { + mp_stop_test(padapter); + padapter->mppriv.mode = MP_OFF; + } + + } else { + RTW_INFO(FUNC_ADPT_FMT"invalid id:%d\n", FUNC_ADPT_ARG(padapter), mp_cmd_id); + ret = H2C_PARAMETERS_ERROR; + rtw_warn_on(1); + } + +exit: + return ret; +} + +u8 rtw_mp_cmd(_adapter *adapter, u8 mp_cmd_id, u8 flags) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + + parm->ec_id = MP_CMD_WK_CID; + parm->type = mp_cmd_id; + parm->size = 0; + parm->pbuf = NULL; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != rtw_mp_cmd_hdl(adapter, mp_cmd_id)) + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + } else { + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + if (flags & RTW_CMDF_WAIT_ACK) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + } + +exit: + return res; +} +#endif /*CONFIG_MP_INCLUDED*/ + +#ifdef CONFIG_RTW_CUSTOMER_STR +static s32 rtw_customer_str_cmd_hdl(_adapter *adapter, u8 write, const u8 *cstr) +{ + int ret = H2C_SUCCESS; + + if (write) + ret = rtw_hal_h2c_customer_str_write(adapter, cstr); + else + ret = rtw_hal_h2c_customer_str_req(adapter); + + return ret == _SUCCESS ? H2C_SUCCESS : H2C_REJECTED; +} + +static u8 rtw_customer_str_cmd(_adapter *adapter, u8 write, const u8 *cstr) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + u8 *str = NULL; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + + if (write) { + str = rtw_zmalloc(RTW_CUSTOMER_STR_LEN); + if (str == NULL) { + rtw_mfree((u8 *)parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + } + + parm->ec_id = CUSTOMER_STR_WK_CID; + parm->type = write; + parm->size = write ? RTW_CUSTOMER_STR_LEN : 0; + parm->pbuf = write ? str : NULL; + + if (write) + _rtw_memcpy(str, cstr, RTW_CUSTOMER_STR_LEN); + + /* need enqueue, prepare cmd_obj and enqueue */ + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + if (write) + rtw_mfree(str, RTW_CUSTOMER_STR_LEN); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, 2 * 1000); + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + + if (res == _SUCCESS) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + +exit: + return res; +} + +inline u8 rtw_customer_str_req_cmd(_adapter *adapter) +{ + return rtw_customer_str_cmd(adapter, 0, NULL); +} + +inline u8 rtw_customer_str_write_cmd(_adapter *adapter, const u8 *cstr) +{ + return rtw_customer_str_cmd(adapter, 1, cstr); +} +#endif /* CONFIG_RTW_CUSTOMER_STR */ + +u8 rtw_c2h_wk_cmd(PADAPTER padapter, u8 *pbuf, u16 length, u8 type) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 *extra_cmd_buf; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + extra_cmd_buf = rtw_zmalloc(length); + if (extra_cmd_buf == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + _rtw_memcpy(extra_cmd_buf, pbuf, length); + pdrvextra_cmd_parm->ec_id = C2H_WK_CID; + pdrvextra_cmd_parm->type = type; + pdrvextra_cmd_parm->size = length; + pdrvextra_cmd_parm->pbuf = extra_cmd_buf; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; +} + +#ifdef CONFIG_FW_C2H_REG +inline u8 rtw_c2h_reg_wk_cmd(_adapter *adapter, u8 *c2h_evt) +{ + return rtw_c2h_wk_cmd(adapter, c2h_evt, c2h_evt ? C2H_REG_LEN : 0, C2H_TYPE_REG); +} +#endif + +#ifdef CONFIG_FW_C2H_PKT +inline u8 rtw_c2h_packet_wk_cmd(_adapter *adapter, u8 *c2h_evt, u16 length) +{ + return rtw_c2h_wk_cmd(adapter, c2h_evt, length, C2H_TYPE_PKT); +} +#endif + +static u8 _rtw_run_in_thread_cmd(_adapter *adapter, void (*func)(void *), void *context, s32 timeout_ms) +{ + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + struct cmd_obj *cmdobj; + struct RunInThread_param *parm; + struct submit_ctx sctx; + s32 res = _SUCCESS; + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (NULL == cmdobj) { + res = _FAIL; + goto exit; + } + + parm = (struct RunInThread_param *)rtw_zmalloc(sizeof(struct RunInThread_param)); + if (NULL == parm) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + parm->func = func; + parm->context = context; + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_RUN_INTHREAD); + + if (timeout_ms >= 0) { + cmdobj->sctx = &sctx; + rtw_sctx_init(&sctx, timeout_ms); + } + + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + + if (res == _SUCCESS && timeout_ms >= 0) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&cmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + cmdobj->sctx = NULL; + _exit_critical_mutex(&cmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + +exit: + return res; +} + +u8 rtw_run_in_thread_cmd(_adapter *adapter, void (*func)(void *), void *context) +{ + return _rtw_run_in_thread_cmd(adapter, func, context, -1); +} + +u8 rtw_run_in_thread_cmd_wait(_adapter *adapter, void (*func)(void *), void *context, s32 timeout_ms) +{ + return _rtw_run_in_thread_cmd(adapter, func, context, timeout_ms); +} + +#ifdef CONFIG_FW_C2H_REG +s32 c2h_evt_hdl(_adapter *adapter, u8 *c2h_evt, c2h_id_filter filter) +{ + s32 ret = _FAIL; + u8 buf[C2H_REG_LEN] = {0}; + u8 id, seq, plen; + u8 *payload; + + if (!c2h_evt) { + /* No c2h event in cmd_obj, read c2h event before handling*/ + if (rtw_hal_c2h_evt_read(adapter, buf) != _SUCCESS) + goto exit; + c2h_evt = buf; + } + + rtw_hal_c2h_reg_hdr_parse(adapter, c2h_evt, &id, &seq, &plen, &payload); + + if (filter && filter(adapter, id, seq, plen, payload) == _FALSE) + goto exit; + + ret = rtw_hal_c2h_handler(adapter, id, seq, plen, payload); + +exit: + return ret; +} +#endif /* CONFIG_FW_C2H_REG */ + +u8 session_tracker_cmd(_adapter *adapter, u8 cmd, struct sta_info *sta, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port) +{ + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *cmd_parm; + struct st_cmd_parm *st_parm; + u8 res = _SUCCESS; + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (cmd_parm == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + st_parm = (struct st_cmd_parm *)rtw_zmalloc(sizeof(struct st_cmd_parm)); + if (st_parm == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + st_parm->cmd = cmd; + st_parm->sta = sta; + if (cmd != ST_CMD_CHK) { + _rtw_memcpy(&st_parm->local_naddr, local_naddr, 4); + _rtw_memcpy(&st_parm->local_port, local_port, 2); + _rtw_memcpy(&st_parm->remote_naddr, remote_naddr, 4); + _rtw_memcpy(&st_parm->remote_port, remote_port, 2); + } + + cmd_parm->ec_id = SESSION_TRACKER_WK_CID; + cmd_parm->type = 0; + cmd_parm->size = sizeof(struct st_cmd_parm); + cmd_parm->pbuf = (u8 *)st_parm; + init_h2fwcmd_w_parm_no_rsp(cmdobj, cmd_parm, CMD_SET_DRV_EXTRA); + cmdobj->no_io = 1; + + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + +exit: + return res; +} + +inline u8 session_tracker_chk_cmd(_adapter *adapter, struct sta_info *sta) +{ + return session_tracker_cmd(adapter, ST_CMD_CHK, sta, NULL, NULL, NULL, NULL); +} + +inline u8 session_tracker_add_cmd(_adapter *adapter, struct sta_info *sta, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port) +{ + return session_tracker_cmd(adapter, ST_CMD_ADD, sta, local_naddr, local_port, remote_naddr, remote_port); +} + +inline u8 session_tracker_del_cmd(_adapter *adapter, struct sta_info *sta, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port) +{ + return session_tracker_cmd(adapter, ST_CMD_DEL, sta, local_naddr, local_port, remote_naddr, remote_port); +} + +void session_tracker_chk_for_sta(_adapter *adapter, struct sta_info *sta) +{ + struct st_ctl_t *st_ctl = &sta->st_ctl; + int i; + _irqL irqL; + _list *plist, *phead, *pnext; + _list dlist; + struct session_tracker *st = NULL; + u8 op_wfd_mode = MIRACAST_DISABLED; + + if (DBG_SESSION_TRACKER) + RTW_INFO(FUNC_ADPT_FMT" sta:%p\n", FUNC_ADPT_ARG(adapter), sta); + + if (!(sta->state & WIFI_ASOC_STATE)) + goto exit; + + for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) { + if (st_ctl->reg[i].s_proto != 0) + break; + } + if (i >= SESSION_TRACKER_REG_ID_NUM) + goto chk_sta; + + _rtw_init_listhead(&dlist); + + _enter_critical_bh(&st_ctl->tracker_q.lock, &irqL); + + phead = &st_ctl->tracker_q.queue; + plist = get_next(phead); + pnext = get_next(plist); + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + st = LIST_CONTAINOR(plist, struct session_tracker, list); + plist = pnext; + pnext = get_next(pnext); + + if (st->status != ST_STATUS_ESTABLISH + && rtw_get_passing_time_ms(st->set_time) > ST_EXPIRE_MS + ) { + rtw_list_delete(&st->list); + rtw_list_insert_tail(&st->list, &dlist); + } + + /* TODO: check OS for status update */ + if (st->status == ST_STATUS_CHECK) + st->status = ST_STATUS_ESTABLISH; + + if (st->status != ST_STATUS_ESTABLISH) + continue; + + #ifdef CONFIG_WFD + if (0) + RTW_INFO(FUNC_ADPT_FMT" local:%u, remote:%u, rtsp:%u, %u, %u\n", FUNC_ADPT_ARG(adapter) + , ntohs(st->local_port), ntohs(st->remote_port), adapter->wfd_info.rtsp_ctrlport, adapter->wfd_info.tdls_rtsp_ctrlport + , adapter->wfd_info.peer_rtsp_ctrlport); + if (ntohs(st->local_port) == adapter->wfd_info.rtsp_ctrlport) + op_wfd_mode |= MIRACAST_SINK; + if (ntohs(st->local_port) == adapter->wfd_info.tdls_rtsp_ctrlport) + op_wfd_mode |= MIRACAST_SINK; + if (ntohs(st->remote_port) == adapter->wfd_info.peer_rtsp_ctrlport) + op_wfd_mode |= MIRACAST_SOURCE; + #endif + } + + _exit_critical_bh(&st_ctl->tracker_q.lock, &irqL); + + plist = get_next(&dlist); + while (rtw_end_of_queue_search(&dlist, plist) == _FALSE) { + st = LIST_CONTAINOR(plist, struct session_tracker, list); + plist = get_next(plist); + rtw_mfree((u8 *)st, sizeof(struct session_tracker)); + } + +chk_sta: + if (STA_OP_WFD_MODE(sta) != op_wfd_mode) { + STA_SET_OP_WFD_MODE(sta, op_wfd_mode); + rtw_sta_media_status_rpt_cmd(adapter, sta, 1); + } + +exit: + return; +} + +void session_tracker_chk_for_adapter(_adapter *adapter) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct sta_info *sta; + int i; + _irqL irqL; + _list *plist, *phead; + u8 op_wfd_mode = MIRACAST_DISABLED; + + _enter_critical_bh(&stapriv->sta_hash_lock, &irqL); + + for (i = 0; i < NUM_STA; i++) { + phead = &(stapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + sta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + + session_tracker_chk_for_sta(adapter, sta); + + op_wfd_mode |= STA_OP_WFD_MODE(sta); + } + } + + _exit_critical_bh(&stapriv->sta_hash_lock, &irqL); + +#ifdef CONFIG_WFD + adapter->wfd_info.op_wfd_mode = MIRACAST_MODE_REVERSE(op_wfd_mode); +#endif +} + +void session_tracker_cmd_hdl(_adapter *adapter, struct st_cmd_parm *parm) +{ + u8 cmd = parm->cmd; + struct sta_info *sta = parm->sta; + + if (cmd == ST_CMD_CHK) { + if (sta) + session_tracker_chk_for_sta(adapter, sta); + else + session_tracker_chk_for_adapter(adapter); + + goto exit; + + } else if (cmd == ST_CMD_ADD || cmd == ST_CMD_DEL) { + struct st_ctl_t *st_ctl; + u32 local_naddr = parm->local_naddr; + u16 local_port = parm->local_port; + u32 remote_naddr = parm->remote_naddr; + u16 remote_port = parm->remote_port; + struct session_tracker *st = NULL; + _irqL irqL; + _list *plist, *phead; + u8 free_st = 0; + u8 alloc_st = 0; + + if (DBG_SESSION_TRACKER) + RTW_INFO(FUNC_ADPT_FMT" cmd:%u, sta:%p, local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT"\n" + , FUNC_ADPT_ARG(adapter), cmd, sta + , IP_ARG(&local_naddr), PORT_ARG(&local_port) + , IP_ARG(&remote_naddr), PORT_ARG(&remote_port) + ); + + if (!(sta->state & WIFI_ASOC_STATE)) + goto exit; + + st_ctl = &sta->st_ctl; + + _enter_critical_bh(&st_ctl->tracker_q.lock, &irqL); + + phead = &st_ctl->tracker_q.queue; + plist = get_next(phead); + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + st = LIST_CONTAINOR(plist, struct session_tracker, list); + + if (st->local_naddr == local_naddr + && st->local_port == local_port + && st->remote_naddr == remote_naddr + && st->remote_port == remote_port) + break; + + plist = get_next(plist); + } + + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + st = NULL; + + switch (cmd) { + case ST_CMD_DEL: + if (st) { + rtw_list_delete(plist); + free_st = 1; + } + goto unlock; + case ST_CMD_ADD: + if (!st) + alloc_st = 1; + } + +unlock: + _exit_critical_bh(&st_ctl->tracker_q.lock, &irqL); + + if (free_st) { + rtw_mfree((u8 *)st, sizeof(struct session_tracker)); + goto exit; + } + + if (alloc_st) { + st = (struct session_tracker *)rtw_zmalloc(sizeof(struct session_tracker)); + if (!st) + goto exit; + + st->local_naddr = local_naddr; + st->local_port = local_port; + st->remote_naddr = remote_naddr; + st->remote_port = remote_port; + st->set_time = rtw_get_current_time(); + st->status = ST_STATUS_CHECK; + + _enter_critical_bh(&st_ctl->tracker_q.lock, &irqL); + rtw_list_insert_tail(&st->list, phead); + _exit_critical_bh(&st_ctl->tracker_q.lock, &irqL); + } + } + +exit: + return; +} + +#if defined(CONFIG_RTW_MESH) && defined(RTW_PER_CMD_SUPPORT_FW) +static s32 rtw_req_per_cmd_hdl(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct macid_bmp req_macid_bmp, *macid_bmp; + u8 i, ret = _FAIL; + + macid_bmp = &macid_ctl->if_g[adapter->iface_id]; + _rtw_memcpy(&req_macid_bmp, macid_bmp, sizeof(struct macid_bmp)); + + /* Clear none mesh's macid */ + for (i = 0; i < macid_ctl->num; i++) { + u8 role; + role = GET_H2CCMD_MSRRPT_PARM_ROLE(&macid_ctl->h2c_msr[i]); + if (role != H2C_MSR_ROLE_MESH) + rtw_macid_map_clr(&req_macid_bmp, i); + } + + /* group_macid: always be 0 in NIC, so only pass macid_bitmap.m0 + * rpt_type: 0 includes all info in 1, use 0 for now + * macid_bitmap: pass m0 only for NIC + */ + ret = rtw_hal_set_req_per_rpt_cmd(adapter, 0, 0, req_macid_bmp.m0); + + return ret; +} + +u8 rtw_req_per_cmd(_adapter *adapter) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (parm == NULL) { + res = _FAIL; + goto exit; + } + + parm->ec_id = REQ_PER_CMD_WK_CID; + parm->type = 0; + parm->size = 0; + parm->pbuf = NULL; + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj)); + if (cmdobj == NULL) { + res = _FAIL; + rtw_mfree((u8 *)parm, sizeof(*parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + +exit: + return res; +} +#endif + + +void rtw_ac_parm_cmd_hdl(_adapter *padapter, u8 *_ac_parm_buf, int ac_type) +{ + + u32 ac_parm_buf; + + _rtw_memcpy(&ac_parm_buf, _ac_parm_buf, sizeof(ac_parm_buf)); + switch (ac_type) { + case XMIT_VO_QUEUE: + RTW_INFO(FUNC_NDEV_FMT" AC_VO = 0x%08x\n", FUNC_ADPT_ARG(padapter), (unsigned int) ac_parm_buf); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&ac_parm_buf)); + break; + + case XMIT_VI_QUEUE: + RTW_INFO(FUNC_NDEV_FMT" AC_VI = 0x%08x\n", FUNC_ADPT_ARG(padapter), (unsigned int) ac_parm_buf); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&ac_parm_buf)); + break; + + case XMIT_BE_QUEUE: + RTW_INFO(FUNC_NDEV_FMT" AC_BE = 0x%08x\n", FUNC_ADPT_ARG(padapter), (unsigned int) ac_parm_buf); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&ac_parm_buf)); + break; + + case XMIT_BK_QUEUE: + RTW_INFO(FUNC_NDEV_FMT" AC_BK = 0x%08x\n", FUNC_ADPT_ARG(padapter), (unsigned int) ac_parm_buf); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&ac_parm_buf)); + break; + + default: + break; + } + +} + + +u8 rtw_drvextra_cmd_hdl(_adapter *padapter, unsigned char *pbuf) +{ + int ret = H2C_SUCCESS; + struct drvextra_cmd_parm *pdrvextra_cmd; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf; + + switch (pdrvextra_cmd->ec_id) { + case STA_MSTATUS_RPT_WK_CID: + rtw_sta_media_status_rpt_cmd_hdl(padapter, (struct sta_media_status_rpt_cmd_parm *)pdrvextra_cmd->pbuf); + break; + + case DYNAMIC_CHK_WK_CID:/*only primary padapter go to this cmd, but execute dynamic_chk_wk_hdl() for two interfaces */ + rtw_dynamic_chk_wk_hdl(padapter); + break; + case POWER_SAVING_CTRL_WK_CID: + power_saving_wk_hdl(padapter); + break; +#ifdef CONFIG_LPS + case LPS_CTRL_WK_CID: + lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; + case DM_IN_LPS_WK_CID: + rtw_dm_in_lps_hdl(padapter); + break; + case LPS_CHANGE_DTIM_CID: + rtw_lps_change_dtim_hdl(padapter, (u8)pdrvextra_cmd->type); + break; +#endif +#if (RATE_ADAPTIVE_SUPPORT == 1) + case RTP_TIMER_CFG_WK_CID: + rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type); + break; +#endif +#ifdef CONFIG_ANTENNA_DIVERSITY + case ANT_SELECT_WK_CID: + antenna_select_wk_hdl(padapter, pdrvextra_cmd->type); + break; +#endif +#ifdef CONFIG_P2P_PS + case P2P_PS_WK_CID: + p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type); + break; +#endif +#ifdef CONFIG_P2P + case P2P_PROTO_WK_CID: + /* + * Commented by Albert 2011/07/01 + * I used the type_size as the type command + */ + ret = p2p_protocol_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; +#endif +#ifdef CONFIG_AP_MODE + case CHECK_HIQ_WK_CID: + rtw_chk_hi_queue_hdl(padapter); + break; +#endif + /* add for CONFIG_IEEE80211W, none 11w can use it */ + case RESET_SECURITYPRIV: + reset_securitypriv_hdl(padapter); + break; + case FREE_ASSOC_RESOURCES: + free_assoc_resources_hdl(padapter, (u8)pdrvextra_cmd->type); + break; + case C2H_WK_CID: + switch (pdrvextra_cmd->type) { + #ifdef CONFIG_FW_C2H_REG + case C2H_TYPE_REG: + c2h_evt_hdl(padapter, pdrvextra_cmd->pbuf, NULL); + break; + #endif + #ifdef CONFIG_FW_C2H_PKT + case C2H_TYPE_PKT: + rtw_hal_c2h_pkt_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size); + break; + #endif + default: + RTW_ERR("unknown C2H type:%d\n", pdrvextra_cmd->type); + rtw_warn_on(1); + break; + } + break; +#ifdef CONFIG_BEAMFORMING + case BEAMFORMING_WK_CID: + beamforming_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; +#endif + case DM_RA_MSK_WK_CID: + rtw_dm_ra_mask_hdl(padapter, (struct sta_info *)pdrvextra_cmd->pbuf); + break; +#ifdef CONFIG_BT_COEXIST + case BTINFO_WK_CID: + rtw_btinfo_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size); + break; + case BTC_REDUCE_WL_TXPWR_CID: + rtw_btc_reduce_wl_txpwr_hdl(padapter, pdrvextra_cmd->type); + break; +#endif +#ifdef CONFIG_DFS_MASTER + case DFS_RADAR_DETECT_WK_CID: + rtw_dfs_rd_hdl(padapter); + break; + case DFS_RADAR_DETECT_EN_DEC_WK_CID: + rtw_dfs_rd_en_decision(padapter, MLME_ACTION_NONE, 0); + break; +#endif + case SESSION_TRACKER_WK_CID: + session_tracker_cmd_hdl(padapter, (struct st_cmd_parm *)pdrvextra_cmd->pbuf); + break; + case EN_HW_UPDATE_TSF_WK_CID: + rtw_hal_set_hwreg(padapter, HW_VAR_EN_HW_UPDATE_TSF, NULL); + break; + case PERIOD_TSF_UPDATE_END_WK_CID: + rtw_hal_periodic_tsf_update_chk(padapter); + break; + case TEST_H2C_CID: + rtw_hal_fill_h2c_cmd(padapter, pdrvextra_cmd->pbuf[0], pdrvextra_cmd->size - 1, &pdrvextra_cmd->pbuf[1]); + break; + case MP_CMD_WK_CID: +#ifdef CONFIG_MP_INCLUDED + ret = rtw_mp_cmd_hdl(padapter, pdrvextra_cmd->type); +#endif + break; +#ifdef CONFIG_RTW_CUSTOMER_STR + case CUSTOMER_STR_WK_CID: + ret = rtw_customer_str_cmd_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; +#endif + +#ifdef CONFIG_RTW_REPEATER_SON + case RSON_SCAN_WK_CID: + rtw_rson_scan_cmd_hdl(padapter, pdrvextra_cmd->type); + break; +#endif + +#ifdef CONFIG_IOCTL_CFG80211 + case MGNT_TX_WK_CID: + ret = rtw_mgnt_tx_handler(padapter, pdrvextra_cmd->pbuf); + break; +#endif /* CONFIG_IOCTL_CFG80211 */ + +#if (defined(CONFIG_P2P) && defined(CONFIG_CONCURRENT_MODE)) || defined(CONFIG_IOCTL_CFG80211) + case ROCH_WK_CID: + ret = rtw_roch_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; +#endif /* (defined(CONFIG_P2P) && defined(CONFIG_CONCURRENT_MODE)) || defined(CONFIG_IOCTL_CFG80211) */ + +#ifdef CONFIG_MCC_MODE + case MCC_CMD_WK_CID: + ret = rtw_mcc_cmd_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf); + break; +#endif /* CONFIG_MCC_MODE */ +#if defined(CONFIG_RTW_MESH) && defined(RTW_PER_CMD_SUPPORT_FW) + case REQ_PER_CMD_WK_CID: + ret = rtw_req_per_cmd_hdl(padapter); + break; +#endif +#ifdef CONFIG_SUPPORT_STATIC_SMPS + case SSMPS_WK_CID : + rtw_ssmps_wk_hdl(padapter, (struct ssmps_cmd_parm *)pdrvextra_cmd->pbuf); + break; +#endif +#ifdef CONFIG_CTRL_TXSS_BY_TP + case TXSS_WK_CID : + rtw_ctrl_txss_wk_hdl(padapter, (struct txss_cmd_parm *)pdrvextra_cmd->pbuf); + break; +#endif + case AC_PARM_CMD_WK_CID: + rtw_ac_parm_cmd_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type); + break; +#ifdef CONFIG_AP_MODE + case STOP_AP_WK_CID: + stop_ap_hdl(padapter); + break; +#endif +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + case TBTX_CONTROL_TX_WK_CID: + tx_control_hdl(padapter); + break; +#endif + default: + break; + } + + if (pdrvextra_cmd->pbuf && pdrvextra_cmd->size > 0) + rtw_mfree(pdrvextra_cmd->pbuf, pdrvextra_cmd->size); + + return ret; +} + +void rtw_survey_cmd_callback(_adapter *padapter , struct cmd_obj *pcmd) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + if (pcmd->res == H2C_DROPPED) { + /* TODO: cancel timer and do timeout handler directly... */ + /* need to make timeout handlerOS independent */ + mlme_set_scan_to_timer(pmlmepriv, 1); + } else if (pcmd->res != H2C_SUCCESS) { + mlme_set_scan_to_timer(pmlmepriv, 1); + } + + /* free cmd */ + rtw_free_cmd_obj(pcmd); + +} +void rtw_disassoc_cmd_callback(_adapter *padapter, struct cmd_obj *pcmd) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + if (pcmd->res != H2C_SUCCESS) { + _enter_critical_bh(&pmlmepriv->lock, &irqL); + set_fwstate(pmlmepriv, WIFI_ASOC_STATE); + _exit_critical_bh(&pmlmepriv->lock, &irqL); + goto exit; + } +#ifdef CONFIG_BR_EXT + else /* clear bridge database */ + nat25_db_cleanup(padapter); +#endif /* CONFIG_BR_EXT */ + + /* free cmd */ + rtw_free_cmd_obj(pcmd); + +exit: + return; +} + +void rtw_joinbss_cmd_callback(_adapter *padapter, struct cmd_obj *pcmd) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + if (pcmd->res == H2C_DROPPED) { + /* TODO: cancel timer and do timeout handler directly... */ + /* need to make timeout handlerOS independent */ + _set_timer(&pmlmepriv->assoc_timer, 1); + } else if (pcmd->res != H2C_SUCCESS) + _set_timer(&pmlmepriv->assoc_timer, 1); + + rtw_free_cmd_obj(pcmd); + +} + +void rtw_create_ibss_post_hdl(_adapter *padapter, int status) +{ + _irqL irqL; + struct wlan_network *pwlan = NULL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + WLAN_BSSID_EX *pdev_network = &padapter->registrypriv.dev_network; + struct wlan_network *mlme_cur_network = &(pmlmepriv->cur_network); + + if (status != H2C_SUCCESS) + _set_timer(&pmlmepriv->assoc_timer, 1); + + _cancel_timer_ex(&pmlmepriv->assoc_timer); + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + { + _irqL irqL; + + pwlan = _rtw_alloc_network(pmlmepriv); + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + if (pwlan == NULL) { + pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue); + if (pwlan == NULL) { + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + goto createbss_cmd_fail; + } + pwlan->last_scanned = rtw_get_current_time(); + } else + rtw_list_insert_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue); + + pdev_network->Length = get_WLAN_BSSID_EX_sz(pdev_network); + _rtw_memcpy(&(pwlan->network), pdev_network, pdev_network->Length); + /* pwlan->fixed = _TRUE; */ + + /* copy pdev_network information to pmlmepriv->cur_network */ + _rtw_memcpy(&mlme_cur_network->network, pdev_network, (get_WLAN_BSSID_EX_sz(pdev_network))); + +#if 0 + /* reset DSConfig */ + mlme_cur_network->network.Configuration.DSConfig = (u32)rtw_ch2freq(pdev_network->Configuration.DSConfig); +#endif + + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + /* we will set WIFI_ASOC_STATE when there is one more sat to join us (rtw_stassoc_event_callback) */ + } + +createbss_cmd_fail: + _exit_critical_bh(&pmlmepriv->lock, &irqL); + return; +} + + + +void rtw_setstaKey_cmdrsp_callback(_adapter *padapter , struct cmd_obj *pcmd) +{ + + struct sta_priv *pstapriv = &padapter->stapriv; + struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp); + struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr); + + + if (psta == NULL) { + goto exit; + } + + /* psta->cmn.aid = psta->cmn.mac_id = psetstakey_rsp->keyid; */ /* CAM_ID(CAM_ENTRY) */ + +exit: + + rtw_free_cmd_obj(pcmd); + + +} + +void rtw_getrttbl_cmd_cmdrsp_callback(_adapter *padapter, struct cmd_obj *pcmd) +{ + + rtw_free_cmd_obj(pcmd); +#ifdef CONFIG_MP_INCLUDED + if (padapter->registrypriv.mp_mode == 1) + padapter->mppriv.workparam.bcompleted = _TRUE; +#endif + + +} + +u8 set_txq_params_cmd(_adapter *adapter, u32 ac_parm, u8 ac_type) +{ + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &adapter->cmdpriv; + u8 *ac_parm_buf = NULL; + u8 sz; + u8 res = _SUCCESS; + + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + sz = sizeof(ac_parm); + ac_parm_buf = rtw_zmalloc(sz); + if (ac_parm_buf == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = AC_PARM_CMD_WK_CID; + pdrvextra_cmd_parm->type = ac_type; + pdrvextra_cmd_parm->size = sz; + pdrvextra_cmd_parm->pbuf = ac_parm_buf; + + _rtw_memcpy(ac_parm_buf, &ac_parm, sz); + + init_h2fwcmd_w_parm_no_rsp(cmdobj, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + res = rtw_enqueue_cmd(pcmdpriv, cmdobj); + +exit: + return res; +} + +/* Driver writes beacon length to REG for FW adjusting beacon receive time */ +#ifdef CONFIG_WRITE_BCN_LEN_TO_FW +u8 rtw_write_bcnlen_to_fw_cmd(_adapter *padapter, u16 bcn_len) +{ + struct cmd_obj *pcmd; + struct write_bcnlen_param *parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } + + parm = (struct write_bcnlen_param *)rtw_zmalloc(sizeof(struct write_bcnlen_param)); + if (parm == NULL) { + rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + parm->bcn_len = bcn_len; + init_h2fwcmd_w_parm_no_rsp(pcmd, parm, CMD_WRITE_BCN_LEN); + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + return res; +} +#endif /* CONFIG_WRITE_BCN_LEN_TO_FW */ diff --git a/core/rtw_debug.c b/core/rtw_debug.c new file mode 100644 index 0000000..e475585 --- /dev/null +++ b/core/rtw_debug.c @@ -0,0 +1,8345 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_DEBUG_C_ + +#include +#include + +#ifdef CONFIG_RTW_DEBUG +const char *rtw_log_level_str[] = { + "_DRV_NONE_ = 0", + "_DRV_ALWAYS_ = 1", + "_DRV_ERR_ = 2", + "_DRV_WARNING_ = 3", + "_DRV_INFO_ = 4", + "_DRV_DEBUG_ = 5", + "_DRV_MAX_ = 6", +}; +#endif + +#ifdef CONFIG_DEBUG_RTL871X + u64 GlobalDebugComponents = 0; +#endif /* CONFIG_DEBUG_RTL871X */ + +#include + +#ifdef CONFIG_TDLS + #define TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE 41 +#endif + +void dump_drv_version(void *sel) +{ + RTW_PRINT_SEL(sel, "%s %s\n", DRV_NAME, DRIVERVERSION); + RTW_PRINT_SEL(sel, "build time: %s %s\n", __DATE__, __TIME__); +} + +#ifdef CONFIG_PROC_DEBUG +void dump_drv_cfg(void *sel) +{ +extern uint rtw_recvbuf_nr; + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)) + char *kernel_version = utsname()->release; + + RTW_PRINT_SEL(sel, "\nKernel Version: %s\n", kernel_version); +#endif + + RTW_PRINT_SEL(sel, "Driver Version: %s\n", DRIVERVERSION); + RTW_PRINT_SEL(sel, "------------------------------------------------\n"); +#ifdef CONFIG_IOCTL_CFG80211 + RTW_PRINT_SEL(sel, "CFG80211\n"); +#ifdef RTW_USE_CFG80211_STA_EVENT + RTW_PRINT_SEL(sel, "RTW_USE_CFG80211_STA_EVENT\n"); +#endif + #ifdef CONFIG_RADIO_WORK + RTW_PRINT_SEL(sel, "CONFIG_RADIO_WORK\n"); + #endif +#else + RTW_PRINT_SEL(sel, "WEXT\n"); +#endif + + RTW_PRINT_SEL(sel, "DBG:%d\n", DBG); +#ifdef CONFIG_RTW_DEBUG + RTW_PRINT_SEL(sel, "CONFIG_RTW_DEBUG\n"); +#endif + +#ifdef CONFIG_CONCURRENT_MODE + RTW_PRINT_SEL(sel, "CONFIG_CONCURRENT_MODE\n"); +#endif + +#ifdef CONFIG_POWER_SAVING + RTW_PRINT_SEL(sel, "CONFIG_POWER_SAVING\n"); + #ifdef CONFIG_IPS + RTW_PRINT_SEL(sel, "CONFIG_IPS\n"); + #endif + #ifdef CONFIG_LPS + RTW_PRINT_SEL(sel, "CONFIG_LPS\n"); + #ifdef CONFIG_LPS_LCLK + RTW_PRINT_SEL(sel, "CONFIG_LPS_LCLK\n"); + #ifdef CONFIG_DETECT_CPWM_BY_POLLING + RTW_PRINT_SEL(sel, "CONFIG_DETECT_CPWM_BY_POLLING\n"); + #endif + #endif /*CONFIG_LPS_LCLK*/ + #ifdef CONFIG_LPS_CHK_BY_TP + RTW_PRINT_SEL(sel, "CONFIG_LPS_CHK_BY_TP\n"); + #endif + #ifdef CONFIG_LPS_ACK + RTW_PRINT_SEL(sel, "CONFIG_LPS_ACK\n"); + #endif + #endif/*CONFIG_LPS*/ +#endif + +#ifdef CONFIG_LOAD_PHY_PARA_FROM_FILE + RTW_PRINT_SEL(sel, "LOAD_PHY_PARA_FROM_FILE - REALTEK_CONFIG_PATH=%s\n", REALTEK_CONFIG_PATH); + #if defined(CONFIG_MULTIDRV) || defined(REALTEK_CONFIG_PATH_WITH_IC_NAME_FOLDER) + RTW_PRINT_SEL(sel, "LOAD_PHY_PARA_FROM_FILE - REALTEK_CONFIG_PATH_WITH_IC_NAME_FOLDER\n"); + #endif + +/* configurations about TX power */ +#ifdef CONFIG_CALIBRATE_TX_POWER_BY_REGULATORY + RTW_PRINT_SEL(sel, "CONFIG_CALIBRATE_TX_POWER_BY_REGULATORY\n"); +#endif +#ifdef CONFIG_CALIBRATE_TX_POWER_TO_MAX + RTW_PRINT_SEL(sel, "CONFIG_CALIBRATE_TX_POWER_TO_MAX\n"); +#endif +#endif + RTW_PRINT_SEL(sel, "RTW_DEF_MODULE_REGULATORY_CERT=0x%02x\n", RTW_DEF_MODULE_REGULATORY_CERT); + + RTW_PRINT_SEL(sel, "CONFIG_TXPWR_BY_RATE=%d\n", CONFIG_TXPWR_BY_RATE); + RTW_PRINT_SEL(sel, "CONFIG_TXPWR_BY_RATE_EN=%d\n", CONFIG_TXPWR_BY_RATE_EN); + RTW_PRINT_SEL(sel, "CONFIG_TXPWR_LIMIT=%d\n", CONFIG_TXPWR_LIMIT); + RTW_PRINT_SEL(sel, "CONFIG_TXPWR_LIMIT_EN=%d\n", CONFIG_TXPWR_LIMIT_EN); + + +#ifdef CONFIG_DISABLE_ODM + RTW_PRINT_SEL(sel, "CONFIG_DISABLE_ODM\n"); +#endif + +#ifdef CONFIG_MINIMAL_MEMORY_USAGE + RTW_PRINT_SEL(sel, "CONFIG_MINIMAL_MEMORY_USAGE\n"); +#endif + + RTW_PRINT_SEL(sel, "CONFIG_RTW_ADAPTIVITY_EN = %d\n", CONFIG_RTW_ADAPTIVITY_EN); +#if (CONFIG_RTW_ADAPTIVITY_EN) + RTW_PRINT_SEL(sel, "ADAPTIVITY_MODE = %s\n", (CONFIG_RTW_ADAPTIVITY_MODE) ? "carrier_sense" : "normal"); +#endif + +#ifdef CONFIG_WOWLAN + RTW_PRINT_SEL(sel, "CONFIG_WOWLAN - "); + +#ifdef CONFIG_GPIO_WAKEUP + RTW_PRINT_SEL(sel, "CONFIG_GPIO_WAKEUP - WAKEUP_GPIO_IDX:%d\n", WAKEUP_GPIO_IDX); +#endif +#endif + +#ifdef CONFIG_TDLS + RTW_PRINT_SEL(sel, "CONFIG_TDLS\n"); +#endif + +#ifdef CONFIG_RTW_80211R + RTW_PRINT_SEL(sel, "CONFIG_RTW_80211R\n"); +#endif + +#ifdef CONFIG_RTW_NETIF_SG + RTW_PRINT_SEL(sel, "CONFIG_RTW_NETIF_SG\n"); +#endif + +#ifdef CONFIG_RTW_WIFI_HAL + RTW_PRINT_SEL(sel, "CONFIG_RTW_WIFI_HAL\n"); +#endif + +#ifdef RTW_BUSY_DENY_SCAN + RTW_PRINT_SEL(sel, "RTW_BUSY_DENY_SCAN\n"); + RTW_PRINT_SEL(sel, "BUSY_TRAFFIC_SCAN_DENY_PERIOD = %u ms\n", \ + BUSY_TRAFFIC_SCAN_DENY_PERIOD); +#endif + +#ifdef CONFIG_RTW_TPT_MODE + RTW_PRINT_SEL(sel, "CONFIG_RTW_TPT_MODE\n"); +#endif + +#ifdef CONFIG_USB_HCI +#ifdef CONFIG_SUPPORT_USB_INT + RTW_PRINT_SEL(sel, "CONFIG_SUPPORT_USB_INT\n"); +#endif +#ifdef CONFIG_USB_INTERRUPT_IN_PIPE + RTW_PRINT_SEL(sel, "CONFIG_USB_INTERRUPT_IN_PIPE\n"); +#endif +#ifdef CONFIG_USB_TX_AGGREGATION + RTW_PRINT_SEL(sel, "CONFIG_USB_TX_AGGREGATION\n"); +#endif +#ifdef CONFIG_USB_RX_AGGREGATION + RTW_PRINT_SEL(sel, "CONFIG_USB_RX_AGGREGATION\n"); +#endif +#ifdef CONFIG_USE_USB_BUFFER_ALLOC_TX + RTW_PRINT_SEL(sel, "CONFIG_USE_USB_BUFFER_ALLOC_TX\n"); +#endif +#ifdef CONFIG_USE_USB_BUFFER_ALLOC_RX + RTW_PRINT_SEL(sel, "CONFIG_USE_USB_BUFFER_ALLOC_RX\n"); +#endif +#ifdef CONFIG_PREALLOC_RECV_SKB + RTW_PRINT_SEL(sel, "CONFIG_PREALLOC_RECV_SKB\n"); +#endif +#ifdef CONFIG_FIX_NR_BULKIN_BUFFER + RTW_PRINT_SEL(sel, "CONFIG_FIX_NR_BULKIN_BUFFER\n"); +#endif +#endif /*CONFIG_USB_HCI*/ + +#ifdef CONFIG_SDIO_HCI +#ifdef CONFIG_TX_AGGREGATION + RTW_PRINT_SEL(sel, "CONFIG_TX_AGGREGATION\n"); +#endif +#ifdef CONFIG_RX_AGGREGATION + RTW_PRINT_SEL(sel, "CONFIG_RX_AGGREGATION\n"); +#endif +#ifdef RTW_XMIT_THREAD_HIGH_PRIORITY + RTW_PRINT_SEL(sel, "RTW_XMIT_THREAD_HIGH_PRIORITY\n"); +#endif +#ifdef RTW_XMIT_THREAD_HIGH_PRIORITY_AGG + RTW_PRINT_SEL(sel, "RTW_XMIT_THREAD_HIGH_PRIORITY_AGG\n"); +#endif + +#ifdef DBG_SDIO + RTW_PRINT_SEL(sel, "DBG_SDIO = %d\n", DBG_SDIO); +#endif +#endif /*CONFIG_SDIO_HCI*/ + +#ifdef CONFIG_PCI_HCI +#endif + + RTW_PRINT_SEL(sel, "CONFIG_IFACE_NUMBER = %d\n", CONFIG_IFACE_NUMBER); +#ifdef CONFIG_MI_WITH_MBSSID_CAM + RTW_PRINT_SEL(sel, "CONFIG_MI_WITH_MBSSID_CAM\n"); +#endif +#ifdef CONFIG_SWTIMER_BASED_TXBCN + RTW_PRINT_SEL(sel, "CONFIG_SWTIMER_BASED_TXBCN\n"); +#endif +#ifdef CONFIG_FW_HANDLE_TXBCN + RTW_PRINT_SEL(sel, "CONFIG_FW_HANDLE_TXBCN\n"); + RTW_PRINT_SEL(sel, "CONFIG_LIMITED_AP_NUM = %d\n", CONFIG_LIMITED_AP_NUM); +#endif +#ifdef CONFIG_CLIENT_PORT_CFG + RTW_PRINT_SEL(sel, "CONFIG_CLIENT_PORT_CFG\n"); +#endif +#ifdef CONFIG_PCI_TX_POLLING + RTW_PRINT_SEL(sel, "CONFIG_PCI_TX_POLLING\n"); +#endif + RTW_PRINT_SEL(sel, "CONFIG_RTW_UP_MAPPING_RULE = %s\n", (CONFIG_RTW_UP_MAPPING_RULE == 1) ? "dscp" : "tos"); + + RTW_PRINT_SEL(sel, "\n=== XMIT-INFO ===\n"); + RTW_PRINT_SEL(sel, "NR_XMITFRAME = %d\n", NR_XMITFRAME); + RTW_PRINT_SEL(sel, "NR_XMITBUFF = %d\n", NR_XMITBUFF); + RTW_PRINT_SEL(sel, "MAX_XMITBUF_SZ = %d\n", MAX_XMITBUF_SZ); + RTW_PRINT_SEL(sel, "NR_XMIT_EXTBUFF = %d\n", NR_XMIT_EXTBUFF); + RTW_PRINT_SEL(sel, "MAX_XMIT_EXTBUF_SZ = %d\n", MAX_XMIT_EXTBUF_SZ); + RTW_PRINT_SEL(sel, "MAX_CMDBUF_SZ = %d\n", MAX_CMDBUF_SZ); + + RTW_PRINT_SEL(sel, "\n=== RECV-INFO ===\n"); + RTW_PRINT_SEL(sel, "NR_RECVFRAME = %d\n", NR_RECVFRAME); + RTW_PRINT_SEL(sel, "NR_RECVBUFF = %d, rtw_recvbuf_nr = %d\n", NR_RECVBUFF, rtw_recvbuf_nr); + RTW_PRINT_SEL(sel, "MAX_RECVBUF_SZ = %d\n", MAX_RECVBUF_SZ); + +} +#endif /* CONFIG_PROC_DEBUG */ + + +void dump_log_level(void *sel) +{ +#ifdef CONFIG_RTW_DEBUG + int i; + + RTW_PRINT_SEL(sel, "drv_log_level:%d\n", rtw_drv_log_level); + for (i = 0; i <= _DRV_MAX_; i++) { + if (rtw_log_level_str[i]) + RTW_PRINT_SEL(sel, "%c %s = %d\n", + (rtw_drv_log_level == i) ? '+' : ' ', rtw_log_level_str[i], i); + } +#else + RTW_PRINT_SEL(sel, "CONFIG_RTW_DEBUG is disabled\n"); +#endif +} + +#ifdef CONFIG_SDIO_HCI +void sd_f0_reg_dump(void *sel, _adapter *adapter) +{ + int i; + + for (i = 0x0; i <= 0xff; i++) { + if (i % 16 == 0) + RTW_PRINT_SEL(sel, "0x%02x ", i); + + _RTW_PRINT_SEL(sel, "%02x ", rtw_sd_f0_read8(adapter, i)); + + if (i % 16 == 15) + _RTW_PRINT_SEL(sel, "\n"); + else if (i % 8 == 7) + _RTW_PRINT_SEL(sel, "\t"); + } +} + +void sdio_local_reg_dump(void *sel, _adapter *adapter) +{ + int i, j = 1; + + for (i = 0x0; i < 0x100; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%02x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, (0x1025 << 16) | i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +} +#endif /* CONFIG_SDIO_HCI */ + +void mac_reg_dump(void *sel, _adapter *adapter) +{ + int i, j = 1; + + RTW_PRINT_SEL(sel, "======= MAC REG =======\n"); + + for (i = 0x0; i < 0x800; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + +#ifdef CONFIG_RTL8814A + { + for (i = 0x1000; i < 0x1650; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + } +#endif /* CONFIG_RTL8814A */ + +#if defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) || defined(CONFIG_RTL8814B) \ + || defined(CONFIG_RTL8723F) + for (i = 0x1000; i < 0x1800; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +#endif /* CONFIG_RTL8822B or 8821c*/ + +#if defined(CONFIG_RTL8192F) + for (i = 0x1000; i < 0x1100; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + for (i = 0x1300; i < 0x1360; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +#endif + +#if defined(CONFIG_RTL8814B) + for (i = 0x2000; i < 0x2800; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + + for (i = 0x3000; i < 0x3800; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +#endif + +} + +void bb_reg_dump(void *sel, _adapter *adapter) +{ + int i, j = 1; + + RTW_PRINT_SEL(sel, "======= BB REG =======\n"); + for (i = 0x800; i < 0x1000; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + +#if defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) || defined(CONFIG_RTL8814B) \ + || defined(CONFIG_RTL8723F) + for (i = 0x1800; i < 0x2000; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +#endif /* CONFIG_RTL8822B */ + +#if defined(CONFIG_RTL8822C) || defined(CONFIG_RTL8814B) || defined(CONFIG_RTL8723F) + for (i = 0x2c00; i < 0x2c60; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + + for (i = 0x2d00; i < 0x2df0; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + + for (i = 0x4000; i < 0x4060; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + + for (i = 0x4100; i < 0x4200; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + +#endif /* CONFIG_RTL8822C || CONFIG_RTL8814B || CONFIG_8723F */ + +#if defined(CONFIG_RTL8814B) + for (i = 0x5200; i < 0x5400; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +#endif /* CONFIG_RTL8814B */ + +#if defined(CONFIG_RTL8723F) + /* TSSI related */ + for (i = 0x4300; i < 0x43C0; i += 4) { + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } +#endif /* CONFIG_RTL8723F */ +} + +void bb_reg_dump_ex(void *sel, _adapter *adapter) +{ + int i; + + RTW_PRINT_SEL(sel, "======= BB REG =======\n"); + for (i = 0x800; i < 0x1000; i += 4) { + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + _RTW_PRINT_SEL(sel, "\n"); + } + +#if defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) || defined(CONFIG_RTL8814B) + for (i = 0x1800; i < 0x2000; i += 4) { + RTW_PRINT_SEL(sel, "0x%04x", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i)); + _RTW_PRINT_SEL(sel, "\n"); + } +#endif /* CONFIG_RTL8822B */ +} + +void rf_reg_dump(void *sel, _adapter *adapter) +{ + struct hal_spec_t *hal_spec = GET_HAL_SPEC(adapter); + int i, j = 1, path; + u32 value; + u8 path_nums = hal_spec->rf_reg_path_num; + + RTW_PRINT_SEL(sel, "======= RF REG =======\n"); + + for (path = 0; path < path_nums; path++) { + RTW_PRINT_SEL(sel, "RF_Path(%x)\n", path); + for (i = 0; i < 0x100; i++) { + value = rtw_hal_read_rfreg(adapter, path, i, 0xffffffff); + if (j % 4 == 1) + RTW_PRINT_SEL(sel, "0x%02x ", i); + _RTW_PRINT_SEL(sel, " 0x%08x ", value); + if ((j++) % 4 == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + } +} + +void rtw_sink_rtp_seq_dbg(_adapter *adapter, u8 *ehdr_pos) +{ + struct recv_priv *precvpriv = &(adapter->recvpriv); + if (precvpriv->sink_udpport > 0) { + if (*((u16 *)(ehdr_pos + 0x24)) == cpu_to_be16(precvpriv->sink_udpport)) { + precvpriv->pre_rtp_rxseq = precvpriv->cur_rtp_rxseq; + precvpriv->cur_rtp_rxseq = be16_to_cpu(*((u16 *)(ehdr_pos + 0x2C))); + if (precvpriv->pre_rtp_rxseq + 1 != precvpriv->cur_rtp_rxseq) { + if(precvpriv->pre_rtp_rxseq == 65535 ) { + if( precvpriv->cur_rtp_rxseq != 0) { + RTW_INFO("%s : RTP Seq num from %d to %d\n", __FUNCTION__, precvpriv->pre_rtp_rxseq, precvpriv->cur_rtp_rxseq); + } + } else { + RTW_INFO("%s : RTP Seq num from %d to %d\n", __FUNCTION__, precvpriv->pre_rtp_rxseq, precvpriv->cur_rtp_rxseq); + } + } + } + } +} + +void sta_rx_reorder_ctl_dump(void *sel, struct sta_info *sta) +{ + struct recv_reorder_ctrl *reorder_ctl; + int i; + + for (i = 0; i < 16; i++) { + reorder_ctl = &sta->recvreorder_ctrl[i]; + if (reorder_ctl->ampdu_size != RX_AMPDU_SIZE_INVALID || reorder_ctl->indicate_seq != 0xFFFF) { + RTW_PRINT_SEL(sel, "tid=%d, enable=%d, ampdu_size=%u, indicate_seq=%u\n" + , i, reorder_ctl->enable, reorder_ctl->ampdu_size, reorder_ctl->indicate_seq + ); + } + } +} + +void dump_tx_rate_bmp(void *sel, struct dvobj_priv *dvobj) +{ + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj); + u8 bw; + + RTW_PRINT_SEL(sel, "%-6s", "bw"); + if (hal_chk_proto_cap(adapter, PROTO_CAP_11AC)) + _RTW_PRINT_SEL(sel, " %-15s", "vht"); + + _RTW_PRINT_SEL(sel, " %-11s %-4s %-3s\n", "ht", "ofdm", "cck"); + + for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) { + if (!hal_is_bw_support(adapter, bw)) + continue; + + RTW_PRINT_SEL(sel, "%6s", ch_width_str(bw)); + if (hal_chk_proto_cap(adapter, PROTO_CAP_11AC)) { + _RTW_PRINT_SEL(sel, " %03x %03x %03x %03x" + , RATE_BMP_GET_VHT_4SS(rfctl->rate_bmp_vht_by_bw[bw]) + , RATE_BMP_GET_VHT_3SS(rfctl->rate_bmp_vht_by_bw[bw]) + , RATE_BMP_GET_VHT_2SS(rfctl->rate_bmp_vht_by_bw[bw]) + , RATE_BMP_GET_VHT_1SS(rfctl->rate_bmp_vht_by_bw[bw]) + ); + } + + _RTW_PRINT_SEL(sel, " %02x %02x %02x %02x" + , bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_4SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0 + , bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_3SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0 + , bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_2SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0 + , bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_1SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0 + ); + + _RTW_PRINT_SEL(sel, " %03x %01x\n" + , bw <= CHANNEL_WIDTH_20 ? RATE_BMP_GET_OFDM(rfctl->rate_bmp_cck_ofdm) : 0 + , bw <= CHANNEL_WIDTH_20 ? RATE_BMP_GET_CCK(rfctl->rate_bmp_cck_ofdm) : 0 + ); + } +} + +void dump_adapters_status(void *sel, struct dvobj_priv *dvobj) +{ +#if defined(CONFIG_RTW_DEBUG) || defined(CONFIG_PROC_DEBUG) + struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj); + int i; + _adapter *iface; + u8 u_ch, u_bw, u_offset; +#if (defined(CONFIG_SUPPORT_MULTI_BCN) && defined(CONFIG_FW_HANDLE_TXBCN)) || defined(CONFIG_CLIENT_PORT_CFG) + char str_val[64] = {'\0'}; +#endif + dump_mi_status(sel, dvobj); + +#if defined(CONFIG_SUPPORT_MULTI_BCN) && defined(CONFIG_FW_HANDLE_TXBCN) + RTW_PRINT_SEL(sel, "[AP] LIMITED_AP_NUM:%d\n", CONFIG_LIMITED_AP_NUM); + RTW_PRINT_SEL(sel, "[AP] vap_map:0x%02x\n", dvobj->vap_map); +#endif +#ifdef CONFIG_HW_P0_TSF_SYNC + RTW_PRINT_SEL(sel, "[AP] p0 tsf sync port = %d\n", dvobj->p0_tsf.sync_port); + RTW_PRINT_SEL(sel, "[AP] p0 tsf timer offset = %d\n", dvobj->p0_tsf.offset); +#endif +#ifdef CONFIG_CLIENT_PORT_CFG + RTW_PRINT_SEL(sel, "[CLT] clt_num = %d\n", dvobj->clt_port.num); + RTW_PRINT_SEL(sel, "[CLT] clt_map = 0x%02x\n", dvobj->clt_port.bmp); +#endif +#ifdef CONFIG_FW_MULTI_PORT_SUPPORT + RTW_PRINT_SEL(sel, "[MI] default port id:%d\n\n", dvobj->dft.port_id); +#endif /* CONFIG_FW_MULTI_PORT_SUPPORT */ + + RTW_PRINT_SEL(sel, "dev status:%s%s\n\n" + , dev_is_surprise_removed(dvobj) ? " SR" : "" + , dev_is_drv_stopped(dvobj) ? " DS" : "" + ); + +#ifdef CONFIG_P2P +#define P2P_INFO_TITLE_FMT " %-3s %-4s" +#define P2P_INFO_TITLE_ARG , "lch", "p2ps" +#ifdef CONFIG_IOCTL_CFG80211 +#define P2P_INFO_VALUE_FMT " %3u %c%3u" +#define P2P_INFO_VALUE_ARG , iface->wdinfo.listen_channel, iface->wdev_data.p2p_enabled ? 'e' : ' ', rtw_p2p_state(&iface->wdinfo) +#else +#define P2P_INFO_VALUE_FMT " %3u %4u" +#define P2P_INFO_VALUE_ARG , iface->wdinfo.listen_channel, rtw_p2p_state(&iface->wdinfo) +#endif +#define P2P_INFO_DASH "---------" +#else +#define P2P_INFO_TITLE_FMT "" +#define P2P_INFO_TITLE_ARG +#define P2P_INFO_VALUE_FMT "" +#define P2P_INFO_VALUE_ARG +#define P2P_INFO_DASH +#endif + +#ifdef DBG_TSF_UPDATE +#define TSF_PAUSE_TIME_TITLE_FMT " %-5s" +#define TSF_PAUSE_TIME_TITLE_ARG , "tsfup" +#define TSF_PAUSE_TIME_VALUE_FMT " %5d" +#define TSF_PAUSE_TIME_VALUE_ARG , ((iface->mlmeextpriv.tsf_update_required && iface->mlmeextpriv.tsf_update_pause_stime) ? (rtw_get_passing_time_ms(iface->mlmeextpriv.tsf_update_pause_stime) > 99999 ? 99999 : rtw_get_passing_time_ms(iface->mlmeextpriv.tsf_update_pause_stime)) : 0) +#else +#define TSF_PAUSE_TIME_TITLE_FMT "" +#define TSF_PAUSE_TIME_TITLE_ARG +#define TSF_PAUSE_TIME_VALUE_FMT "" +#define TSF_PAUSE_TIME_VALUE_ARG +#endif + +#if (defined(CONFIG_SUPPORT_MULTI_BCN) && defined(CONFIG_FW_HANDLE_TXBCN)) || defined(CONFIG_CLIENT_PORT_CFG) +#define INFO_FMT " %-4s" +#define INFO_ARG , "info" +#define INFO_CNT_FMT " %-20s" +#define INFO_CNT_ARG , str_val +#else +#define INFO_FMT "" +#define INFO_ARG +#define INFO_CNT_FMT "" +#define INFO_CNT_ARG +#endif + + RTW_PRINT_SEL(sel, "%-2s %-15s %c %-3s %-3s %-3s %-17s %-4s %-7s %-5s" + P2P_INFO_TITLE_FMT + TSF_PAUSE_TIME_TITLE_FMT + " %s"INFO_FMT"\n" + , "id", "ifname", ' ', "bup", "nup", "ncd", "macaddr", "port", "ch", "class" + P2P_INFO_TITLE_ARG + TSF_PAUSE_TIME_TITLE_ARG + , "status"INFO_ARG); + + RTW_PRINT_SEL(sel, "---------------------------------------------------------------" + P2P_INFO_DASH + "-------\n"); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface) { + #if (defined(CONFIG_SUPPORT_MULTI_BCN) && defined(CONFIG_FW_HANDLE_TXBCN)) || defined(CONFIG_CLIENT_PORT_CFG) + _rtw_memset(&str_val, '\0', sizeof(str_val)); + #endif + #if defined(CONFIG_AP_MODE) && defined(CONFIG_SUPPORT_MULTI_BCN) && defined(CONFIG_FW_HANDLE_TXBCN) + if (MLME_IS_AP(iface) || MLME_IS_MESH(iface)) { + u8 len; + char *p = str_val; + char tmp_str[10] = {'\0'}; + + len = snprintf(tmp_str, sizeof(tmp_str), "%s", "ap_id:"); + strncpy(p, tmp_str, len); + p += len; + _rtw_memset(&tmp_str, '\0', sizeof(tmp_str)); + #ifdef DBG_HW_PORT + len = snprintf(tmp_str, sizeof(tmp_str), "%d (%d,%d)", iface->vap_id, iface->hw_port, iface->client_port); + #else + len = snprintf(tmp_str, sizeof(tmp_str), "%d", iface->vap_id); + #endif + strncpy(p, tmp_str, len); + } + #endif + #ifdef CONFIG_CLIENT_PORT_CFG + if (MLME_IS_STA(iface)) { + u8 len; + char *p = str_val; + char tmp_str[10] = {'\0'}; + + len = snprintf(tmp_str, sizeof(tmp_str), "%s", "c_pid:"); + strncpy(p, tmp_str, len); + p += len; + _rtw_memset(&tmp_str, '\0', sizeof(tmp_str)); + #ifdef DBG_HW_PORT + len = snprintf(tmp_str, sizeof(tmp_str), "%d (%d,%d)", iface->client_port, iface->hw_port, iface->client_port); + #else + len = snprintf(tmp_str, sizeof(tmp_str), "%d", iface->client_port); + #endif + strncpy(p, tmp_str, len); + } + #endif + + RTW_PRINT_SEL(sel, "%2d %-15s %c %3u %3u %3u "MAC_FMT" %4hhu %3u,%u,%u %5u" + P2P_INFO_VALUE_FMT + TSF_PAUSE_TIME_VALUE_FMT + " "MLME_STATE_FMT" " INFO_CNT_FMT"\n" + , i, iface->registered ? ADPT_ARG(iface) : NULL + , iface->registered ? 'R' : ' ' + , iface->bup + , iface->netif_up + , iface->net_closed + , MAC_ARG(adapter_mac_addr(iface)) + , rtw_hal_get_port(iface) + , iface->mlmeextpriv.cur_channel + , iface->mlmeextpriv.cur_bwmode + , iface->mlmeextpriv.cur_ch_offset + , rtw_get_op_class_by_chbw(iface->mlmeextpriv.cur_channel + , iface->mlmeextpriv.cur_bwmode + , iface->mlmeextpriv.cur_ch_offset) + P2P_INFO_VALUE_ARG + TSF_PAUSE_TIME_VALUE_ARG + , MLME_STATE_ARG(iface) + INFO_CNT_ARG + ); + } + } + + RTW_PRINT_SEL(sel, "---------------------------------------------------------------" + P2P_INFO_DASH + "-------\n"); + + if (rtw_mi_get_ch_setting_union(dvobj_get_primary_adapter(dvobj), &u_ch, &u_bw, &u_offset)) + RTW_PRINT_SEL(sel, "%55s %3u,%u,%u %5u\n" + , "union:" + , u_ch, u_bw, u_offset, rtw_get_op_class_by_chbw(u_ch, u_bw, u_offset)); + + RTW_PRINT_SEL(sel, "%55s %3u,%u,%u offch_state:%d\n" + , "oper:" + , dvobj->oper_channel + , dvobj->oper_bwmode + , dvobj->oper_ch_offset + , rfctl->offch_state + ); + +#ifdef CONFIG_DFS_MASTER + if (rfctl->radar_detect_ch != 0) { + RTW_PRINT_SEL(sel, "%55s %3u,%u,%u" + , "radar_detect:" + , rfctl->radar_detect_ch + , rfctl->radar_detect_bw + , rfctl->radar_detect_offset + ); + + if (rfctl->radar_detect_by_others) + _RTW_PRINT_SEL(sel, ", by AP of STA link"); + else { + u32 non_ocp_ms; + u32 cac_ms; + u8 dfs_domain = rtw_rfctl_get_dfs_domain(rfctl); + + _RTW_PRINT_SEL(sel, ", domain:%s(%u)", rtw_dfs_regd_str(dfs_domain), dfs_domain); + + rtw_get_ch_waiting_ms(rfctl + , rfctl->radar_detect_ch + , rfctl->radar_detect_bw + , rfctl->radar_detect_offset + , &non_ocp_ms + , &cac_ms + ); + + if (non_ocp_ms) + _RTW_PRINT_SEL(sel, ", non_ocp:%d", non_ocp_ms); + if (cac_ms) + _RTW_PRINT_SEL(sel, ", cac:%d", cac_ms); + } + + _RTW_PRINT_SEL(sel, "\n"); + } +#endif /* CONFIG_DFS_MASTER */ +#endif /* CONFIG_RTW_DEBUG || CONFIG_PROC_DEBUG */ +} + +#if defined(CONFIG_RTW_DEBUG) || defined(CONFIG_PROC_DEBUG) +#define SEC_CAM_ENT_ID_TITLE_FMT "%-2s" +#define SEC_CAM_ENT_ID_TITLE_ARG "id" +#define SEC_CAM_ENT_ID_VALUE_FMT "%2u" +#define SEC_CAM_ENT_ID_VALUE_ARG(id) (id) + +#define SEC_CAM_ENT_TITLE_FMT "%-6s %-17s %-32s %-3s %-8s %-2s %-2s %-5s" +#define SEC_CAM_ENT_TITLE_ARG "ctrl", "addr", "key", "kid", "type", "MK", "GK", "valid" +#define SEC_CAM_ENT_VALUE_FMT "0x%04x "MAC_FMT" "KEY_FMT" %3u %-8s %2u %2u %5u" +#define SEC_CAM_ENT_VALUE_ARG(ent) \ + (ent)->ctrl \ + , MAC_ARG((ent)->mac) \ + , KEY_ARG((ent)->key) \ + , ((ent)->ctrl) & 0x03 \ + , (((ent)->ctrl) & 0x200) ? \ + security_type_str((((ent)->ctrl) >> 2 & 0x7) | _SEC_TYPE_256_) : \ + security_type_str(((ent)->ctrl) >> 2 & 0x7) \ + , (((ent)->ctrl) >> 5) & 0x01 \ + , (((ent)->ctrl) >> 6) & 0x01 \ + , (((ent)->ctrl) >> 15) & 0x01 + +void dump_sec_cam_ent(void *sel, struct sec_cam_ent *ent, int id) +{ + if (id >= 0) { + RTW_PRINT_SEL(sel, SEC_CAM_ENT_ID_VALUE_FMT " " SEC_CAM_ENT_VALUE_FMT"\n" + , SEC_CAM_ENT_ID_VALUE_ARG(id), SEC_CAM_ENT_VALUE_ARG(ent)); + } else + RTW_PRINT_SEL(sel, SEC_CAM_ENT_VALUE_FMT"\n", SEC_CAM_ENT_VALUE_ARG(ent)); +} + +void dump_sec_cam_ent_title(void *sel, u8 has_id) +{ + if (has_id) { + RTW_PRINT_SEL(sel, SEC_CAM_ENT_ID_TITLE_FMT " " SEC_CAM_ENT_TITLE_FMT"\n" + , SEC_CAM_ENT_ID_TITLE_ARG, SEC_CAM_ENT_TITLE_ARG); + } else + RTW_PRINT_SEL(sel, SEC_CAM_ENT_TITLE_FMT"\n", SEC_CAM_ENT_TITLE_ARG); +} +#endif + +void dump_sec_cam(void *sel, _adapter *adapter) +{ +#if defined(CONFIG_RTW_DEBUG) || defined(CONFIG_PROC_DEBUG) + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + struct sec_cam_ent ent; + int i; + + RTW_PRINT_SEL(sel, "HW sec cam:\n"); + dump_sec_cam_ent_title(sel, 1); + for (i = 0; i < cam_ctl->num; i++) { + rtw_sec_read_cam_ent(adapter, i, (u8 *)(&ent.ctrl), ent.mac, ent.key); + dump_sec_cam_ent(sel , &ent, i); + } +#endif +} + +void dump_sec_cam_cache(void *sel, _adapter *adapter) +{ +#if defined(CONFIG_RTW_DEBUG) || defined(CONFIG_PROC_DEBUG) + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + int i; + + RTW_PRINT_SEL(sel, "SW sec cam cache:\n"); + dump_sec_cam_ent_title(sel, 1); + for (i = 0; i < cam_ctl->num; i++) { + if (dvobj->cam_cache[i].ctrl != 0) + dump_sec_cam_ent(sel, &dvobj->cam_cache[i], i); + } +#endif +} + +static u8 fwdl_test_chksum_fail = 0; +static u8 fwdl_test_wintint_rdy_fail = 0; + +bool rtw_fwdl_test_trigger_chksum_fail(void) +{ + if (fwdl_test_chksum_fail) { + RTW_PRINT("fwdl test case: trigger chksum_fail\n"); + fwdl_test_chksum_fail--; + return _TRUE; + } + return _FALSE; +} + +bool rtw_fwdl_test_trigger_wintint_rdy_fail(void) +{ + if (fwdl_test_wintint_rdy_fail) { + RTW_PRINT("fwdl test case: trigger wintint_rdy_fail\n"); + fwdl_test_wintint_rdy_fail--; + return _TRUE; + } + return _FALSE; +} + +static u8 del_rx_ampdu_test_no_tx_fail = 0; + +bool rtw_del_rx_ampdu_test_trigger_no_tx_fail(void) +{ + if (del_rx_ampdu_test_no_tx_fail) { + RTW_PRINT("del_rx_ampdu test case: trigger no_tx_fail\n"); + del_rx_ampdu_test_no_tx_fail--; + return _TRUE; + } + return _FALSE; +} + +static u32 g_wait_hiq_empty_ms = 0; + +u32 rtw_get_wait_hiq_empty_ms(void) +{ + return g_wait_hiq_empty_ms; +} + +static systime sta_linking_test_start_time = 0; +static u32 sta_linking_test_wait_ms = 0; +static u8 sta_linking_test_force_fail = 0; + +void rtw_sta_linking_test_set_start(void) +{ + sta_linking_test_start_time = rtw_get_current_time(); +} + +bool rtw_sta_linking_test_wait_done(void) +{ + return rtw_get_passing_time_ms(sta_linking_test_start_time) >= sta_linking_test_wait_ms; +} + +bool rtw_sta_linking_test_force_fail(void) +{ + return sta_linking_test_force_fail; +} + +#ifdef CONFIG_AP_MODE +static u16 ap_linking_test_force_auth_fail = 0; +static u16 ap_linking_test_force_asoc_fail = 0; + +u16 rtw_ap_linking_test_force_auth_fail(void) +{ + return ap_linking_test_force_auth_fail; +} + +u16 rtw_ap_linking_test_force_asoc_fail(void) +{ + return ap_linking_test_force_asoc_fail; +} +#endif + +#ifdef CONFIG_PROC_DEBUG +int proc_get_defs_param(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *mlme = &adapter->mlmepriv; + + RTW_PRINT_SEL(m, "%s %15s\n", "lmt_sta", "lmt_time"); + RTW_PRINT_SEL(m, "%-15u %-15u\n" + , mlme->defs_lmt_sta + , mlme->defs_lmt_time + ); + + return 0; +} + +ssize_t proc_set_defs_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *mlme = &adapter->mlmepriv; + + char tmp[32]; + u32 defs_lmt_sta; + u32 defs_lmt_time; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%u %u", &defs_lmt_sta, &defs_lmt_time); + + if (num >= 1) + mlme->defs_lmt_sta = defs_lmt_sta; + if (num >= 2) + mlme->defs_lmt_time = defs_lmt_time; + } + + return count; + +} + +ssize_t proc_set_write_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 addr, val, len; + + if (count < 3) { + RTW_INFO("argument size is less than 3\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%x %x %x", &addr, &val, &len); + + if (num != 3) { + RTW_INFO("invalid write_reg parameter!\n"); + return count; + } + + switch (len) { + case 1: + rtw_write8(padapter, addr, (u8)val); + break; + case 2: + rtw_write16(padapter, addr, (u16)val); + break; + case 4: + rtw_write32(padapter, addr, val); + break; + default: + RTW_INFO("error write length=%d", len); + break; + } + + } + + return count; + +} + +static u32 proc_get_read_addr = 0xeeeeeeee; +static u32 proc_get_read_len = 0x4; + +int proc_get_read_reg(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + if (proc_get_read_addr == 0xeeeeeeee) { + RTW_PRINT_SEL(m, "address not initialized\n"); + return 0; + } + + switch (proc_get_read_len) { + case 1: + RTW_PRINT_SEL(m, "rtw_read8(0x%x)=0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr)); + break; + case 2: + RTW_PRINT_SEL(m, "rtw_read16(0x%x)=0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr)); + break; + case 4: + RTW_PRINT_SEL(m, "rtw_read32(0x%x)=0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr)); + break; + default: + RTW_PRINT_SEL(m, "error read length=%d\n", proc_get_read_len); + break; + } + + return 0; +} + +ssize_t proc_set_read_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[16]; + u32 addr, len; + + if (count < 2) { + RTW_INFO("argument size is less than 2\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%x %x", &addr, &len); + + if (num != 2) { + RTW_INFO("invalid read_reg parameter!\n"); + return count; + } + + proc_get_read_addr = addr; + + proc_get_read_len = len; + } + + return count; + +} + +int proc_get_rx_stat(struct seq_file *m, void *v) +{ + _irqL irqL; + _list *plist, *phead; + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct sta_info *psta = NULL; + struct stainfo_stats *pstats = NULL; + struct sta_priv *pstapriv = &(adapter->stapriv); + u32 i, j; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 null_addr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + pstats = &psta->sta_stats; + + if (pstats == NULL) + continue; + if ((_rtw_memcmp(psta->cmn.mac_addr, bc_addr, ETH_ALEN) != _TRUE) + && (_rtw_memcmp(psta->cmn.mac_addr, null_addr, ETH_ALEN) != _TRUE) + && (_rtw_memcmp(psta->cmn.mac_addr, adapter_mac_addr(adapter), ETH_ALEN) != _TRUE)) { + RTW_PRINT_SEL(m, "MAC :\t\t"MAC_FMT "\n", MAC_ARG(psta->cmn.mac_addr)); + RTW_PRINT_SEL(m, "data_rx_cnt :\t%llu\n", sta_rx_data_uc_pkts(psta) - pstats->last_rx_data_uc_pkts); + pstats->last_rx_data_uc_pkts = sta_rx_data_uc_pkts(psta); + RTW_PRINT_SEL(m, "duplicate_cnt :\t%u\n", pstats->duplicate_cnt); + pstats->duplicate_cnt = 0; + RTW_PRINT_SEL(m, "rx_per_rate_cnt :\n"); + + for (j = 0; j < 0x60; j++) { + RTW_PRINT_SEL(m, "%08u ", pstats->rxratecnt[j]); + pstats->rxratecnt[j] = 0; + if ((j%8) == 7) + RTW_PRINT_SEL(m, "\n"); + } + RTW_PRINT_SEL(m, "\n"); + } + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + return 0; +} + +int proc_get_tx_stat(struct seq_file *m, void *v) +{ + _irqL irqL; + _list *plist, *phead; + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct sta_info *psta = NULL; + u8 sta_mac[NUM_STA][ETH_ALEN] = {{0}}; + uint mac_id[NUM_STA]; + struct stainfo_stats *pstats = NULL; + struct sta_priv *pstapriv = &(adapter->stapriv); + struct sta_priv *pstapriv_primary = &(GET_PRIMARY_ADAPTER(adapter))->stapriv; + u32 i, macid_rec_idx = 0; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 null_addr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + struct submit_ctx gotc2h; + + if (!adapter->hal_func.reqtxrpt) { + RTW_PRINT_SEL(m, "Not support.\n"); + return 0; + } + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + if ((_rtw_memcmp(psta->cmn.mac_addr, bc_addr, ETH_ALEN) != _TRUE) + && (_rtw_memcmp(psta->cmn.mac_addr, null_addr, ETH_ALEN) != _TRUE) + && (_rtw_memcmp(psta->cmn.mac_addr, adapter_mac_addr(adapter), ETH_ALEN) != _TRUE)) { + _rtw_memcpy(&sta_mac[macid_rec_idx][0], psta->cmn.mac_addr, ETH_ALEN); + mac_id[macid_rec_idx] = psta->cmn.mac_id; + macid_rec_idx++; + } + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < macid_rec_idx; i++) { + _rtw_memcpy(pstapriv_primary->c2h_sta_mac, &sta_mac[i][0], ETH_ALEN); + pstapriv_primary->c2h_adapter_id = adapter->iface_id; + rtw_sctx_init(&gotc2h, 60); + pstapriv_primary->gotc2h = &gotc2h; + rtw_hal_reqtxrpt(adapter, mac_id[i]); + if (rtw_sctx_wait(&gotc2h, __func__)) { + psta = rtw_get_stainfo(pstapriv, &sta_mac[i][0]); + if(psta) { + pstats = &psta->sta_stats; +#if (!defined(ROKU_PRIVATE) && !defined(CONFIG_RTW_MULTI_AP)) + RTW_PRINT_SEL(m, "data_sent_cnt :\t%u\n", pstats->tx_ok_cnt + pstats->tx_fail_cnt); + RTW_PRINT_SEL(m, "success_cnt :\t%u\n", pstats->tx_ok_cnt); + RTW_PRINT_SEL(m, "failure_cnt :\t%u\n", pstats->tx_fail_cnt); + RTW_PRINT_SEL(m, "retry_cnt :\t%u\n\n", pstats->tx_retry_cnt); +#else + RTW_PRINT_SEL(m, "MAC: " MAC_FMT " sent: %u fail: %u retry: %u\n", + MAC_ARG(&sta_mac[i][0]), pstats->tx_ok_cnt, pstats->tx_fail_cnt, pstats->tx_retry_cnt); +#endif /* ROKU_PRIVATE */ + + } else + RTW_PRINT_SEL(m, "STA is gone\n"); + } else { + //to avoid c2h modify counters + pstapriv_primary->gotc2h = NULL; + _rtw_memset(pstapriv_primary->c2h_sta_mac, 0, ETH_ALEN); + pstapriv_primary->c2h_adapter_id = CONFIG_IFACE_NUMBER; + RTW_PRINT_SEL(m, "Warming : Query timeout, operation abort!!\n"); + break; + } + pstapriv_primary->gotc2h = NULL; + _rtw_memset(pstapriv_primary->c2h_sta_mac, 0, ETH_ALEN); + pstapriv_primary->c2h_adapter_id = CONFIG_IFACE_NUMBER; + } + return 0; +} + +int proc_get_fwstate(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + RTW_PRINT_SEL(m, "fwstate=0x%x\n", get_fwstate(pmlmepriv)); + + return 0; +} + +int proc_get_sec_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct security_priv *sec = &padapter->securitypriv; + + RTW_PRINT_SEL(m, "auth_alg=0x%x, enc_alg=0x%x, auth_type=0x%x, enc_type=0x%x\n", + sec->dot11AuthAlgrthm, sec->dot11PrivacyAlgrthm, + sec->ndisauthtype, sec->ndisencryptstatus); + + RTW_PRINT_SEL(m, "hw_decrypted=%d\n", sec->hw_decrypted); + +#ifdef DBG_SW_SEC_CNT + RTW_PRINT_SEL(m, "wep_sw_enc_cnt=%llu, %llu, %llu\n" + , sec->wep_sw_enc_cnt_bc , sec->wep_sw_enc_cnt_mc, sec->wep_sw_enc_cnt_uc); + RTW_PRINT_SEL(m, "wep_sw_dec_cnt=%llu, %llu, %llu\n" + , sec->wep_sw_dec_cnt_bc , sec->wep_sw_dec_cnt_mc, sec->wep_sw_dec_cnt_uc); + + RTW_PRINT_SEL(m, "tkip_sw_enc_cnt=%llu, %llu, %llu\n" + , sec->tkip_sw_enc_cnt_bc , sec->tkip_sw_enc_cnt_mc, sec->tkip_sw_enc_cnt_uc); + RTW_PRINT_SEL(m, "tkip_sw_dec_cnt=%llu, %llu, %llu\n" + , sec->tkip_sw_dec_cnt_bc , sec->tkip_sw_dec_cnt_mc, sec->tkip_sw_dec_cnt_uc); + + RTW_PRINT_SEL(m, "aes_sw_enc_cnt=%llu, %llu, %llu\n" + , sec->aes_sw_enc_cnt_bc , sec->aes_sw_enc_cnt_mc, sec->aes_sw_enc_cnt_uc); + RTW_PRINT_SEL(m, "aes_sw_dec_cnt=%llu, %llu, %llu\n" + , sec->aes_sw_dec_cnt_bc , sec->aes_sw_dec_cnt_mc, sec->aes_sw_dec_cnt_uc); + + RTW_PRINT_SEL(m, "gcmp_sw_enc_cnt=%llu, %llu, %llu\n" + , sec->gcmp_sw_enc_cnt_bc , sec->gcmp_sw_enc_cnt_mc, sec->gcmp_sw_enc_cnt_uc); + RTW_PRINT_SEL(m, "gcmp_sw_dec_cnt=%llu, %llu, %llu\n" + , sec->gcmp_sw_dec_cnt_bc , sec->gcmp_sw_dec_cnt_mc, sec->gcmp_sw_dec_cnt_uc); +#endif /* DBG_SW_SEC_CNT */ + + return 0; +} + +int proc_get_mlmext_state(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + RTW_PRINT_SEL(m, "pmlmeinfo->state=0x%x\n", pmlmeinfo->state); + + return 0; +} + +#ifdef CONFIG_LAYER2_ROAMING +int proc_get_roam_flags(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "0x%02x\n", rtw_roam_flags(adapter)); + + return 0; +} + +ssize_t proc_set_roam_flags(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + char tmp[32]; + u8 flags; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx", &flags); + + if (num == 1) + rtw_assign_roam_flags(adapter, flags); + } + + return count; + +} + +int proc_get_roam_param(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *mlme = &adapter->mlmepriv; + + RTW_PRINT_SEL(m, "%12s %15s %26s %16s\n", "rssi_diff_th", "scanr_exp_ms", "scan_interval(unit:2 sec)", "rssi_threshold"); + RTW_PRINT_SEL(m, "%-15u %-13u %-27u %-11u\n" + , mlme->roam_rssi_diff_th + , mlme->roam_scanr_exp_ms + , mlme->roam_scan_int + , mlme->roam_rssi_threshold + ); + + return 0; +} + +ssize_t proc_set_roam_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *mlme = &adapter->mlmepriv; + + char tmp[32]; + u8 rssi_diff_th; + u32 scanr_exp_ms; + u32 scan_int; + u8 rssi_threshold; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhu %u %u %hhu", &rssi_diff_th, &scanr_exp_ms, &scan_int, &rssi_threshold); + + if (num >= 1) + mlme->roam_rssi_diff_th = rssi_diff_th; + if (num >= 2) + mlme->roam_scanr_exp_ms = scanr_exp_ms; + if (num >= 3) + mlme->roam_scan_int = scan_int; + if (num >= 4) + mlme->roam_rssi_threshold = rssi_threshold; + } + + return count; + +} + +ssize_t proc_set_roam_tgt_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + char tmp[32]; + u8 addr[ETH_ALEN]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", addr, addr + 1, addr + 2, addr + 3, addr + 4, addr + 5); + if (num == 6) + _rtw_memcpy(adapter->mlmepriv.roam_tgt_addr, addr, ETH_ALEN); + + RTW_INFO("set roam_tgt_addr to "MAC_FMT"\n", MAC_ARG(adapter->mlmepriv.roam_tgt_addr)); + } + + return count; +} +#endif /* CONFIG_LAYER2_ROAMING */ + +#ifdef CONFIG_WAR_OFFLOAD +int proc_get_war_offload_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + + if (_TRUE == pwrpriv->wowlan_war_offload_mode) { + RTW_PRINT_SEL(m, "\n[ Offload Feature Enabled ]\n"); + + if (WAR_ARP_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) { + RTW_PRINT_SEL(m, "\n ARP Reponse offload enabled\n"); + } +#ifdef CONFIG_OFFLOAD_MDNS_V4 + if (WAR_MDNS_V4_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) { + RTW_PRINT_SEL(m, "\n MDNS v4 Reponse offload enabled\n"); + } + if (WAR_MDNS_V4_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl) { + RTW_PRINT_SEL(m, "\n MDNS v4 Wakeup offload enabled\n"); + } +#endif /* CONFIG_OFFLOAD_MDNS_v4 */ +#ifdef CONFIG_OFFLOAD_MDNS_V6 + if (WAR_MDNS_V6_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) { + RTW_PRINT_SEL(m, "\n MDNS v6 Reponse offload enabled\n"); + } + if (WAR_MDNS_V6_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl) { + RTW_PRINT_SEL(m, "\n MDNS v6 Wakeup offload enabled\n"); + } +#endif /* CONFIG_OFFLOAD_MDNS_V6 */ + + if (WAR_ARP_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl) { + RTW_PRINT_SEL(m, "\n ARP Request wakeup enabled\n"); + } + + } else { + RTW_PRINT_SEL(m, "\n[ Offload Feature Disabled ]\n"); + } + + return 0; +} + +ssize_t proc_set_war_offload_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char tmp[32]; + u32 offload_cfg = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%x", &offload_cfg); + + if (num == 1) { + RTW_INFO(FUNC_ADPT_FMT ": Set war offload cfg = %x\n", FUNC_ADPT_ARG(padapter), offload_cfg); + pwrpriv->wowlan_war_offload_ctrl = offload_cfg; + pwrpriv->wowlan_war_offload_mode = offload_cfg?_TRUE:_FALSE; + + } + } + + return count; +} + +ssize_t proc_set_war_offload_ipv4_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char tmp[128]; + u32 ip_addr = 0, ip_subnet = 0, ip_gateway = 0, index = 0; + struct war_ipv4_fmt* pip_info = &pwrpriv->wowlan_war_offload_ipv4; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%d %x %x %x", &index, &ip_addr, &ip_subnet, &ip_gateway); + + if (num == 4) { + pip_info->ip_addr[index-1] = ip_addr; + pip_info->ip_subnet[index-1] = ip_subnet; + pip_info->ip_gateway[index-1] = ip_gateway; + RTW_INFO(FUNC_ADPT_FMT "Setup IPv4 address:\n", FUNC_ADPT_ARG(padapter)); + RTW_INFO("Index(%d) IP=%d.%d.%d.%d\n", index, (ip_addr & 0xff), ((ip_addr & 0xff00)>>8), ((ip_addr & 0xff0000)>>16), ((ip_addr & 0xff000000)>>24)); + } else { + RTW_INFO("Wrong input buffer count (%d)\n", num); + return -EFAULT; + } + } + + return count; +} + +ssize_t proc_set_war_offload_ipv6_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char tmp[255]; + u32 ip_addr = 0, ip_subnet = 0, ip_gateway = 0; + struct war_ipv6_fmt* pip_info = &pwrpriv->wowlan_war_offload_ipv6; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num; + int i; + u32 index; + u16 val[64]; + u16 big_endian_val[64]; + + num = sscanf(tmp, "%d %hx:%hx:%hx:%hx:%hx:%hx:%hx:%hx", &index, &val[0], &val[1], &val[2], &val[3], &val[4], &val[5], &val[6], &val[7]); + for (i=0;i<8;i++) { + big_endian_val[i] = htons(val[i]); + } + _rtw_memcpy(pip_info->ipv6_addr[index-1], big_endian_val, RTW_IPv6_ADDR_LEN); + + if (num == 9) { + RTW_INFO(FUNC_ADPT_FMT "Setup IPv6 address\n", FUNC_ADPT_ARG(padapter)); + } else { + RTW_INFO("Wrong input count (%d)\n", num); + return -EFAULT; + } + } + + return count; +} + +#if defined(CONFIG_OFFLOAD_MDNS_V4) || defined(CONFIG_OFFLOAD_MDNS_V6) + +int proc_get_war_offload_mdns_domain_name(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + + if (_TRUE == pwrpriv->wowlan_war_offload_mode) { + if ((WAR_MDNS_V4_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V4_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl)) { + RTW_PRINT_SEL(m, "\nDomain Name:[%s](%d)\n\n", + pwrpriv->wowlan_war_offload_mdns_domain_name, pwrpriv->wowlan_war_offload_mdns_domain_name_len); + } else { + RTW_PRINT_SEL(m, "\nMSND RSP Not enabled\n\n"); + } + } else { + RTW_PRINT_SEL(m, "\nOffload Not enabled\n\n"); + } + + return 0; +} + +ssize_t proc_set_war_offload_mdns_domain_name(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char tmp[MAX_MDNS_DOMAIN_NAME_LEN+1]; + char domain_name[MAX_MDNS_DOMAIN_NAME_LEN+1]; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is large than MAX_MDNS_DOMAIN_NAME_LEN(%d)\n", FUNC_ADPT_ARG(padapter), MAX_MDNS_DOMAIN_NAME_LEN); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%s", domain_name); + if(1 == num) { + pwrpriv->wowlan_war_offload_mdns_domain_name_len = strlen(domain_name); + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_domain_name, 0x00, MAX_MDNS_DOMAIN_NAME_LEN); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_domain_name, domain_name, strlen(domain_name)); + } + } + + return count; +} + + +int proc_get_war_offload_mdns_machine_name(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + int i=0; + + if (_TRUE == pwrpriv->wowlan_war_offload_mode) { + if ((WAR_MDNS_V4_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V4_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl)) { + for(i=0; iwowlan_war_offload_mdns_mnane_num; i++) + { + RTW_PRINT_SEL(m, "[%d]", i); + rtw_wow_war_mdns_dump_buf(m, "Machine Name", + pwrpriv->wowlan_war_offload_mdns_mnane[i].name, pwrpriv->wowlan_war_offload_mdns_mnane[i].name_len); + } + RTW_PRINT_SEL(m, "\n"); + } else { + RTW_PRINT_SEL(m, "\nMSND RSP Not enabled\n\n"); + } + } else { + RTW_PRINT_SEL(m, "\nOffload Not enabled\n\n"); + } + + return 0; +} + + +ssize_t proc_set_war_offload_mdns_machine_name(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char tmp[MAX_MDNS_MACHINE_NAME_LEN*3-1+1]; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length, %lu, is large than MAX_MDNS_MACHINE_NAME_LEN(%d)\n", FUNC_ADPT_ARG(padapter), (count+1)/3, MAX_MDNS_MACHINE_NAME_LEN); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + if( strncmp(tmp, "clean", 5) == 0 ) + { + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_mnane, 0, sizeof(pwrpriv->wowlan_war_offload_mdns_mnane)); + pwrpriv->wowlan_war_offload_mdns_mnane_num = 0; + }else{ + int idx = pwrpriv->wowlan_war_offload_mdns_mnane_num; + if(idx == MAX_MDNS_MACHINE_NAME_NUM){ + RTW_INFO(FUNC_ADPT_FMT ": the num of machine name is already %d(MAX_MDNS_MACHINE_NAME_NUM)!\n", FUNC_ADPT_ARG(padapter), MAX_MDNS_MACHINE_NAME_NUM); + return -EFAULT; + } + if(rtw_wow_war_mdns_parser_pattern(tmp, pwrpriv->wowlan_war_offload_mdns_mnane[idx].name, + (u32 *) &pwrpriv->wowlan_war_offload_mdns_mnane[idx].name_len, MAX_MDNS_MACHINE_NAME_LEN)) + pwrpriv->wowlan_war_offload_mdns_mnane_num++; + } + } + + return count; +} + + +int proc_get_war_offload_mdns_service_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + struct war_mdns_service_info *psinfo = pwrpriv->wowlan_war_offload_mdns_service; + int i=0, j=0; + + if (_TRUE == pwrpriv->wowlan_war_offload_mode) { + if ((WAR_MDNS_V4_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V4_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_WAKEUP_EN & pwrpriv->wowlan_war_offload_ctrl)) { + for(i=0; iwowlan_war_offload_mdns_service_info_num; i++) + { + RTW_PRINT_SEL(m, "[%d] service info ===> \n", i+1); + RTW_PRINT_SEL(m, "\tservice-transport-domain : %s(%d)- %s(%d)- %s(%d)\n", + psinfo[i].service, psinfo[i].service_len, + psinfo[i].transport, psinfo[i].transport_len, + psinfo[i].domain, psinfo[i].domain_len); + RTW_PRINT_SEL(m, "\ttarget for srv rsp : %s(%d)\n", psinfo[i].target, psinfo[i].target_len); + RTW_PRINT_SEL(m, "\tport : %x-%x, ttl : %d \n", psinfo[i].port[0], psinfo[i].port[1], psinfo[i].ttl); + j = psinfo[i].txt_rsp_idx; + RTW_PRINT_SEL(m, "\ttype txt rsp. [%d] \n", j); + rtw_wow_war_mdns_dump_txt(m, "type txt rsp. (Str)", + pwrpriv->wowlan_war_offload_mdns_txt_rsp[j].txt, pwrpriv->wowlan_war_offload_mdns_txt_rsp[j].txt_len); + + } + RTW_PRINT_SEL(m, "\n"); + } else { + RTW_PRINT_SEL(m, "\nMSND RSP Not enabled\n\n"); + } + } else { + RTW_PRINT_SEL(m, "\nOffload Not enabled\n\n"); + } + + return 0; +} + +ssize_t proc_set_war_offload_mdns_service_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + struct war_mdns_service_info *psinfo = pwrpriv->wowlan_war_offload_mdns_service; + u8 idx = 0, port[2], i=0; + char *tmp=NULL; + char srv[MAX_MDNS_SERVICE_NAME_LEN+1], trans[MAX_MDNS_TRANS_LEN+1], domain[MAX_MDNS_DOMAIN_LEN+1]; + char target[MAX_MDNS_TARGET_LEN+1]; + u32 ttl, tmp_txt_len=0, port0 =0, port1 =0; + u16 max_input_size = (MAX_MDNS_SERVICE_NAME_LEN+MAX_MDNS_TRANS_LEN+MAX_MDNS_DOMAIN_LEN+MAX_MDNS_TARGET_LEN+2); + int txt_idx; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > (sizeof(char)*(max_input_size)) ) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + tmp = rtw_zvmalloc(sizeof(char)*(max_input_size)); + if (NULL == tmp) { + RTW_INFO(FUNC_ADPT_FMT ": tmp buffer allocate fail!!\n", FUNC_ADPT_ARG(padapter)); + count = -EFAULT; + goto exit; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%15s %4s %5s %63s %x %x %u %d", srv, trans, domain, target, &port0, &port1, &ttl, &txt_idx); + /* MAX_MDNS_SERVICE_NAME_LEN(15), MAX_MDNS_TRANS_LEN(4), MAX_MDNS_DOMAIN_LEN(5), MAX_MDNS_TARGET_LEN(63) */ + int idx = pwrpriv->wowlan_war_offload_mdns_service_info_num; + u16 curent_txt_total_size = 0; + //u16 sscanf_parameter_length = strlen(srv)+strlen(trans)+strlen(domain)+strlen(target)+2+2+4+1+num; + + if( strncmp(srv, "clean", 5) == 0 ) { + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_service, 0, sizeof(pwrpriv->wowlan_war_offload_mdns_service)); + pwrpriv->wowlan_war_offload_mdns_service_info_num = 0; + } + /*else if(count != sscanf_parameter_length) + { + RTW_INFO(FUNC_ADPT_FMT ": Length of total parameters does not match the input buffer. (%d != %lu)\n", + FUNC_ADPT_ARG(padapter), sscanf_parameter_length, count); + RTW_INFO(FUNC_ADPT_FMT ": Please check the content and length of each parameter.\n", FUNC_ADPT_ARG(padapter)); + RTW_INFO(FUNC_ADPT_FMT ": input buffer = (%s)(%lu)!\n\n", FUNC_ADPT_ARG(padapter), tmp, count); + RTW_INFO(FUNC_ADPT_FMT ": srv = %s (%lu)!\n", FUNC_ADPT_ARG(padapter), srv, strlen(srv)); + RTW_INFO(FUNC_ADPT_FMT ": trans = %s (%lu)!\n", FUNC_ADPT_ARG(padapter), trans, strlen(trans)); + RTW_INFO(FUNC_ADPT_FMT ": domain = %s (%lu)!\n", FUNC_ADPT_ARG(padapter), domain, strlen(domain)); + RTW_INFO(FUNC_ADPT_FMT ": target = %s (%lu)!\n", FUNC_ADPT_ARG(padapter), target, strlen(target)); + RTW_INFO(FUNC_ADPT_FMT ": port = %x-%x, ttl = %d!\n", FUNC_ADPT_ARG(padapter), port0, port1, ttl); + RTW_INFO(FUNC_ADPT_FMT ": txt idx = %d!\n", FUNC_ADPT_ARG(padapter), txt_idx); + count = -EFAULT; + goto exit; + }*/else + { + port[0] = (u8)port0; + port[1] = (u8)port1; + + if(txt_idx >= MAX_MDNS_TXT_NUM) { + RTW_INFO(FUNC_ADPT_FMT ": input txt idx, %d, is out of range (0~%d)!\n", FUNC_ADPT_ARG(padapter), txt_idx, MAX_MDNS_TXT_NUM-1); + count = -EFAULT; + goto exit; + } + + if(pwrpriv->wowlan_war_offload_mdns_txt_rsp[txt_idx].txt_len == 0) { + RTW_INFO(FUNC_ADPT_FMT ": wowlan_war_offload_mdns_txt_rsp[%d] is null! Please initiate it first.\n", FUNC_ADPT_ARG(padapter), txt_idx); + count = -EFAULT; + goto exit; + } + + // 1. set the value of members for this new service + psinfo[idx].service_len = strlen(srv); + _rtw_memcpy(psinfo[idx].service, srv, psinfo[idx].service_len ); + psinfo[idx].transport_len = strlen(trans); + _rtw_memcpy(psinfo[idx].transport, trans, psinfo[idx].transport_len ); + psinfo[idx].domain_len = strlen(domain); + _rtw_memcpy(psinfo[idx].domain, domain, psinfo[idx].domain_len ); + psinfo[idx].target_len = strlen(target); + _rtw_memcpy(psinfo[idx].target, target, psinfo[idx].target_len ); + _rtw_memcpy(psinfo[idx].port, port, 2 ); + psinfo[idx].ttl = ttl; + psinfo[idx].txt_rsp_idx = txt_idx; + pwrpriv->wowlan_war_offload_mdns_service_info_num++; + } + } + +exit: + if(tmp) + rtw_vmfree(tmp, sizeof(char)*(max_input_size)); + return count; + +} + +int proc_get_war_offload_mdns_txt_rsp(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + int i=0; + + if (_TRUE == pwrpriv->wowlan_war_offload_mode) { + if ((WAR_MDNS_V4_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) || + (WAR_MDNS_V6_RSP_EN & pwrpriv->wowlan_war_offload_ctrl) ) { + for(i=0; iwowlan_war_offload_mdns_txt_rsp_num; i++) { + RTW_PRINT_SEL(m, "[%d]", i); + if(pwrpriv->wowlan_war_offload_mdns_txt_rsp[i].txt_len==0){ + RTW_PRINT_SEL(m, " (null)\n"); + continue; + } + rtw_wow_war_mdns_dump_txt(m, "type txt rsp. (Str)", + pwrpriv->wowlan_war_offload_mdns_txt_rsp[i].txt, pwrpriv->wowlan_war_offload_mdns_txt_rsp[i].txt_len); + rtw_wow_war_mdns_dump_buf(m, "type txt rsp. (Hex)", + pwrpriv->wowlan_war_offload_mdns_txt_rsp[i].txt, pwrpriv->wowlan_war_offload_mdns_txt_rsp[i].txt_len); + } + RTW_PRINT_SEL(m, "\n"); + } else { + RTW_PRINT_SEL(m, "\nMSND RSP Not enabled\n\n"); + } + } else { + RTW_PRINT_SEL(m, "\nOffload Not enabled\n\n"); + } + + return 0; +} + +ssize_t proc_set_war_offload_mdns_txt_rsp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + u16 max_input_size = (1+6+MAX_MDNS_TXT_SINGLE_LEN+2); + char* tmp=NULL; + char op[7]={0}, txt_str[MAX_MDNS_TXT_SINGLE_LEN+1]={0}; + int idx; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + tmp = rtw_zvmalloc(sizeof(char)*(max_input_size)); + if (NULL == tmp) { + RTW_INFO(FUNC_ADPT_FMT ": tmp buffer allocate fail!!\n", FUNC_ADPT_ARG(padapter)); + count = -EFAULT; + goto exit; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + if( strncmp(tmp, "clean", 5) == 0 ) + { + /* clean ==> */ + if(pwrpriv->wowlan_war_offload_mdns_service_info_num==0){ + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_txt_rsp, 0, sizeof(pwrpriv->wowlan_war_offload_mdns_txt_rsp)); + }else{ + RTW_INFO(FUNC_ADPT_FMT ": Txt rsp are refered! (Current service_info_num = %d)\n", FUNC_ADPT_ARG(padapter), pwrpriv->wowlan_war_offload_mdns_service_info_num); + count = -EFAULT; + goto exit; + } + + }else{ + /* set ==> */ + int num = sscanf(tmp, "%d %6s %256c", &idx, op, txt_str); + u16 sscanf_parameter_length = 0, txt_len = 0; + + txt_len = (strlen(txt_str)>MAX_MDNS_TXT_SINGLE_LEN)?MAX_MDNS_TXT_SINGLE_LEN:(strlen(txt_str)-1); + txt_str[txt_len]='\0'; + sscanf_parameter_length = 1 + strlen(op) + txt_len + num; + + if(count != sscanf_parameter_length) { + RTW_INFO(FUNC_ADPT_FMT ": Length of total parameters does not match the input buffer. (%d != %lu)(num=%d)\n", + FUNC_ADPT_ARG(padapter), sscanf_parameter_length, count, num); + RTW_INFO(FUNC_ADPT_FMT ": Please check the content and length of each parameter.\n", FUNC_ADPT_ARG(padapter)); + RTW_INFO(FUNC_ADPT_FMT ": input buffer = (%s)(%lu)!\n\n", FUNC_ADPT_ARG(padapter), tmp, count); + RTW_INFO(FUNC_ADPT_FMT ": op. = %s (%lu)!\n", FUNC_ADPT_ARG(padapter), op, strlen(op)); + RTW_INFO(FUNC_ADPT_FMT ": txt = %s (%lu)!\n", FUNC_ADPT_ARG(padapter), txt_str, strlen(txt_str)); + count = -EFAULT; + goto exit; + } else { + + u16 offset; + + if(idx >= MAX_MDNS_TXT_NUM) { + RTW_INFO(FUNC_ADPT_FMT ": the index, %d, is over the range of txt rsp(0~%d)!\n", FUNC_ADPT_ARG(padapter), idx, MAX_MDNS_TXT_NUM-1); + count = -EFAULT; + goto exit; + } + + if( strncmp(op, "new", 3) == 0 ) { + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt, 0, pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len); + pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len = 0; + }else if(strncmp(op, "append", 6) == 0 ){ + if((pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len+strlen(txt_str)+1) > MAX_MDNS_TXT_LEN) { + RTW_INFO(FUNC_ADPT_FMT ": the txt rsp(%d) will be over the limitation(%d) if append input string(%lu)!\n", FUNC_ADPT_ARG(padapter), + pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len, + MAX_MDNS_TXT_LEN, strlen(txt_str)+1); + count = -EFAULT; + goto exit; + } + }else{ + RTW_INFO(FUNC_ADPT_FMT ": Invaild op str %s (new/append only)!\n", FUNC_ADPT_ARG(padapter), op); + count = -EFAULT; + goto exit; + } + + offset = pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len; + pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt[offset++] = strlen(txt_str); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt + offset, txt_str, strlen(txt_str)); + pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len += (strlen(txt_str) + 1); /* actul len with length field */ + + /* Dump ==> */ + //RTW_PRINT_SEL(RTW_DBGDUMP, "[%d]", idx); + //rtw_wow_war_mdns_dump_txt(RTW_DBGDUMP, "type txt rsp. (Str)", + // pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt, pwrpriv->wowlan_war_offload_mdns_txt_rsp[idx].txt_len); + } + } + } + +exit: + if(tmp) + rtw_vmfree(tmp, sizeof(char)*(max_input_size)); + return count; + +} + +#endif /* CONFIG_OFFLOAD_MDNS_V4 || CONFIG_OFFLOAD_MDNS_V6 */ +#endif /* CONFIG_WAR_OFFLOAD */ + + + +int proc_get_qos_option(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + RTW_PRINT_SEL(m, "qos_option=%d\n", pmlmepriv->qospriv.qos_option); + + return 0; +} + +int proc_get_ht_option(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + +#ifdef CONFIG_80211N_HT + RTW_PRINT_SEL(m, "ht_option=%d\n", pmlmepriv->htpriv.ht_option); +#endif /* CONFIG_80211N_HT */ + + return 0; +} + +int proc_get_rf_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + RTW_PRINT_SEL(m, "cur_ch=%d, cur_bw=%d, cur_ch_offet=%d\n", + pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset); + + RTW_PRINT_SEL(m, "oper_ch=%d, oper_bw=%d, oper_ch_offet=%d\n", + rtw_get_oper_ch(padapter), rtw_get_oper_bw(padapter), rtw_get_oper_choffset(padapter)); + + return 0; +} + +int proc_get_scan_param(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct ss_res *ss = &mlmeext->sitesurvey_res; + +#define SCAN_PARAM_TITLE_FMT "%10s" +#define SCAN_PARAM_VALUE_FMT "%-10u" +#define SCAN_PARAM_TITLE_ARG , "scan_ch_ms" +#define SCAN_PARAM_VALUE_ARG , ss->scan_ch_ms +#ifdef CONFIG_80211N_HT +#define SCAN_PARAM_TITLE_FMT_HT " %15s %13s" +#define SCAN_PARAM_VALUE_FMT_HT " %-15u %-13u" +#define SCAN_PARAM_TITLE_ARG_HT , "rx_ampdu_accept", "rx_ampdu_size" +#define SCAN_PARAM_VALUE_ARG_HT , ss->rx_ampdu_accept, ss->rx_ampdu_size +#else +#define SCAN_PARAM_TITLE_FMT_HT "" +#define SCAN_PARAM_VALUE_FMT_HT "" +#define SCAN_PARAM_TITLE_ARG_HT +#define SCAN_PARAM_VALUE_ARG_HT +#endif +#ifdef CONFIG_SCAN_BACKOP +#define SCAN_PARAM_TITLE_FMT_BACKOP " %9s %12s" +#define SCAN_PARAM_VALUE_FMT_BACKOP " %-9u %-12u" +#define SCAN_PARAM_TITLE_ARG_BACKOP , "backop_ms", "scan_cnt_max" +#define SCAN_PARAM_VALUE_ARG_BACKOP , ss->backop_ms, ss->scan_cnt_max +#else +#define SCAN_PARAM_TITLE_FMT_BACKOP "" +#define SCAN_PARAM_VALUE_FMT_BACKOP "" +#define SCAN_PARAM_TITLE_ARG_BACKOP +#define SCAN_PARAM_VALUE_ARG_BACKOP +#endif + + RTW_PRINT_SEL(m, + SCAN_PARAM_TITLE_FMT + SCAN_PARAM_TITLE_FMT_HT + SCAN_PARAM_TITLE_FMT_BACKOP + "\n" + SCAN_PARAM_TITLE_ARG + SCAN_PARAM_TITLE_ARG_HT + SCAN_PARAM_TITLE_ARG_BACKOP + ); + + RTW_PRINT_SEL(m, + SCAN_PARAM_VALUE_FMT + SCAN_PARAM_VALUE_FMT_HT + SCAN_PARAM_VALUE_FMT_BACKOP + "\n" + SCAN_PARAM_VALUE_ARG + SCAN_PARAM_VALUE_ARG_HT + SCAN_PARAM_VALUE_ARG_BACKOP + ); + + return 0; +} + +ssize_t proc_set_scan_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct ss_res *ss = &mlmeext->sitesurvey_res; + + char tmp[32] = {0}; + + u16 scan_ch_ms; +#define SCAN_PARAM_INPUT_FMT "%hu" +#define SCAN_PARAM_INPUT_ARG , &scan_ch_ms +#ifdef CONFIG_80211N_HT + u8 rx_ampdu_accept; + u8 rx_ampdu_size; +#define SCAN_PARAM_INPUT_FMT_HT " %hhu %hhu" +#define SCAN_PARAM_INPUT_ARG_HT , &rx_ampdu_accept, &rx_ampdu_size +#else +#define SCAN_PARAM_INPUT_FMT_HT "" +#define SCAN_PARAM_INPUT_ARG_HT +#endif +#ifdef CONFIG_SCAN_BACKOP + u16 backop_ms; + u8 scan_cnt_max; +#define SCAN_PARAM_INPUT_FMT_BACKOP " %hu %hhu" +#define SCAN_PARAM_INPUT_ARG_BACKOP , &backop_ms, &scan_cnt_max +#else +#define SCAN_PARAM_INPUT_FMT_BACKOP "" +#define SCAN_PARAM_INPUT_ARG_BACKOP +#endif + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, + SCAN_PARAM_INPUT_FMT + SCAN_PARAM_INPUT_FMT_HT + SCAN_PARAM_INPUT_FMT_BACKOP + SCAN_PARAM_INPUT_ARG + SCAN_PARAM_INPUT_ARG_HT + SCAN_PARAM_INPUT_ARG_BACKOP + ); + + if (num-- > 0) + ss->scan_ch_ms = scan_ch_ms; +#ifdef CONFIG_80211N_HT + if (num-- > 0) + ss->rx_ampdu_accept = rx_ampdu_accept; + if (num-- > 0) + ss->rx_ampdu_size = rx_ampdu_size; +#endif +#ifdef CONFIG_SCAN_BACKOP + if (num-- > 0) + ss->backop_ms = backop_ms; + if (num-- > 0) + ss->scan_cnt_max = scan_cnt_max; +#endif + } + + return count; +} + +int proc_get_scan_abort(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + u32 pass_ms; + + pass_ms = rtw_scan_abort_timeout(adapter, 10000); + + RTW_PRINT_SEL(m, "%u\n", pass_ms); + + return 0; +} + +#ifdef CONFIG_RTW_REPEATER_SON +int proc_get_rson_data(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char rson_data_str[256]; + + rtw_rson_get_property_str(padapter, rson_data_str); + RTW_PRINT_SEL(m, "%s\n", rson_data_str); + return 0; +} + +ssize_t proc_set_rson_data(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + char tmp[64] = {0}; + int num; + u8 field[10], value[64]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + num = sscanf(tmp, "%s %s", field, value); + if (num != 2) { + RTW_INFO("Invalid format : echo > son_data\n"); + return count; + } + RTW_INFO("field=%s value=%s\n", field, value); + num = rtw_rson_set_property(padapter, field, value); + if (num != 1) { + RTW_INFO("Invalid field(%s) or value(%s)\n", field, value); + return count; + } + } + return count; +} +#endif /*CONFIG_RTW_REPEATER_SON*/ + +int proc_get_survey_info(struct seq_file *m, void *v) +{ + _irqL irqL; + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + _queue *queue = &(pmlmepriv->scanned_queue); + struct wlan_network *pnetwork = NULL; + _list *plist, *phead; + s32 notify_signal; + s16 notify_noise = 0; + u16 index = 0, ie_cap = 0; + unsigned char *ie_wpa = NULL, *ie_wpa2 = NULL, *ie_wps = NULL; + unsigned char *ie_p2p = NULL, *ssid = NULL; + char flag_str[64]; + int ielen = 0; + u32 wpsielen = 0; +#ifdef CONFIG_RTW_MESH + const char *ssid_title_str = "ssid/mesh_id"; +#else + const char *ssid_title_str = "ssid"; +#endif + u8 rsni = 255; + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + phead = get_list_head(queue); + if (!phead) + goto _exit; + plist = get_next(phead); + if (!plist) + goto _exit; + +#ifdef CONFIG_RTW_REPEATER_SON + rtw_rson_show_survey_info(m, plist, phead); +#else + + RTW_PRINT_SEL(m, "%5s %-17s %3s %-3s %-4s %-4s %4s %5s %32s %32s \n", + "index", "bssid", "ch", "RSSI", "SdBm", "Noise", "rsni", "age", "flag", ssid_title_str); + while (1) { + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + if (!pnetwork) + break; + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE && + is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network, 0)) { + notify_signal = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);/* dbm */ + } else { + notify_signal = translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength);/* dbm */ + } + +#ifdef CONFIG_BACKGROUND_NOISE_MONITOR + if (IS_NM_ENABLE(padapter)) + notify_noise = rtw_noise_query_by_chan_num(padapter, pnetwork->network.Configuration.DSConfig); +#endif +#ifdef CONFIG_RTW_ACS + rsni = rtw_acs_get_rsni(padapter, (int)(pnetwork->network.Rssi), pnetwork->network.Configuration.DSConfig); +#endif + + ie_wpa = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &ielen, pnetwork->network.IELength - 12); + ie_wpa2 = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &ielen, pnetwork->network.IELength - 12); + ie_cap = rtw_get_capability(&pnetwork->network); + ie_wps = rtw_get_wps_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &wpsielen); +#ifdef CONFIG_P2P + ie_p2p = rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &ielen); +#endif + ssid = pnetwork->network.Ssid.Ssid; + sprintf(flag_str, "%s%s%s%s%s%s%s", + (ie_wpa) ? "[WPA]" : "", + (ie_wpa2) ? "[WPA2]" : "", + (!ie_wpa && !ie_wpa && ie_cap & BIT(4)) ? "[WEP]" : "", + (ie_wps) ? "[WPS]" : "", + (pnetwork->network.InfrastructureMode == Ndis802_11IBSS) ? "[IBSS]" : + (pnetwork->network.InfrastructureMode == Ndis802_11_mesh) ? "[MESH]" : "", + (ie_cap & BIT(0)) ? "[ESS]" : "", + (ie_p2p) ? "[P2P]" : ""); + RTW_PRINT_SEL(m, "%5d "MAC_FMT" %3d %4d %4d %4d %4d %5d %32s %32s\n", + ++index, + MAC_ARG(pnetwork->network.MacAddress), + pnetwork->network.Configuration.DSConfig, + (int)pnetwork->network.Rssi, + notify_signal, + notify_noise, + rsni, + rtw_get_passing_time_ms(pnetwork->last_scanned), + flag_str, + pnetwork->network.InfrastructureMode == Ndis802_11_mesh ? pnetwork->network.mesh_id.Ssid : pnetwork->network.Ssid.Ssid + ); + plist = get_next(plist); + } +#endif +_exit: + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + return 0; +} + +ssize_t proc_set_survey_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + u8 _status = _FALSE; + u8 ssc_chk; + char tmp[32] = {0}; + char cmd[8] = {0}; + bool acs = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%s", cmd); + + if (num < 1) + return count; + + if (strcmp("acs", cmd) == 0) + acs = 1; + } + +#if 1 + ssc_chk = rtw_sitesurvey_condition_check(padapter, _FALSE); + if (ssc_chk != SS_ALLOW) + goto exit; + + rtw_ps_deny(padapter, PS_DENY_SCAN); + if (_FAIL == rtw_pwr_wakeup(padapter)) + goto cancel_ps_deny; + if (!rtw_is_adapter_up(padapter)) { + RTW_INFO("scan abort!! adapter cannot use\n"); + goto cancel_ps_deny; + } +#else +#ifdef CONFIG_MP_INCLUDED + if (rtw_mp_mode_check(padapter)) { + RTW_INFO("MP mode block Scan request\n"); + goto exit; + } +#endif + if (rtw_is_scan_deny(padapter)) { + RTW_INFO(FUNC_ADPT_FMT ": scan deny\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + + rtw_ps_deny(padapter, PS_DENY_SCAN); + if (_FAIL == rtw_pwr_wakeup(padapter)) + goto cancel_ps_deny; + + if (!rtw_is_adapter_up(padapter)) { + RTW_INFO("scan abort!! adapter cannot use\n"); + goto cancel_ps_deny; + } + + if (rtw_mi_busy_traffic_check(padapter)) { + RTW_INFO("scan abort!! BusyTraffic == _TRUE\n"); + goto cancel_ps_deny; + } + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) && check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + RTW_INFO("scan abort!! AP mode process WPS\n"); + goto cancel_ps_deny; + } + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY | WIFI_UNDER_LINKING) == _TRUE) { + RTW_INFO("scan abort!! fwstate=0x%x\n", pmlmepriv->fw_state); + goto cancel_ps_deny; + } + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(padapter, + WIFI_UNDER_SURVEY | WIFI_UNDER_LINKING | WIFI_UNDER_WPS)) { + RTW_INFO("scan abort!! buddy_fwstate check failed\n"); + goto cancel_ps_deny; + } +#endif +#endif + + if (acs) { + #ifdef CONFIG_RTW_ACS + _status = rtw_set_acs_sitesurvey(padapter); + #endif + } else + _status = rtw_set_802_11_bssid_list_scan(padapter, NULL); + +cancel_ps_deny: + rtw_ps_deny_cancel(padapter, PS_DENY_SCAN); +exit: + return count; +} +#ifdef ROKU_PRIVATE +int proc_get_infra_ap(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct sta_info *psta; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct ht_priv_infra_ap *phtpriv = &pmlmepriv->htpriv_infra_ap; +#ifdef CONFIG_80211AC_VHT + struct vht_priv_infra_ap *pvhtpriv = &pmlmepriv->vhtpriv_infra_ap; +#endif + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct sta_priv *pstapriv = &padapter->stapriv; + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress); + if (psta) { + unsigned int i, j; + unsigned int Rx_ss = 0, Tx_ss = 0; + struct recv_reorder_ctrl *preorder_ctrl; + + RTW_PRINT_SEL(m, "SSID=%s\n", pmlmeinfo->network.Ssid.Ssid); + RTW_PRINT_SEL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->cmn.mac_addr)); + RTW_PRINT_SEL(m, "Supported rate="); + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + if (pmlmeinfo->SupportedRates_infra_ap[i] == 0) + break; + RTW_PRINT_SEL(m, " 0x%x", pmlmeinfo->SupportedRates_infra_ap[i]); + } + RTW_PRINT_SEL(m, "\n"); +#ifdef CONFIG_80211N_HT + if (pmlmeinfo->ht_vht_received & BIT(0)) { + RTW_PRINT_SEL(m, "Supported MCS set="); + for (i = 0; i < 16 ; i++) + RTW_PRINT_SEL(m, " 0x%02x", phtpriv->MCS_set_infra_ap[i]); + RTW_PRINT_SEL(m, "\n"); + RTW_PRINT_SEL(m, "highest supported data rate=0x%x\n", phtpriv->rx_highest_data_rate_infra_ap); + RTW_PRINT_SEL(m, "HT_supported_channel_width_set=0x%x\n", phtpriv->channel_width_infra_ap); + RTW_PRINT_SEL(m, "sgi_20m=%d, sgi_40m=%d\n", phtpriv->sgi_20m_infra_ap, phtpriv->sgi_40m_infra_ap); + RTW_PRINT_SEL(m, "ldpc_cap=0x%x, stbc_cap=0x%x\n", phtpriv->ldpc_cap_infra_ap, phtpriv->stbc_cap_infra_ap); + RTW_PRINT_SEL(m, "HT_number_of_stream=%d\n", phtpriv->Rx_ss_infra_ap); + } +#endif + +#ifdef CONFIG_80211AC_VHT + if (pmlmeinfo->ht_vht_received & BIT(1)) { + RTW_PRINT_SEL(m, "VHT_supported_channel_width_set=0x%x\n", pvhtpriv->channel_width_infra_ap); + RTW_PRINT_SEL(m, "vht_ldpc_cap=0x%x, vht_stbc_cap=0x%x, vht_beamform_cap=0x%x\n", pvhtpriv->ldpc_cap_infra_ap, pvhtpriv->stbc_cap_infra_ap, pvhtpriv->beamform_cap_infra_ap); + RTW_PRINT_SEL(m, "Rx_vht_mcs_map=0x%x, Tx_vht_mcs_map=0x%x\n", *(u16 *)pvhtpriv->vht_mcs_map_infra_ap, *(u16 *)pvhtpriv->vht_mcs_map_tx_infra_ap); + RTW_PRINT_SEL(m, "VHT_number_of_stream=%d\n", pvhtpriv->number_of_streams_infra_ap); + } +#endif + } else + RTW_PRINT_SEL(m, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress)); + } else + RTW_PRINT_SEL(m, "this only applies to STA mode\n"); + return 0; +} + +#endif /* ROKU_PRIVATE */ + +static int wireless_mode_to_str(u32 mode, char *str) +{ + str[0]='\0'; + if (mode&WIRELESS_11A) + sprintf(str+strlen(str),"%s","A/"); + if (mode&WIRELESS_11B) + sprintf(str+strlen(str),"%s","B/"); + if (mode&WIRELESS_11G) + sprintf(str+strlen(str),"%s","G/"); + if (mode&(WIRELESS_11_24N|WIRELESS_11_5N)) + sprintf(str+strlen(str),"%s","N/"); + if (mode&WIRELESS_11AC) + sprintf(str+strlen(str),"%s","AC/"); + + if (strlen(str)>1) + str[strlen(str)-1]='\0'; + + return strlen(str); +} + +int proc_get_ap_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + struct sta_info *psta; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_priv *pstapriv = &padapter->stapriv; + char wl_mode[16]; + + /* ap vendor */ + char vendor[VENDOR_NAME_LEN] = {0}; + get_assoc_AP_Vendor(vendor,pmlmeinfo->assoc_AP_vendor); + RTW_PRINT_SEL(m,"AP Vendor %s\n", vendor); + + psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress); + if (psta) { + wireless_mode_to_str(psta->wireless_mode, wl_mode); + RTW_PRINT_SEL(m, "SSID=%s\n", cur_network->network.Ssid.Ssid); + RTW_PRINT_SEL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->cmn.mac_addr)); + RTW_PRINT_SEL(m, "cur_channel=%d, cur_bwmode=%d(%s), cur_ch_offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, ch_width_str(pmlmeext->cur_bwmode), pmlmeext->cur_ch_offset); + RTW_PRINT_SEL(m, "wireless_mode=0x%x(%s), rtsen=%d, cts2slef=%d\n", psta->wireless_mode, wl_mode, psta->rtsen, psta->cts2self); + RTW_PRINT_SEL(m, "state=0x%x, aid=%d, macid=%d, raid=%d\n", + psta->state, psta->cmn.aid, psta->cmn.mac_id, psta->cmn.ra_info.rate_id); +#ifdef CONFIG_80211N_HT + RTW_PRINT_SEL(m, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate); + RTW_PRINT_SEL(m, "bwmode=%d, ch_offset=%d, sgi_20m=%d,sgi_40m=%d\n" + , psta->cmn.bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m); + RTW_PRINT_SEL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable); + RTW_PRINT_SEL(m, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap); + RTW_PRINT_SEL(m, "ldpc_cap=0x%x, stbc_cap=0x%x, beamform_cap=0x%x\n", psta->htpriv.ldpc_cap, psta->htpriv.stbc_cap, psta->htpriv.beamform_cap); +#endif /* CONFIG_80211N_HT */ +#ifdef CONFIG_80211AC_VHT + RTW_PRINT_SEL(m, "vht_en=%d, vht_sgi_80m=%d\n", psta->vhtpriv.vht_option, psta->vhtpriv.sgi_80m); + RTW_PRINT_SEL(m, "vht_ldpc_cap=0x%x, vht_stbc_cap=0x%x, vht_beamform_cap=0x%x\n", psta->vhtpriv.ldpc_cap, psta->vhtpriv.stbc_cap, psta->vhtpriv.beamform_cap); + RTW_PRINT_SEL(m, "vht_mcs_map=0x%x, vht_highest_rate=0x%x, vht_ampdu_len=%d\n", *(u16 *)psta->vhtpriv.vht_mcs_map, psta->vhtpriv.vht_highest_rate, psta->vhtpriv.ampdu_len); +#endif + sta_rx_reorder_ctl_dump(m, psta); + } else + RTW_PRINT_SEL(m, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress)); + + return 0; +} + +ssize_t proc_reset_trx_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct recv_priv *precvpriv = &padapter->recvpriv; + char cmd[32] = {0}; + u8 cnt = 0; + + if (count > sizeof(cmd)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(cmd, buffer, count)) { + int num = sscanf(cmd, "%hhx", &cnt); + + if (num == 1 && cnt == 0) { + precvpriv->dbg_rx_ampdu_drop_count = 0; + precvpriv->dbg_rx_ampdu_forced_indicate_count = 0; + precvpriv->dbg_rx_ampdu_loss_count = 0; + precvpriv->dbg_rx_dup_mgt_frame_drop_count = 0; + precvpriv->dbg_rx_ampdu_window_shift_cnt = 0; + precvpriv->dbg_rx_conflic_mac_addr_cnt = 0; + precvpriv->dbg_rx_drop_count = 0; + } + } + + return count; +} + +int proc_get_trx_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + int i; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct recv_priv *precvpriv = &padapter->recvpriv; + struct hw_xmit *phwxmit; + u16 vo_params[4], vi_params[4], be_params[4], bk_params[4]; + + padapter->hal_func.read_wmmedca_reg(padapter, vo_params, vi_params, be_params, bk_params); + + RTW_PRINT_SEL(m, "wmm_edca_vo, aifs = %u us, cw_min = %u, cw_max = %u, txop_limit = %u us\n", vo_params[0], vo_params[1], vo_params[2], vo_params[3]); + RTW_PRINT_SEL(m, "wmm_edca_vi, aifs = %u us, cw_min = %u, cw_max = %u, txop_limit = %u us\n", vi_params[0], vi_params[1], vi_params[2], vi_params[3]); + RTW_PRINT_SEL(m, "wmm_edca_be, aifs = %u us, cw_min = %u, cw_max = %u, txop_limit = %u us\n", be_params[0], be_params[1], be_params[2], be_params[3]); + RTW_PRINT_SEL(m, "wmm_edca_bk, aifs = %u us, cw_min = %u, cw_max = %u, txop_limit = %u us\n", bk_params[0], bk_params[1], bk_params[2], bk_params[3]); + + dump_os_queue(m, padapter); + + RTW_PRINT_SEL(m, "free_xmitbuf_cnt=%d, free_xmitframe_cnt=%d\n" + , pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt); + RTW_PRINT_SEL(m, "free_ext_xmitbuf_cnt=%d, free_xframe_ext_cnt=%d\n" + , pxmitpriv->free_xmit_extbuf_cnt, pxmitpriv->free_xframe_ext_cnt); + RTW_PRINT_SEL(m, "free_recvframe_cnt=%d\n" + , precvpriv->free_recvframe_cnt); + + for (i = 0; i < pxmitpriv->hwxmit_entry; i++) { + phwxmit = pxmitpriv->hwxmits + i; +#ifdef CONFIG_RTW_MGMT_QUEUE + if (i == pxmitpriv->hwxmit_entry - 1) + RTW_PRINT_SEL(m, "%d, hw_mgmt_q.accnt=%d\n", i, phwxmit->accnt); + else +#endif + RTW_PRINT_SEL(m, "%d, hwq.accnt=%d\n", i, phwxmit->accnt); + } + + rtw_hal_get_hwreg(padapter, HW_VAR_DUMP_MAC_TXFIFO, (u8 *)m); + +#ifdef CONFIG_USB_HCI + RTW_PRINT_SEL(m, "rx_urb_pending_cn=%d\n", ATOMIC_READ(&(precvpriv->rx_pending_cnt))); +#endif + + dump_rx_bh_tk(m, &GET_PRIMARY_ADAPTER(padapter)->recvpriv); + + /* Folowing are RX info */ + RTW_PRINT_SEL(m, "RX: Count of Packets dropped by Driver: %llu\n", (unsigned long long)precvpriv->dbg_rx_drop_count); + /* Counts of packets whose seq_num is less than preorder_ctrl->indicate_seq, Ex delay, retransmission, redundant packets and so on */ + RTW_PRINT_SEL(m, "Rx: Counts of Packets Whose Seq_Num Less Than Reorder Control Seq_Num: %llu\n", (unsigned long long)precvpriv->dbg_rx_ampdu_drop_count); + /* How many times the Rx Reorder Timer is triggered. */ + RTW_PRINT_SEL(m, "Rx: Reorder Time-out Trigger Counts: %llu\n", (unsigned long long)precvpriv->dbg_rx_ampdu_forced_indicate_count); + /* Total counts of packets loss */ + RTW_PRINT_SEL(m, "Rx: Packet Loss Counts: %llu\n", (unsigned long long)precvpriv->dbg_rx_ampdu_loss_count); + RTW_PRINT_SEL(m, "Rx: Duplicate Management Frame Drop Count: %llu\n", (unsigned long long)precvpriv->dbg_rx_dup_mgt_frame_drop_count); + RTW_PRINT_SEL(m, "Rx: AMPDU BA window shift Count: %llu\n", (unsigned long long)precvpriv->dbg_rx_ampdu_window_shift_cnt); + /*The same mac addr counts*/ + RTW_PRINT_SEL(m, "Rx: Conflict MAC Address Frames Count: %llu\n", (unsigned long long)precvpriv->dbg_rx_conflic_mac_addr_cnt); + return 0; +} + +int proc_get_rate_ctl(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + u8 data_rate = 0, sgi = 0, data_fb = 0; + + if (adapter->fix_rate != 0xff) { + data_rate = adapter->fix_rate & 0x7F; + sgi = adapter->fix_rate >> 7; + data_fb = adapter->data_fb ? 1 : 0; + RTW_PRINT_SEL(m, "FIXED %s%s%s\n" + , HDATA_RATE(data_rate) + , data_rate > DESC_RATE54M ? (sgi ? " SGI" : " LGI") : "" + , data_fb ? " FB" : "" + ); + RTW_PRINT_SEL(m, "0x%02x %u\n", adapter->fix_rate, adapter->data_fb); + } else + RTW_PRINT_SEL(m, "RA\n"); + + return 0; +} + +#ifdef CONFIG_PHDYM_FW_FIXRATE +void phydm_fw_fix_rate(void *dm_void, u8 en, u8 macid, u8 bw, u8 rate); +#endif +ssize_t proc_set_rate_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter); + char tmp[32]; + u8 fix_rate = 0xFF; +#ifdef CONFIG_PHDYM_FW_FIXRATE + u8 bw = 0; +#else + u8 data_fb = 0; +#endif + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { +#ifdef CONFIG_PHDYM_FW_FIXRATE + struct dm_struct *dm = adapter_to_phydm(adapter); + u8 en = 1, macid = 255; + _irqL irqL; + _list *plist, *phead; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &(adapter->stapriv); + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 null_addr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint mac_id[NUM_STA]; + int i, macid_rec_idx = 0; + int num = sscanf(tmp, "%hhx %hhu %hhu", &fix_rate, &bw, &macid); + + if (num < 1) { + RTW_INFO("Invalid input!! \"ex: echo > /proc/.../rate_ctl\"\n"); + return count; + } + + if ((fix_rate == 0) || (fix_rate == 0xFF)) + en = 0; + + if (macid != 255) { + RTW_INFO("Call phydm_fw_fix_rate()--en[%d] mac_id[%d] bw[%d] fix_rate[%d]\n", en, macid, bw, fix_rate); + phydm_fw_fix_rate(dm, en, macid, bw, fix_rate); + return count; + } + + /* no specific macid, apply to all macids except bc/mc macid */ + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + if ((_rtw_memcmp(psta->cmn.mac_addr, bc_addr, ETH_ALEN) != _TRUE) + && (_rtw_memcmp(psta->cmn.mac_addr, null_addr, ETH_ALEN) != _TRUE) + && (_rtw_memcmp(psta->cmn.mac_addr, adapter_mac_addr(adapter), ETH_ALEN) != _TRUE)) { + mac_id[macid_rec_idx] = psta->cmn.mac_id; + macid_rec_idx++; + } + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (i = 0; i < macid_rec_idx; i++) { + RTW_INFO("Call phydm_fw_fix_rate()--en[%d] mac_id[%d] bw[%d] fix_rate[%d]\n", en, mac_id[i], bw, fix_rate); + phydm_fw_fix_rate(dm, en, mac_id[i], bw, fix_rate); + } +#else + int num = sscanf(tmp, "%hhx %hhu", &fix_rate, &data_fb); + + if (num >= 1) { + u8 fix_rate_ori = adapter->fix_rate; + + adapter->fix_rate = fix_rate; + if (fix_rate == 0xFF) + hal_data->ForcedDataRate = 0; + else + hal_data->ForcedDataRate = hw_rate_to_m_rate(fix_rate & 0x7F); + + if (adapter->fix_bw != 0xFF && fix_rate_ori != fix_rate) + rtw_run_in_thread_cmd(adapter, ((void *)(rtw_update_tx_rate_bmp)), adapter_to_dvobj(adapter)); + } + if (num >= 2) + adapter->data_fb = data_fb ? 1 : 0; +#endif + } + + return count; +} + +#ifdef CONFIG_AP_MODE +int proc_get_bmc_tx_rate(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + if (!MLME_IS_AP(adapter) && !MLME_IS_MESH(adapter)) { + RTW_PRINT_SEL(m, "[ERROR] Not in SoftAP/Mesh mode !!\n"); + return 0; + } + + RTW_PRINT_SEL(m, " BMC Tx rate - %s\n", MGN_RATE_STR(adapter->bmc_tx_rate)); + return 0; +} + +ssize_t proc_set_bmc_tx_rate(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 bmc_tx_rate; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx", &bmc_tx_rate); + + if (num >= 1) + /*adapter->bmc_tx_rate = hw_rate_to_m_rate(bmc_tx_rate);*/ + adapter->bmc_tx_rate = bmc_tx_rate; + } + + return count; +} +#endif /*CONFIG_AP_MODE*/ + + +int proc_get_tx_power_offset(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "Tx power offset - %u\n", adapter->power_offset); + return 0; +} + +ssize_t proc_set_tx_power_offset(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 power_offset = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhu", &power_offset); + + if (num >= 1) { + if (power_offset > 5) + power_offset = 0; + + adapter->power_offset = power_offset; + } + } + + return count; +} + +int proc_get_bw_ctl(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + u8 data_bw = 0; + + if (adapter->fix_bw != 0xff) { + data_bw = adapter->fix_bw; + RTW_PRINT_SEL(m, "FIXED %s\n", ch_width_str(data_bw)); + } else + RTW_PRINT_SEL(m, "Auto\n"); + + return 0; +} + +ssize_t proc_set_bw_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 fix_bw; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%hhu", &fix_bw); + + if (num >= 1) { + u8 fix_bw_ori = adapter->fix_bw; + + adapter->fix_bw = fix_bw; + + if (adapter->fix_rate != 0xFF && fix_bw_ori != fix_bw) + rtw_run_in_thread_cmd(adapter, ((void *)(rtw_update_tx_rate_bmp)), adapter_to_dvobj(adapter)); + } + } + + return count; +} + +#ifdef DBG_RX_COUNTER_DUMP +int proc_get_rx_cnt_dump(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + int i; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "BIT0- Dump RX counters of DRV\n"); + RTW_PRINT_SEL(m, "BIT1- Dump RX counters of MAC\n"); + RTW_PRINT_SEL(m, "BIT2- Dump RX counters of PHY\n"); + RTW_PRINT_SEL(m, "BIT3- Dump TRX data frame of DRV\n"); + RTW_PRINT_SEL(m, "dump_rx_cnt_mode = 0x%02x\n", adapter->dump_rx_cnt_mode); + + return 0; +} +ssize_t proc_set_rx_cnt_dump(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 dump_rx_cnt_mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx", &dump_rx_cnt_mode); + + if (num == 1) { + rtw_dump_phy_rxcnts_preprocess(adapter, dump_rx_cnt_mode); + adapter->dump_rx_cnt_mode = dump_rx_cnt_mode; + } + } + + return count; +} +#endif + +ssize_t proc_set_fwdl_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) + sscanf(tmp, "%hhu %hhu", &fwdl_test_chksum_fail, &fwdl_test_wintint_rdy_fail); + + return count; +} + +ssize_t proc_set_del_rx_ampdu_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) + sscanf(tmp, "%hhu", &del_rx_ampdu_test_no_tx_fail); + + return count; +} + +ssize_t proc_set_wait_hiq_empty(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) + sscanf(tmp, "%u", &g_wait_hiq_empty_ms); + + return count; +} + +ssize_t proc_set_sta_linking_test(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + u32 wait_ms = 0; + u8 force_fail = 0; + int num = sscanf(tmp, "%u %hhu", &wait_ms, &force_fail); + + if (num >= 1) + sta_linking_test_wait_ms = wait_ms; + if (num >= 2) + sta_linking_test_force_fail = force_fail; + } + + return count; +} + +#ifdef CONFIG_AP_MODE +ssize_t proc_set_ap_linking_test(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + u16 force_auth_fail = 0; + u16 force_asoc_fail = 0; + int num = sscanf(tmp, "%hu %hu", &force_auth_fail, &force_asoc_fail); + + if (num >= 1) + ap_linking_test_force_auth_fail = force_auth_fail; + if (num >= 2) + ap_linking_test_force_asoc_fail = force_asoc_fail; + } + + return count; +} +#endif /* CONFIG_AP_MODE */ + +int proc_get_ps_dbg_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = padapter->dvobj; + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; + + RTW_PRINT_SEL(m, "dbg_sdio_alloc_irq_cnt=%d\n", pdbgpriv->dbg_sdio_alloc_irq_cnt); + RTW_PRINT_SEL(m, "dbg_sdio_free_irq_cnt=%d\n", pdbgpriv->dbg_sdio_free_irq_cnt); + RTW_PRINT_SEL(m, "dbg_sdio_alloc_irq_error_cnt=%d\n", pdbgpriv->dbg_sdio_alloc_irq_error_cnt); + RTW_PRINT_SEL(m, "dbg_sdio_free_irq_error_cnt=%d\n", pdbgpriv->dbg_sdio_free_irq_error_cnt); + RTW_PRINT_SEL(m, "dbg_sdio_init_error_cnt=%d\n", pdbgpriv->dbg_sdio_init_error_cnt); + RTW_PRINT_SEL(m, "dbg_sdio_deinit_error_cnt=%d\n", pdbgpriv->dbg_sdio_deinit_error_cnt); + RTW_PRINT_SEL(m, "dbg_suspend_error_cnt=%d\n", pdbgpriv->dbg_suspend_error_cnt); + RTW_PRINT_SEL(m, "dbg_suspend_cnt=%d\n", pdbgpriv->dbg_suspend_cnt); + RTW_PRINT_SEL(m, "dbg_resume_cnt=%d\n", pdbgpriv->dbg_resume_cnt); + RTW_PRINT_SEL(m, "dbg_resume_error_cnt=%d\n", pdbgpriv->dbg_resume_error_cnt); + RTW_PRINT_SEL(m, "dbg_deinit_fail_cnt=%d\n", pdbgpriv->dbg_deinit_fail_cnt); + RTW_PRINT_SEL(m, "dbg_carddisable_cnt=%d\n", pdbgpriv->dbg_carddisable_cnt); + RTW_PRINT_SEL(m, "dbg_ps_insuspend_cnt=%d\n", pdbgpriv->dbg_ps_insuspend_cnt); + RTW_PRINT_SEL(m, "dbg_dev_unload_inIPS_cnt=%d\n", pdbgpriv->dbg_dev_unload_inIPS_cnt); + RTW_PRINT_SEL(m, "dbg_scan_pwr_state_cnt=%d\n", pdbgpriv->dbg_scan_pwr_state_cnt); + RTW_PRINT_SEL(m, "dbg_downloadfw_pwr_state_cnt=%d\n", pdbgpriv->dbg_downloadfw_pwr_state_cnt); + RTW_PRINT_SEL(m, "dbg_carddisable_error_cnt=%d\n", pdbgpriv->dbg_carddisable_error_cnt); + RTW_PRINT_SEL(m, "dbg_fw_read_ps_state_fail_cnt=%d\n", pdbgpriv->dbg_fw_read_ps_state_fail_cnt); + RTW_PRINT_SEL(m, "dbg_leave_ips_fail_cnt=%d\n", pdbgpriv->dbg_leave_ips_fail_cnt); + RTW_PRINT_SEL(m, "dbg_leave_lps_fail_cnt=%d\n", pdbgpriv->dbg_leave_lps_fail_cnt); + RTW_PRINT_SEL(m, "dbg_h2c_leave32k_fail_cnt=%d\n", pdbgpriv->dbg_h2c_leave32k_fail_cnt); + RTW_PRINT_SEL(m, "dbg_diswow_dload_fw_fail_cnt=%d\n", pdbgpriv->dbg_diswow_dload_fw_fail_cnt); + RTW_PRINT_SEL(m, "dbg_enwow_dload_fw_fail_cnt=%d\n", pdbgpriv->dbg_enwow_dload_fw_fail_cnt); + RTW_PRINT_SEL(m, "dbg_ips_drvopen_fail_cnt=%d\n", pdbgpriv->dbg_ips_drvopen_fail_cnt); + RTW_PRINT_SEL(m, "dbg_poll_fail_cnt=%d\n", pdbgpriv->dbg_poll_fail_cnt); + RTW_PRINT_SEL(m, "dbg_rpwm_toogle_cnt=%d\n", pdbgpriv->dbg_rpwm_toogle_cnt); + RTW_PRINT_SEL(m, "dbg_rpwm_timeout_fail_cnt=%d\n", pdbgpriv->dbg_rpwm_timeout_fail_cnt); + RTW_PRINT_SEL(m, "dbg_sreset_cnt=%d\n", pdbgpriv->dbg_sreset_cnt); + RTW_PRINT_SEL(m, "dbg_fw_mem_dl_error_cnt=%d\n", pdbgpriv->dbg_fw_mem_dl_error_cnt); + + return 0; +} +ssize_t proc_set_ps_dbg_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter->dvobj; + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; + char tmp[32]; + u8 ps_dbg_cmd_id; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx", &ps_dbg_cmd_id); + + if (num == 1 && ps_dbg_cmd_id == 1) /*Clean all*/ + _rtw_memset(pdbgpriv, 0, sizeof(struct debug_priv)); + + } + + return count; +} + + +#ifdef CONFIG_DBG_COUNTER + +int proc_get_rx_logs(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct rx_logs *rx_logs = &padapter->rx_logs; + + RTW_PRINT_SEL(m, + "intf_rx=%d\n" + "intf_rx_err_recvframe=%d\n" + "intf_rx_err_skb=%d\n" + "intf_rx_report=%d\n" + "core_rx=%d\n" + "core_rx_pre=%d\n" + "core_rx_pre_ver_err=%d\n" + "core_rx_pre_mgmt=%d\n" + "core_rx_pre_mgmt_err_80211w=%d\n" + "core_rx_pre_mgmt_err=%d\n" + "core_rx_pre_ctrl=%d\n" + "core_rx_pre_ctrl_err=%d\n" + "core_rx_pre_data=%d\n" + "core_rx_pre_data_wapi_seq_err=%d\n" + "core_rx_pre_data_wapi_key_err=%d\n" + "core_rx_pre_data_handled=%d\n" + "core_rx_pre_data_err=%d\n" + "core_rx_pre_data_unknown=%d\n" + "core_rx_pre_unknown=%d\n" + "core_rx_enqueue=%d\n" + "core_rx_dequeue=%d\n" + "core_rx_post=%d\n" + "core_rx_post_decrypt=%d\n" + "core_rx_post_decrypt_wep=%d\n" + "core_rx_post_decrypt_tkip=%d\n" + "core_rx_post_decrypt_aes=%d\n" + "core_rx_post_decrypt_wapi=%d\n" + "core_rx_post_decrypt_hw=%d\n" + "core_rx_post_decrypt_unknown=%d\n" + "core_rx_post_decrypt_err=%d\n" + "core_rx_post_defrag_err=%d\n" + "core_rx_post_portctrl_err=%d\n" + "core_rx_post_indicate=%d\n" + "core_rx_post_indicate_in_oder=%d\n" + "core_rx_post_indicate_reoder=%d\n" + "core_rx_post_indicate_err=%d\n" + "os_indicate=%d\n" + "os_indicate_ap_mcast=%d\n" + "os_indicate_ap_forward=%d\n" + "os_indicate_ap_self=%d\n" + "os_indicate_err=%d\n" + "os_netif_ok=%d\n" + "os_netif_err=%d\n", + rx_logs->intf_rx, + rx_logs->intf_rx_err_recvframe, + rx_logs->intf_rx_err_skb, + rx_logs->intf_rx_report, + rx_logs->core_rx, + rx_logs->core_rx_pre, + rx_logs->core_rx_pre_ver_err, + rx_logs->core_rx_pre_mgmt, + rx_logs->core_rx_pre_mgmt_err_80211w, + rx_logs->core_rx_pre_mgmt_err, + rx_logs->core_rx_pre_ctrl, + rx_logs->core_rx_pre_ctrl_err, + rx_logs->core_rx_pre_data, + rx_logs->core_rx_pre_data_wapi_seq_err, + rx_logs->core_rx_pre_data_wapi_key_err, + rx_logs->core_rx_pre_data_handled, + rx_logs->core_rx_pre_data_err, + rx_logs->core_rx_pre_data_unknown, + rx_logs->core_rx_pre_unknown, + rx_logs->core_rx_enqueue, + rx_logs->core_rx_dequeue, + rx_logs->core_rx_post, + rx_logs->core_rx_post_decrypt, + rx_logs->core_rx_post_decrypt_wep, + rx_logs->core_rx_post_decrypt_tkip, + rx_logs->core_rx_post_decrypt_aes, + rx_logs->core_rx_post_decrypt_wapi, + rx_logs->core_rx_post_decrypt_hw, + rx_logs->core_rx_post_decrypt_unknown, + rx_logs->core_rx_post_decrypt_err, + rx_logs->core_rx_post_defrag_err, + rx_logs->core_rx_post_portctrl_err, + rx_logs->core_rx_post_indicate, + rx_logs->core_rx_post_indicate_in_oder, + rx_logs->core_rx_post_indicate_reoder, + rx_logs->core_rx_post_indicate_err, + rx_logs->os_indicate, + rx_logs->os_indicate_ap_mcast, + rx_logs->os_indicate_ap_forward, + rx_logs->os_indicate_ap_self, + rx_logs->os_indicate_err, + rx_logs->os_netif_ok, + rx_logs->os_netif_err + ); + + return 0; +} + +int proc_get_tx_logs(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct tx_logs *tx_logs = &padapter->tx_logs; + + RTW_PRINT_SEL(m, + "os_tx=%d\n" + "os_tx_err_up=%d\n" + "os_tx_err_xmit=%d\n" + "os_tx_m2u=%d\n" + "os_tx_m2u_ignore_fw_linked=%d\n" + "os_tx_m2u_ignore_self=%d\n" + "os_tx_m2u_entry=%d\n" + "os_tx_m2u_entry_err_xmit=%d\n" + "os_tx_m2u_entry_err_skb=%d\n" + "os_tx_m2u_stop=%d\n" + "core_tx=%d\n" + "core_tx_err_pxmitframe=%d\n" + "core_tx_err_brtx=%d\n" + "core_tx_upd_attrib=%d\n" + "core_tx_upd_attrib_adhoc=%d\n" + "core_tx_upd_attrib_sta=%d\n" + "core_tx_upd_attrib_ap=%d\n" + "core_tx_upd_attrib_unknown=%d\n" + "core_tx_upd_attrib_dhcp=%d\n" + "core_tx_upd_attrib_icmp=%d\n" + "core_tx_upd_attrib_active=%d\n" + "core_tx_upd_attrib_err_ucast_sta=%d\n" + "core_tx_upd_attrib_err_ucast_ap_link=%d\n" + "core_tx_upd_attrib_err_sta=%d\n" + "core_tx_upd_attrib_err_link=%d\n" + "core_tx_upd_attrib_err_sec=%d\n" + "core_tx_ap_enqueue_warn_fwstate=%d\n" + "core_tx_ap_enqueue_warn_sta=%d\n" + "core_tx_ap_enqueue_warn_nosta=%d\n" + "core_tx_ap_enqueue_warn_link=%d\n" + "core_tx_ap_enqueue_warn_trigger=%d\n" + "core_tx_ap_enqueue_mcast=%d\n" + "core_tx_ap_enqueue_ucast=%d\n" + "core_tx_ap_enqueue=%d\n" + "intf_tx=%d\n" + "intf_tx_pending_ac=%d\n" + "intf_tx_pending_fw_under_survey=%d\n" + "intf_tx_pending_fw_under_linking=%d\n" + "intf_tx_pending_xmitbuf=%d\n" + "intf_tx_enqueue=%d\n" + "core_tx_enqueue=%d\n" + "core_tx_enqueue_class=%d\n" + "core_tx_enqueue_class_err_sta=%d\n" + "core_tx_enqueue_class_err_nosta=%d\n" + "core_tx_enqueue_class_err_fwlink=%d\n" + "intf_tx_direct=%d\n" + "intf_tx_direct_err_coalesce=%d\n" + "intf_tx_dequeue=%d\n" + "intf_tx_dequeue_err_coalesce=%d\n" + "intf_tx_dump_xframe=%d\n" + "intf_tx_dump_xframe_err_txdesc=%d\n" + "intf_tx_dump_xframe_err_port=%d\n", + tx_logs->os_tx, + tx_logs->os_tx_err_up, + tx_logs->os_tx_err_xmit, + tx_logs->os_tx_m2u, + tx_logs->os_tx_m2u_ignore_fw_linked, + tx_logs->os_tx_m2u_ignore_self, + tx_logs->os_tx_m2u_entry, + tx_logs->os_tx_m2u_entry_err_xmit, + tx_logs->os_tx_m2u_entry_err_skb, + tx_logs->os_tx_m2u_stop, + tx_logs->core_tx, + tx_logs->core_tx_err_pxmitframe, + tx_logs->core_tx_err_brtx, + tx_logs->core_tx_upd_attrib, + tx_logs->core_tx_upd_attrib_adhoc, + tx_logs->core_tx_upd_attrib_sta, + tx_logs->core_tx_upd_attrib_ap, + tx_logs->core_tx_upd_attrib_unknown, + tx_logs->core_tx_upd_attrib_dhcp, + tx_logs->core_tx_upd_attrib_icmp, + tx_logs->core_tx_upd_attrib_active, + tx_logs->core_tx_upd_attrib_err_ucast_sta, + tx_logs->core_tx_upd_attrib_err_ucast_ap_link, + tx_logs->core_tx_upd_attrib_err_sta, + tx_logs->core_tx_upd_attrib_err_link, + tx_logs->core_tx_upd_attrib_err_sec, + tx_logs->core_tx_ap_enqueue_warn_fwstate, + tx_logs->core_tx_ap_enqueue_warn_sta, + tx_logs->core_tx_ap_enqueue_warn_nosta, + tx_logs->core_tx_ap_enqueue_warn_link, + tx_logs->core_tx_ap_enqueue_warn_trigger, + tx_logs->core_tx_ap_enqueue_mcast, + tx_logs->core_tx_ap_enqueue_ucast, + tx_logs->core_tx_ap_enqueue, + tx_logs->intf_tx, + tx_logs->intf_tx_pending_ac, + tx_logs->intf_tx_pending_fw_under_survey, + tx_logs->intf_tx_pending_fw_under_linking, + tx_logs->intf_tx_pending_xmitbuf, + tx_logs->intf_tx_enqueue, + tx_logs->core_tx_enqueue, + tx_logs->core_tx_enqueue_class, + tx_logs->core_tx_enqueue_class_err_sta, + tx_logs->core_tx_enqueue_class_err_nosta, + tx_logs->core_tx_enqueue_class_err_fwlink, + tx_logs->intf_tx_direct, + tx_logs->intf_tx_direct_err_coalesce, + tx_logs->intf_tx_dequeue, + tx_logs->intf_tx_dequeue_err_coalesce, + tx_logs->intf_tx_dump_xframe, + tx_logs->intf_tx_dump_xframe_err_txdesc, + tx_logs->intf_tx_dump_xframe_err_port + ); + + return 0; +} + +int proc_get_int_logs(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, + "all=%d\n" + "err=%d\n" + "tbdok=%d\n" + "tbder=%d\n" + "bcnderr=%d\n" + "bcndma=%d\n" + "bcndma_e=%d\n" + "rx=%d\n" + "rx_rdu=%d\n" + "rx_fovw=%d\n" + "txfovw=%d\n" + "mgntok=%d\n" + "highdok=%d\n" + "bkdok=%d\n" + "bedok=%d\n" + "vidok=%d\n" + "vodok=%d\n", + padapter->int_logs.all, + padapter->int_logs.err, + padapter->int_logs.tbdok, + padapter->int_logs.tbder, + padapter->int_logs.bcnderr, + padapter->int_logs.bcndma, + padapter->int_logs.bcndma_e, + padapter->int_logs.rx, + padapter->int_logs.rx_rdu, + padapter->int_logs.rx_fovw, + padapter->int_logs.txfovw, + padapter->int_logs.mgntok, + padapter->int_logs.highdok, + padapter->int_logs.bkdok, + padapter->int_logs.bedok, + padapter->int_logs.vidok, + padapter->int_logs.vodok + ); + + return 0; +} + +#endif /* CONFIG_DBG_COUNTER */ + +int proc_get_hw_status(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = padapter->dvobj; + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; + struct registry_priv *regsty = dvobj_to_regsty(dvobj); + + if (regsty->check_hw_status == 0) + RTW_PRINT_SEL(m, "RX FIFO full count: not check in watch dog\n"); + else if (pdbgpriv->dbg_rx_fifo_last_overflow == 1 + && pdbgpriv->dbg_rx_fifo_curr_overflow == 1 + && pdbgpriv->dbg_rx_fifo_diff_overflow == 1 + ) + RTW_PRINT_SEL(m, "RX FIFO full count: no implementation\n"); + else { + RTW_PRINT_SEL(m, "RX FIFO full count: last_time=%llu, current_time=%llu, differential=%llu\n" + , pdbgpriv->dbg_rx_fifo_last_overflow, pdbgpriv->dbg_rx_fifo_curr_overflow, pdbgpriv->dbg_rx_fifo_diff_overflow); + } + + return 0; +} + +ssize_t proc_set_hw_status(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = padapter->dvobj; + struct registry_priv *regsty = dvobj_to_regsty(dvobj); + char tmp[32]; + u32 enable; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &enable); + + if (num == 1 && regsty && enable <= 1) { + regsty->check_hw_status = enable; + RTW_INFO("check_hw_status=%d\n", regsty->check_hw_status); + } + } + + return count; +} + +#ifdef CONFIG_HUAWEI_PROC +int proc_get_huawei_trx_info(struct seq_file *sel, void *v) +{ + struct net_device *dev = sel->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dm_struct *dm = adapter_to_phydm(padapter); + struct sta_info *psta; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct ra_sta_info *ra_info; + u8 curr_tx_sgi = _FALSE; + u8 curr_tx_rate = 0; + u8 mac_id; +#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA + u8 isCCKrate, rf_path; + struct hal_spec_t *hal_spec = GET_HAL_SPEC(padapter); + struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info; +#endif + + if (!dm->is_linked) { + RTW_PRINT_SEL(sel, "NO link\n\n"); + return 0; + } + + /*============ tx info ============ */ + for (mac_id = 0; mac_id < macid_ctl->num; mac_id++) { + if (rtw_macid_is_used(macid_ctl, mac_id) && !rtw_macid_is_bmc(macid_ctl, mac_id)) { + psta = macid_ctl->sta[mac_id]; + if (!psta) + continue; + + RTW_PRINT_SEL(sel, "STA [" MAC_FMT "]\n", MAC_ARG(psta->cmn.mac_addr)); + + ra_info = &psta->cmn.ra_info; + curr_tx_sgi = rtw_get_current_tx_sgi(padapter, psta); + curr_tx_rate = rtw_get_current_tx_rate(padapter, psta); + RTW_PRINT_SEL(sel, "curr_tx_rate : %s (%s)\n", + HDATA_RATE(curr_tx_rate), (curr_tx_sgi) ? "S" : "L"); + RTW_PRINT_SEL(sel, "curr_tx_bw : %s\n", ch_width_str(ra_info->curr_tx_bw)); + } + } + + /*============ rx info ============ */ + RTW_PRINT_SEL(sel, "rx_rate : %s\n", HDATA_RATE(dm->rx_rate)); +#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA + isCCKrate = (psample_pkt_rssi->data_rate <= DESC_RATE11M) ? TRUE : FALSE; + + for (rf_path = 0; rf_path < hal_spec->rf_reg_path_num; rf_path++) { + if (!(GET_HAL_RX_PATH_BMP(padapter) & BIT(rf_path))) + continue; + if (!isCCKrate) + _RTW_PRINT_SEL(sel , "RF_PATH_%d : rx_ofdm_pwr:%d(dBm), rx_ofdm_snr:%d(dB)\n", + rf_path, psample_pkt_rssi->ofdm_pwr[rf_path], psample_pkt_rssi->ofdm_snr[rf_path]); + } +#endif + RTW_PRINT_SEL(sel, "\n"); + return 0; +} +#endif /* CONFIG_HUAWEI_PROC */ + +int proc_get_trx_info_debug(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + /*============ tx info ============ */ + rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, m); + + /*============ rx info ============ */ + rtw_hal_set_odm_var(padapter, HAL_ODM_RX_INFO_DUMP, m, _FALSE); + + return 0; +} + +int proc_get_rx_signal(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "rssi:%d\n", padapter->recvpriv.rssi); +#ifdef CONFIG_MP_INCLUDED + if (padapter->registrypriv.mp_mode == 1) { + struct dm_struct *odm = adapter_to_phydm(padapter); + if (padapter->mppriv.antenna_rx == ANTENNA_A) + RTW_PRINT_SEL(m, "Antenna: A\n"); + else if (padapter->mppriv.antenna_rx == ANTENNA_B) + RTW_PRINT_SEL(m, "Antenna: B\n"); + else if (padapter->mppriv.antenna_rx == ANTENNA_C) + RTW_PRINT_SEL(m, "Antenna: C\n"); + else if (padapter->mppriv.antenna_rx == ANTENNA_D) + RTW_PRINT_SEL(m, "Antenna: D\n"); + else if (padapter->mppriv.antenna_rx == ANTENNA_AB) + RTW_PRINT_SEL(m, "Antenna: AB\n"); + else if (padapter->mppriv.antenna_rx == ANTENNA_BC) + RTW_PRINT_SEL(m, "Antenna: BC\n"); + else if (padapter->mppriv.antenna_rx == ANTENNA_CD) + RTW_PRINT_SEL(m, "Antenna: CD\n"); + else + RTW_PRINT_SEL(m, "Antenna: __\n"); + + RTW_PRINT_SEL(m, "rx_rate = %s\n", HDATA_RATE(odm->rx_rate)); + return 0; + } else +#endif + { + /* RTW_PRINT_SEL(m, "rxpwdb:%d\n", padapter->recvpriv.rxpwdb); */ + RTW_PRINT_SEL(m, "signal_strength:%u\n", padapter->recvpriv.signal_strength); + RTW_PRINT_SEL(m, "signal_qual:%u\n", padapter->recvpriv.signal_qual); + } +#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA + rtw_odm_get_perpkt_rssi(m, padapter); + rtw_get_raw_rssi_info(m, padapter); +#endif + return 0; +} + +ssize_t proc_set_rx_signal(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 is_signal_dbg, signal_strength; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength); + + if (num < 1) + return count; + + is_signal_dbg = is_signal_dbg == 0 ? 0 : 1; + + if (is_signal_dbg && num < 2) + return count; + + signal_strength = signal_strength > 100 ? 100 : signal_strength; + + padapter->recvpriv.is_signal_dbg = is_signal_dbg; + padapter->recvpriv.signal_strength_dbg = signal_strength; + + if (is_signal_dbg) + RTW_INFO("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength); + else + RTW_INFO("set %s\n", "HW_SIGNAL_STRENGTH"); + + } + + return count; + +} + +int proc_get_mac_rptbuf(struct seq_file *m, void *v) +{ +#ifdef CONFIG_RTL8814A + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + u16 i; + u16 mac_id; + u32 shcut_addr = 0; + u32 read_addr = 0; + + RTW_PRINT_SEL(m, "TX ShortCut:\n"); + for (mac_id = 0; mac_id < 64; mac_id++) { + rtw_write16(padapter, 0x140, 0x662 | ((mac_id & BIT5) >> 5)); + shcut_addr = 0x8000; + shcut_addr = shcut_addr | ((mac_id & 0x1f) << 7); + RTW_PRINT_SEL(m, "mac_id=%d, 0x140=%x =>\n", mac_id, 0x662 | ((mac_id & BIT5) >> 5)); + for (i = 0; i < 30; i++) { + read_addr = 0; + read_addr = shcut_addr | (i << 2); + RTW_PRINT_SEL(m, "i=%02d: MAC_%04x= %08x ", i, read_addr, rtw_read32(padapter, read_addr)); + if (!((i + 1) % 4)) + RTW_PRINT_SEL(m, "\n"); + if (i == 29) + RTW_PRINT_SEL(m, "\n"); + } + } +#endif /* CONFIG_RTL8814A */ + return 0; +} + +#ifdef CONFIG_80211N_HT + +int proc_get_ht_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "%d\n", pregpriv->ht_enable); + + return 0; +} + +ssize_t proc_set_ht_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if ( num == 1 && pregpriv && mode < 2) { + pregpriv->ht_enable = mode; + RTW_INFO("ht_enable=%d\n", pregpriv->ht_enable); + } + } + + return count; + +} + +int proc_get_bw_mode(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->bw_mode); + + return 0; +} + +ssize_t proc_set_bw_mode(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + u8 bw_2g; + u8 bw_5g; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%x ", &mode); + bw_5g = mode >> 4; + bw_2g = mode & 0x0f; + + if (num == 1 && pregpriv && bw_2g <= 4 && bw_5g <= 4) { + pregpriv->bw_mode = mode; + printk("bw_mode=0x%x\n", mode); + } + } + + return count; + +} + +int proc_get_ampdu_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "%d\n", pregpriv->ampdu_enable); + + return 0; +} + +ssize_t proc_set_ampdu_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if (num == 1 && pregpriv && mode < 2) { + pregpriv->ampdu_enable = mode; + printk("ampdu_enable=%d\n", mode); + } + + } + + return count; + +} + + +void dump_regsty_rx_ampdu_size_limit(void *sel, _adapter *adapter) +{ + struct registry_priv *regsty = adapter_to_regsty(adapter); + int i; + + RTW_PRINT_SEL(sel, "%-3s %-3s %-3s %-3s %-4s\n" + , "", "20M", "40M", "80M", "160M"); + for (i = 0; i < 4; i++) + RTW_PRINT_SEL(sel, "%dSS %3u %3u %3u %4u\n", i + 1 + , regsty->rx_ampdu_sz_limit_by_nss_bw[i][0] + , regsty->rx_ampdu_sz_limit_by_nss_bw[i][1] + , regsty->rx_ampdu_sz_limit_by_nss_bw[i][2] + , regsty->rx_ampdu_sz_limit_by_nss_bw[i][3]); +} + +int proc_get_rx_ampdu(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + _RTW_PRINT_SEL(m, "accept: "); + if (padapter->fix_rx_ampdu_accept == RX_AMPDU_ACCEPT_INVALID) + RTW_PRINT_SEL(m, "%u%s\n", rtw_rx_ampdu_is_accept(padapter), "(auto)"); + else + RTW_PRINT_SEL(m, "%u%s\n", padapter->fix_rx_ampdu_accept, "(fixed)"); + + _RTW_PRINT_SEL(m, "size: "); + if (padapter->fix_rx_ampdu_size == RX_AMPDU_SIZE_INVALID) { + RTW_PRINT_SEL(m, "%u%s\n", rtw_rx_ampdu_size(padapter), "(auto) with conditional limit:"); + dump_regsty_rx_ampdu_size_limit(m, padapter); + } else + RTW_PRINT_SEL(m, "%u%s\n", padapter->fix_rx_ampdu_size, "(fixed)"); + RTW_PRINT_SEL(m, "\n"); + + RTW_PRINT_SEL(m, "%19s %17s\n", "fix_rx_ampdu_accept", "fix_rx_ampdu_size"); + + _RTW_PRINT_SEL(m, "%-19d %-17u\n" + , padapter->fix_rx_ampdu_accept + , padapter->fix_rx_ampdu_size); + + return 0; +} + +ssize_t proc_set_rx_ampdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 accept; + u8 size; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhu %hhu", &accept, &size); + + if (num >= 1) + rtw_rx_ampdu_set_accept(padapter, accept, RX_AMPDU_DRV_FIXED); + if (num >= 2) + rtw_rx_ampdu_set_size(padapter, size, RX_AMPDU_DRV_FIXED); + + rtw_rx_ampdu_apply(padapter); + } + + return count; +} +#ifdef CONFIG_SDIO_TX_ENABLE_AVAL_INT +int proc_get_tx_aval_th(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + if (padapter) { + + switch(dvobj->tx_aval_int_thr_mode) { + case 0: + RTW_PRINT_SEL(m, "tx_aval_int_thr_mode = %u (auto) \n", dvobj->tx_aval_int_thr_mode); + break; + case 1: + RTW_PRINT_SEL(m, "tx_aval_int_thr_mode = %u (fixed)\n", dvobj->tx_aval_int_thr_mode); + RTW_PRINT_SEL(m, "tx_aval_threshold = 0x%x\n", dvobj->tx_aval_int_thr_value); + break; + case 2: + RTW_PRINT_SEL(m, "tx_aval_int_thr_mode = %u(by sdio_tx_max_len)\n", dvobj->tx_aval_int_thr_mode); + break; + default: + break; + } + } + return 0; +} + +ssize_t proc_set_tx_aval_th(struct file *file, const char __user *buffer + , size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + char tmp[32]; + u32 mode; + u32 threshold; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d %d ",&mode, &threshold); + + if(num >= 1) + dvobj->tx_aval_int_thr_mode = mode; + if(num >= 2) + dvobj->tx_aval_int_thr_value = threshold; + RTW_INFO("dvobj->tx_aval_int_thr_mode= 0x%x\n", mode); + RTW_INFO("dvobj->tx_aval_int_thr_value= 0x%x(range need 1~255)\n", threshold); + } + + return count; +} +#endif /*CONFIG_SDIO_TX_ENABLE_AVAL_INT*/ + +int proc_get_rx_ampdu_factor(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + + if (padapter) + RTW_PRINT_SEL(m, "rx ampdu factor = %x\n", padapter->driver_rx_ampdu_factor); + + return 0; +} + +ssize_t proc_set_rx_ampdu_factor(struct file *file, const char __user *buffer + , size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 factor; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &factor); + + if (padapter && (num == 1)) { + RTW_INFO("padapter->driver_rx_ampdu_factor = %x\n", factor); + + if (factor > 0x03) + padapter->driver_rx_ampdu_factor = 0xFF; + else + padapter->driver_rx_ampdu_factor = factor; + } + } + + return count; +} + +int proc_get_tx_max_agg_num(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + + if (padapter) + RTW_PRINT_SEL(m, "tx max AMPDU num = 0x%02x\n", padapter->driver_tx_max_agg_num); + + return 0; +} + +ssize_t proc_set_tx_max_agg_num(struct file *file, const char __user *buffer + , size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 agg_num; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx ", &agg_num); + + if (padapter && (num == 1)) { + RTW_INFO("padapter->driver_tx_max_agg_num = 0x%02x\n", agg_num); + + padapter->driver_tx_max_agg_num = agg_num; + } + } + + return count; +} + +int proc_get_rx_ampdu_density(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + + if (padapter) + RTW_PRINT_SEL(m, "rx ampdu densityg = %x\n", padapter->driver_rx_ampdu_spacing); + + return 0; +} + +ssize_t proc_set_rx_ampdu_density(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 density; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &density); + + if (padapter && (num == 1)) { + RTW_INFO("padapter->driver_rx_ampdu_spacing = %x\n", density); + + if (density > 0x07) + padapter->driver_rx_ampdu_spacing = 0xFF; + else + padapter->driver_rx_ampdu_spacing = density; + } + } + + return count; +} + +int proc_get_tx_ampdu_density(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + + if (padapter) + RTW_PRINT_SEL(m, "tx ampdu density = %x\n", padapter->driver_ampdu_spacing); + + return 0; +} + +ssize_t proc_set_tx_ampdu_density(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 density; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &density); + + if (padapter && (num == 1)) { + RTW_INFO("padapter->driver_ampdu_spacing = %x\n", density); + + if (density > 0x07) + padapter->driver_ampdu_spacing = 0xFF; + else + padapter->driver_ampdu_spacing = density; + } + } + + return count; +} + +int proc_get_tx_quick_addba_req(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (padapter) + RTW_PRINT_SEL(m, "tx_quick_addba_req = %x\n", pregpriv->tx_quick_addba_req); + + return 0; +} + +ssize_t proc_set_tx_quick_addba_req(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 enable; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &enable); + + if (padapter && (num == 1)) { + pregpriv->tx_quick_addba_req = enable; + RTW_INFO("tx_quick_addba_req = %d\n", pregpriv->tx_quick_addba_req); + } + } + + return count; +} +#ifdef CONFIG_TX_AMSDU +int proc_get_tx_amsdu(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter) + { + RTW_PRINT_SEL(m, "tx amsdu = %d\n", padapter->tx_amsdu); + RTW_PRINT_SEL(m, "amsdu set timer conut = %u\n", pxmitpriv->amsdu_debug_set_timer); + RTW_PRINT_SEL(m, "amsdu time out count = %u\n", pxmitpriv->amsdu_debug_timeout); + RTW_PRINT_SEL(m, "amsdu coalesce one count = %u\n", pxmitpriv->amsdu_debug_coalesce_one); + RTW_PRINT_SEL(m, "amsdu coalesce two count = %u\n", pxmitpriv->amsdu_debug_coalesce_two); + } + + return 0; +} + +ssize_t proc_set_tx_amsdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + char tmp[32]; + u32 amsdu; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &amsdu); + + if (padapter && (num == 1)) { + RTW_INFO("padapter->tx_amsdu = %x\n", amsdu); + + if (amsdu > 3) + padapter->tx_amsdu = 0; + else if(amsdu == 3) + { + pxmitpriv->amsdu_debug_set_timer = 0; + pxmitpriv->amsdu_debug_timeout = 0; + pxmitpriv->amsdu_debug_coalesce_one = 0; + pxmitpriv->amsdu_debug_coalesce_two = 0; + } + else + padapter->tx_amsdu = amsdu; + } + } + + return count; +} + +int proc_get_tx_amsdu_rate(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + if (padapter) + RTW_PRINT_SEL(m, "tx amsdu rate = %d Mbps\n", padapter->tx_amsdu_rate); + + return 0; +} + +ssize_t proc_set_tx_amsdu_rate(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 amsdu_rate; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &amsdu_rate); + + if (padapter && (num == 1)) { + RTW_INFO("padapter->tx_amsdu_rate = %x\n", amsdu_rate); + padapter->tx_amsdu_rate = amsdu_rate; + } + } + + return count; +} +#endif /* CONFIG_TX_AMSDU */ +#endif /* CONFIG_80211N_HT */ + +#ifdef CONFIG_80211AC_VHT +int proc_get_vht_24g_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "%d\n", pregpriv->vht_24g_enable); + + return 0; +} + +ssize_t proc_set_vht_24g_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if ((num == 1) && pregpriv && (mode < 2)) { + pregpriv->vht_24g_enable = mode; + RTW_INFO("vht_24g_enable = %d\n", pregpriv->vht_24g_enable); + } + } + + return count; + +} +#endif + +ssize_t proc_set_dyn_rrsr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + char tmp[32] = {0}; + u32 num = 0, enable = 0, rrsr_val = 0; /* gpio_mode:0 input 1:output; */ + + if (count < 2) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + num = sscanf(tmp, "%d 0x%x", &enable, &rrsr_val); + RTW_INFO("num=%u enable=%d rrsr_val=0x%x\n", num, enable, rrsr_val); + pregpriv->en_dyn_rrsr = enable; + pregpriv->set_rrsr_value = rrsr_val; + rtw_phydm_dyn_rrsr_en(padapter, enable); + rtw_phydm_set_rrsr(padapter, rrsr_val, TRUE); + + } + return count; + +} +int proc_get_dyn_rrsr(struct seq_file *m, void *v) { + + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + u32 init_rrsr =0xFFFFFFFF; + + if (padapter) + RTW_PRINT_SEL(m, "en_dyn_rrsr = %d fixed_rrsr_value =0x%x %s\n" + , pregpriv->en_dyn_rrsr + , pregpriv->set_rrsr_value + , (pregpriv->set_rrsr_value == init_rrsr)?"(default)":"(fixed)" + ); + + return 0; +} +int proc_get_en_fwps(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "check_fw_ps = %d , 1:enable get FW PS state , 0: disable get FW PS state\n" + , pregpriv->check_fw_ps); + + return 0; +} + +ssize_t proc_set_en_fwps(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if (num == 1 && pregpriv && mode < 2) { + pregpriv->check_fw_ps = mode; + RTW_INFO("pregpriv->check_fw_ps=%d\n", pregpriv->check_fw_ps); + } + + } + + return count; +} + +/* +int proc_get_two_path_rssi(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + if(padapter) + RTW_PRINT_SEL(m, "%d %d\n", + padapter->recvpriv.RxRssi[0], padapter->recvpriv.RxRssi[1]); + + return 0; +} +*/ +#ifdef CONFIG_80211N_HT +void rtw_dump_dft_phy_cap(void *sel, _adapter *adapter) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + #ifdef CONFIG_80211AC_VHT + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + #endif + + #ifdef CONFIG_80211AC_VHT + RTW_PRINT_SEL(sel, "[DFT CAP] VHT STBC Tx : %s\n", (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] VHT STBC Rx : %s\n", (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_RX)) ? "V" : "X"); + #endif + RTW_PRINT_SEL(sel, "[DFT CAP] HT STBC Tx : %s\n", (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] HT STBC Rx : %s\n\n", (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) ? "V" : "X"); + + #ifdef CONFIG_80211AC_VHT + RTW_PRINT_SEL(sel, "[DFT CAP] VHT LDPC Tx : %s\n", (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_TX)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] VHT LDPC Rx : %s\n", (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_RX)) ? "V" : "X"); + #endif + RTW_PRINT_SEL(sel, "[DFT CAP] HT LDPC Tx : %s\n", (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] HT LDPC Rx : %s\n\n", (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX)) ? "V" : "X"); + + #ifdef CONFIG_BEAMFORMING + #ifdef CONFIG_80211AC_VHT + RTW_PRINT_SEL(sel, "[DFT CAP] VHT MU Bfer : %s\n", (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] VHT MU Bfee : %s\n", (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] VHT SU Bfer : %s\n", (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] VHT SU Bfee : %s\n", (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)) ? "V" : "X"); + #endif + RTW_PRINT_SEL(sel, "[DFT CAP] HT Bfer : %s\n", (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DFT CAP] HT Bfee : %s\n", (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) ? "V" : "X"); + #endif +} + +void rtw_get_dft_phy_cap(void *sel, _adapter *adapter) +{ + RTW_PRINT_SEL(sel, "\n ======== PHY CAP protocol ========\n"); + rtw_ht_use_default_setting(adapter); + #ifdef CONFIG_80211AC_VHT + rtw_vht_use_default_setting(adapter); + #endif + #ifdef CONFIG_80211N_HT + rtw_dump_dft_phy_cap(sel, adapter); + #endif +} + +void rtw_dump_drv_phy_cap(void *sel, _adapter *adapter) +{ + struct registry_priv *pregistry_priv = &adapter->registrypriv; + + RTW_PRINT_SEL(sel, "\n ======== DRV's configuration ========\n"); + #if 0 + RTW_PRINT_SEL(sel, "[DRV CAP] TRx Capability : 0x%08x\n", phy_spec->trx_cap); + RTW_PRINT_SEL(sel, "[DRV CAP] Tx Stream Num Index : %d\n", (phy_spec->trx_cap >> 24) & 0xFF); /*Tx Stream Num Index [31:24]*/ + RTW_PRINT_SEL(sel, "[DRV CAP] Rx Stream Num Index : %d\n", (phy_spec->trx_cap >> 16) & 0xFF); /*Rx Stream Num Index [23:16]*/ + RTW_PRINT_SEL(sel, "[DRV CAP] Tx Path Num Index : %d\n", (phy_spec->trx_cap >> 8) & 0xFF);/*Tx Path Num Index [15:8]*/ + RTW_PRINT_SEL(sel, "[DRV CAP] Rx Path Num Index : %d\n", (phy_spec->trx_cap & 0xFF));/*Rx Path Num Index [7:0]*/ + #endif + #ifdef CONFIG_80211N_HT + RTW_PRINT_SEL(sel, "[DRV CAP] STBC Capability : 0x%02x\n", pregistry_priv->stbc_cap); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT STBC Tx : %s\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT1)) ? "V" : "X"); /*BIT1: Enable VHT STBC Tx*/ + RTW_PRINT_SEL(sel, "[DRV CAP] VHT STBC Rx : %s\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT0)) ? "V" : "X"); /*BIT0: Enable VHT STBC Rx*/ + RTW_PRINT_SEL(sel, "[DRV CAP] HT STBC Tx : %s\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT5)) ? "V" : "X"); /*BIT5: Enable HT STBC Tx*/ + RTW_PRINT_SEL(sel, "[DRV CAP] HT STBC Rx : %s\n\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT4)) ? "V" : "X"); /*BIT4: Enable HT STBC Rx*/ + + RTW_PRINT_SEL(sel, "[DRV CAP] LDPC Capability : 0x%02x\n", pregistry_priv->ldpc_cap); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT LDPC Tx : %s\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT1)) ? "V" : "X"); /*BIT1: Enable VHT LDPC Tx*/ + RTW_PRINT_SEL(sel, "[DRV CAP] VHT LDPC Rx : %s\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT0)) ? "V" : "X"); /*BIT0: Enable VHT LDPC Rx*/ + RTW_PRINT_SEL(sel, "[DRV CAP] HT LDPC Tx : %s\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT5)) ? "V" : "X"); /*BIT5: Enable HT LDPC Tx*/ + RTW_PRINT_SEL(sel, "[DRV CAP] HT LDPC Rx : %s\n\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT4)) ? "V" : "X"); /*BIT4: Enable HT LDPC Rx*/ + #endif /* CONFIG_80211N_HT */ + #ifdef CONFIG_BEAMFORMING + #if 0 + RTW_PRINT_SEL(sel, "[DRV CAP] TxBF parameter : 0x%08x\n", phy_spec->txbf_param); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT Sounding Dim : %d\n", (phy_spec->txbf_param >> 24) & 0xFF); /*VHT Sounding Dim [31:24]*/ + RTW_PRINT_SEL(sel, "[DRV CAP] VHT Steering Ant : %d\n", (phy_spec->txbf_param >> 16) & 0xFF); /*VHT Steering Ant [23:16]*/ + RTW_PRINT_SEL(sel, "[DRV CAP] HT Sounding Dim : %d\n", (phy_spec->txbf_param >> 8) & 0xFF); /*HT Sounding Dim [15:8]*/ + RTW_PRINT_SEL(sel, "[DRV CAP] HT Steering Ant : %d\n", phy_spec->txbf_param & 0xFF); /*HT Steering Ant [7:0]*/ + #endif + + /* + * BIT0: Enable VHT SU Beamformer + * BIT1: Enable VHT SU Beamformee + * BIT2: Enable VHT MU Beamformer, depend on VHT SU Beamformer + * BIT3: Enable VHT MU Beamformee, depend on VHT SU Beamformee + * BIT4: Enable HT Beamformer + * BIT5: Enable HT Beamformee + */ + RTW_PRINT_SEL(sel, "[DRV CAP] TxBF Capability : 0x%02x\n", pregistry_priv->beamform_cap); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT MU Bfer : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT2)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT MU Bfee : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT3)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT SU Bfer : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT0)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DRV CAP] VHT SU Bfee : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT1)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DRV CAP] HT Bfer : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT4)) ? "V" : "X"); + RTW_PRINT_SEL(sel, "[DRV CAP] HT Bfee : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT5)) ? "V" : "X"); + + RTW_PRINT_SEL(sel, "[DRV CAP] Tx Bfer rf_num : %d\n", pregistry_priv->beamformer_rf_num); + RTW_PRINT_SEL(sel, "[DRV CAP] Tx Bfee rf_num : %d\n", pregistry_priv->beamformee_rf_num); + #endif +} + +int proc_get_stbc_cap(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->stbc_cap); + + return 0; +} + +ssize_t proc_set_stbc_cap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if (num == 1 && pregpriv) { + pregpriv->stbc_cap = mode; + RTW_INFO("stbc_cap = 0x%02x\n", mode); + } + } + + return count; +} +int proc_get_rx_stbc(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "%d\n", pregpriv->rx_stbc); + + return 0; +} + +ssize_t proc_set_rx_stbc(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if (num == 1 && pregpriv && (mode == 0 || mode == 1 || mode == 2 || mode == 3)) { + pregpriv->rx_stbc = mode; + printk("rx_stbc=%d\n", mode); + } + } + + return count; + +} +int proc_get_ldpc_cap(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->ldpc_cap); + + return 0; +} + +ssize_t proc_set_ldpc_cap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if (num == 1 && pregpriv) { + pregpriv->ldpc_cap = mode; + RTW_INFO("ldpc_cap = 0x%02x\n", mode); + } + } + + return count; +} +#ifdef CONFIG_BEAMFORMING +int proc_get_txbf_cap(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->beamform_cap); + + return 0; +} + +ssize_t proc_set_txbf_cap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 mode; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &mode); + + if (num == 1 && pregpriv) { + pregpriv->beamform_cap = mode; + RTW_INFO("beamform_cap = 0x%02x\n", mode); + } + } + + return count; +} +#endif +#endif /* CONFIG_80211N_HT */ + +/*int proc_get_rssi_disp(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + return 0; +} +*/ + +/*ssize_t proc_set_rssi_disp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 enable=0; + + if (count < 1) + { + RTW_INFO("argument size is less than 1\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%x", &enable); + + if (num != 1) { + RTW_INFO("invalid set_rssi_disp parameter!\n"); + return count; + } + + if(enable) + { + RTW_INFO("Linked info Function Enable\n"); + padapter->bLinkInfoDump = enable ; + } + else + { + RTW_INFO("Linked info Function Disable\n"); + padapter->bLinkInfoDump = 0 ; + } + + } + + return count; + +} + +*/ +#ifdef CONFIG_AP_MODE + +int proc_get_all_sta_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _irqL irqL; + struct sta_info *psta; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct sta_priv *pstapriv = &padapter->stapriv; + int i; + _list *plist, *phead; + + RTW_MAP_DUMP_SEL(m, "sta_dz_bitmap=", pstapriv->sta_dz_bitmap, pstapriv->aid_bmp_len); + RTW_MAP_DUMP_SEL(m, "tim_bitmap=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + plist = get_next(plist); + + /* if(extra_arg == psta->cmn.aid) */ + { + RTW_PRINT_SEL(m, "==============================\n"); + RTW_PRINT_SEL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->cmn.mac_addr)); + RTW_PRINT_SEL(m, "rtsen=%d, cts2slef=%d\n", psta->rtsen, psta->cts2self); + RTW_PRINT_SEL(m, "state=0x%x, aid=%d, macid=%d, raid=%d\n", + psta->state, psta->cmn.aid, psta->cmn.mac_id, psta->cmn.ra_info.rate_id); +#ifdef CONFIG_RTS_FULL_BW + if(psta->vendor_8812) + RTW_PRINT_SEL(m,"Vendor Realtek 8812\n"); +#endif/*CONFIG_RTS_FULL_BW*/ +#ifdef CONFIG_80211N_HT + RTW_PRINT_SEL(m, "qos_en=%d, ht_en=%d, init_rate=%d, ht_bitrate=%u\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate + , rtw_desc_rate_to_bitrate(psta->cmn.bw_mode, psta->init_rate, psta->cmn.bw_mode ? psta->htpriv.sgi_40m : psta->htpriv.sgi_20m) / 10); + RTW_PRINT_SEL(m, "bwmode=%d, ch_offset=%d, sgi_20m=%d,sgi_40m=%d\n" + , psta->cmn.bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m); + RTW_PRINT_SEL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable); + RTW_PRINT_SEL(m, "tx_amsdu_enable = %d\n", psta->htpriv.tx_amsdu_enable); + RTW_PRINT_SEL(m, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap); +#endif /* CONFIG_80211N_HT */ +#ifdef CONFIG_80211AC_VHT + RTW_PRINT_SEL(m, "vht_en=%d, vht_sgi_80m=%d, vht_bitrate=%u\n", psta->vhtpriv.vht_option, psta->vhtpriv.sgi_80m + , psta->vhtpriv.vht_option ? rtw_vht_mcs_to_data_rate(CHANNEL_WIDTH_80, psta->vhtpriv.sgi_80m, psta->vhtpriv.vht_highest_rate) / 2 : 0); + RTW_PRINT_SEL(m, "vht_ldpc_cap=0x%x, vht_stbc_cap=0x%x, vht_beamform_cap=0x%x\n", psta->vhtpriv.ldpc_cap, psta->vhtpriv.stbc_cap, psta->vhtpriv.beamform_cap); + RTW_PRINT_SEL(m, "vht_mcs_map=0x%x, vht_highest_rate=0x%x, vht_ampdu_len=%d\n", *(u16 *)psta->vhtpriv.vht_mcs_map, psta->vhtpriv.vht_highest_rate, psta->vhtpriv.ampdu_len); +#endif + RTW_PRINT_SEL(m, "sleepq_len=%d\n", psta->sleepq_len); + RTW_PRINT_SEL(m, "sta_xmitpriv.vo_q_qcnt=%d\n", psta->sta_xmitpriv.vo_q.qcnt); + RTW_PRINT_SEL(m, "sta_xmitpriv.vi_q_qcnt=%d\n", psta->sta_xmitpriv.vi_q.qcnt); + RTW_PRINT_SEL(m, "sta_xmitpriv.be_q_qcnt=%d\n", psta->sta_xmitpriv.be_q.qcnt); + RTW_PRINT_SEL(m, "sta_xmitpriv.bk_q_qcnt=%d\n", psta->sta_xmitpriv.bk_q.qcnt); + +#ifdef CONFIG_RTW_MGMT_QUEUE + RTW_PRINT_SEL(m, "management sleepq_len=%d\n", psta->mgmt_sleepq_len); + RTW_PRINT_SEL(m, "sta_xmitpriv.mgmt_q_qcnt=%d\n", psta->sta_xmitpriv.mgmt_q.qcnt); +#endif + + RTW_PRINT_SEL(m, "capability=0x%x\n", psta->capability); + RTW_PRINT_SEL(m, "flags=0x%x\n", psta->flags); + RTW_PRINT_SEL(m, "wpa_psk=0x%x\n", psta->wpa_psk); + RTW_PRINT_SEL(m, "wpa2_group_cipher=0x%x\n", psta->wpa2_group_cipher); + RTW_PRINT_SEL(m, "wpa2_pairwise_cipher=0x%x\n", psta->wpa2_pairwise_cipher); + RTW_PRINT_SEL(m, "qos_info=0x%x\n", psta->qos_info); + RTW_PRINT_SEL(m, "dot118021XPrivacy=0x%x\n", psta->dot118021XPrivacy); + + sta_rx_reorder_ctl_dump(m, psta); + +#ifdef CONFIG_TDLS + RTW_PRINT_SEL(m, "tdls_sta_state=0x%08x\n", psta->tdls_sta_state); + RTW_PRINT_SEL(m, "PeerKey_Lifetime=%d\n", psta->TDLS_PeerKey_Lifetime); +#endif /* CONFIG_TDLS */ + RTW_PRINT_SEL(m, "rx_data_uc_pkts=%llu\n", sta_rx_data_uc_pkts(psta)); + RTW_PRINT_SEL(m, "rx_data_mc_pkts=%llu\n", psta->sta_stats.rx_data_mc_pkts); + RTW_PRINT_SEL(m, "rx_data_bc_pkts=%llu\n", psta->sta_stats.rx_data_bc_pkts); + RTW_PRINT_SEL(m, "rx_uc_bytes=%llu\n", sta_rx_uc_bytes(psta)); + RTW_PRINT_SEL(m, "rx_mc_bytes=%llu\n", psta->sta_stats.rx_mc_bytes); + RTW_PRINT_SEL(m, "rx_bc_bytes=%llu\n", psta->sta_stats.rx_bc_bytes); + if (psta->sta_stats.rx_tp_kbits >> 10) + RTW_PRINT_SEL(m, "rx_tp =%d (Mbps)\n", psta->sta_stats.rx_tp_kbits >> 10); + else + RTW_PRINT_SEL(m, "rx_tp =%d (Kbps)\n", psta->sta_stats.rx_tp_kbits); + + RTW_PRINT_SEL(m, "tx_data_pkts=%llu\n", psta->sta_stats.tx_pkts); + RTW_PRINT_SEL(m, "tx_bytes=%llu\n", psta->sta_stats.tx_bytes); + if (psta->sta_stats.tx_tp_kbits >> 10) + RTW_PRINT_SEL(m, "tx_tp =%d (Mbps)\n", psta->sta_stats.tx_tp_kbits >> 10); + else + RTW_PRINT_SEL(m, "tx_tp =%d (Kbps)\n", psta->sta_stats.tx_tp_kbits); +#ifdef CONFIG_RTW_80211K + RTW_PRINT_SEL(m, "rm_en_cap="RM_CAP_FMT"\n", RM_CAP_ARG(psta->rm_en_cap)); +#endif + dump_st_ctl(m, &psta->st_ctl); + + if (STA_OP_WFD_MODE(psta)) + RTW_PRINT_SEL(m, "op_wfd_mode:0x%02x\n", STA_OP_WFD_MODE(psta)); + + RTW_PRINT_SEL(m, "tx_bitrate_100kbps=%u\n", rtw_desc_rate_to_bitrate(psta->cmn.bw_mode, rtw_get_current_tx_rate(padapter, psta), rtw_get_current_tx_sgi(padapter, psta))); + RTW_PRINT_SEL(m, "rx_bitrate_100kbps=%u\n", rtw_desc_rate_to_bitrate(psta->cmn.bw_mode, psta->curr_rx_rate & 0x7f, (psta->curr_rx_rate & 0x80) >> 7)); + RTW_PRINT_SEL(m, "rssi=%d\n", psta->cmn.rssi_stat.rssi); + RTW_PRINT_SEL(m, "==============================\n"); + } + + } + + } + + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + return 0; +} + +#endif + +#ifdef CONFIG_PREALLOC_RX_SKB_BUFFER +int proc_get_rtkm_info(struct seq_file *m, void *v) +{ +#ifdef CONFIG_USB_HCI + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct recv_priv *precvpriv = &padapter->recvpriv; + struct recv_buf *precvbuf; + + precvbuf = (struct recv_buf *)precvpriv->precv_buf; +#endif /* CONFIG_USB_HCI */ + + RTW_PRINT_SEL(m, "============[RTKM Info]============\n"); + RTW_PRINT_SEL(m, "MAX_RTKM_NR_PREALLOC_RECV_SKB: %d\n", rtw_rtkm_get_nr_recv_skb()); + RTW_PRINT_SEL(m, "MAX_RTKM_RECVBUF_SZ: %d\n", rtw_rtkm_get_buff_size()); + + RTW_PRINT_SEL(m, "============[Driver Info]============\n"); + RTW_PRINT_SEL(m, "NR_PREALLOC_RECV_SKB: %d\n", NR_PREALLOC_RECV_SKB); +#ifdef CONFIG_USB_HCI + RTW_PRINT_SEL(m, "MAX_RECVBUF_SZ: %d\n", precvbuf->alloc_sz); +#else /* !CONFIG_USB_HCI */ + RTW_PRINT_SEL(m, "MAX_RECVBUF_SZ: %d\n", MAX_RECVBUF_SZ); +#endif /* !CONFIG_USB_HCI */ + + return 0; +} +#endif /* CONFIG_PREALLOC_RX_SKB_BUFFER */ + +#ifdef DBG_MEMORY_LEAK +#include +extern atomic_t _malloc_cnt;; +extern atomic_t _malloc_size;; + +int proc_get_malloc_cnt(struct seq_file *m, void *v) +{ + RTW_PRINT_SEL(m, "_malloc_cnt=%d\n", atomic_read(&_malloc_cnt)); + RTW_PRINT_SEL(m, "_malloc_size=%d\n", atomic_read(&_malloc_size)); + + return 0; +} +#endif /* DBG_MEMORY_LEAK */ + +#ifdef CONFIG_FIND_BEST_CHANNEL +int proc_get_best_channel(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + u32 i, best_channel_24G = 1, best_channel_5G = 36, index_24G = 0, index_5G = 0; + + for (i = 0; i < rfctl->max_chan_nums && rfctl->channel_set[i].ChannelNum != 0; i++) { + if (rfctl->channel_set[i].ChannelNum == 1) + index_24G = i; + if (rfctl->channel_set[i].ChannelNum == 36) + index_5G = i; + } + + for (i = 0; i < rfctl->max_chan_nums && rfctl->channel_set[i].ChannelNum != 0; i++) { + /* 2.4G */ + if (rfctl->channel_set[i].ChannelNum == 6) { + if (rfctl->channel_set[i].rx_count < rfctl->channel_set[index_24G].rx_count) { + index_24G = i; + best_channel_24G = rfctl->channel_set[i].ChannelNum; + } + } + + /* 5G */ + if (rfctl->channel_set[i].ChannelNum >= 36 + && rfctl->channel_set[i].ChannelNum < 140) { + /* Find primary channel */ + if (((rfctl->channel_set[i].ChannelNum - 36) % 8 == 0) + && (rfctl->channel_set[i].rx_count < rfctl->channel_set[index_5G].rx_count)) { + index_5G = i; + best_channel_5G = rfctl->channel_set[i].ChannelNum; + } + } + + if (rfctl->channel_set[i].ChannelNum >= 149 + && rfctl->channel_set[i].ChannelNum < 165) { + /* find primary channel */ + if (((rfctl->channel_set[i].ChannelNum - 149) % 8 == 0) + && (rfctl->channel_set[i].rx_count < rfctl->channel_set[index_5G].rx_count)) { + index_5G = i; + best_channel_5G = rfctl->channel_set[i].ChannelNum; + } + } +#if 1 /* debug */ + RTW_PRINT_SEL(m, "The rx cnt of channel %3d = %d\n", + rfctl->channel_set[i].ChannelNum, rfctl->channel_set[i].rx_count); +#endif + } + + RTW_PRINT_SEL(m, "best_channel_5G = %d\n", best_channel_5G); + RTW_PRINT_SEL(m, "best_channel_24G = %d\n", best_channel_24G); + + return 0; +} + +ssize_t proc_set_best_channel(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + char tmp[32]; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int i; + for (i = 0; i < rfctl->max_chan_nums && rfctl->channel_set[i].ChannelNum != 0; i++) + rfctl->channel_set[i].rx_count = 0; + + RTW_INFO("set %s\n", "Clean Best Channel Count"); + } + + return count; +} +#endif /* CONFIG_FIND_BEST_CHANNEL */ + +#ifdef CONFIG_BT_COEXIST +int proc_get_btcoex_dbg(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + PADAPTER padapter; + char buf[512] = {0}; + padapter = (PADAPTER)rtw_netdev_priv(dev); + + rtw_btcoex_GetDBG(padapter, buf, 512); + + _RTW_PRINT_SEL(m, "%s", buf); + + return 0; +} + +ssize_t proc_set_btcoex_dbg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + PADAPTER padapter; + u8 tmp[80] = {0}; + u32 module[2] = {0}; + u32 num; + + padapter = (PADAPTER)rtw_netdev_priv(dev); + + /* RTW_INFO("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter)); */ + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", + FUNC_ADPT_ARG(padapter)); + + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", + FUNC_ADPT_ARG(padapter)); + + return -EFAULT; + } + + num = count; + if (num > (sizeof(tmp) - 1)) + num = (sizeof(tmp) - 1); + + if (copy_from_user(tmp, buffer, num)) { + RTW_INFO(FUNC_ADPT_FMT ": copy buffer from user space FAIL!\n", + FUNC_ADPT_ARG(padapter)); + + return -EFAULT; + } + + num = sscanf(tmp, "%x %x", module, module + 1); + if (1 == num) { + if (0 == module[0]) + _rtw_memset(module, 0, sizeof(module)); + else + _rtw_memset(module, 0xFF, sizeof(module)); + } else if (2 != num) { + RTW_INFO(FUNC_ADPT_FMT ": input(\"%s\") format incorrect!\n", + FUNC_ADPT_ARG(padapter), tmp); + + if (0 == num) + return -EFAULT; + } + + RTW_INFO(FUNC_ADPT_FMT ": input 0x%08X 0x%08X\n", + FUNC_ADPT_ARG(padapter), module[0], module[1]); + rtw_btcoex_SetDBG(padapter, module); + + return count; +} + +int proc_get_btcoex_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + PADAPTER padapter; + const u32 bufsize = 40 * 100; + u8 *pbuf = NULL; + + padapter = (PADAPTER)rtw_netdev_priv(dev); + + pbuf = rtw_zmalloc(bufsize); + if (NULL == pbuf) + return -ENOMEM; + + rtw_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize); + + _RTW_PRINT_SEL(m, "%s\n", pbuf); + + rtw_mfree(pbuf, bufsize); + + return 0; +} + +#ifdef CONFIG_RF4CE_COEXIST +int proc_get_rf4ce_state(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + u8 state = 0, voice = 0; + + state = rtw_btcoex_GetRf4ceLinkState(adapter); + + RTW_PRINT_SEL(m, "RF4CE %s\n", state?"Connected":"Disconnect"); + + return 0; +} + +/* This interface is designed for user space application to inform RF4CE state + * Initial define for DHC 1295 E387 project + * + * echo state voice > rf4ce_state + * state + * 0: RF4CE disconnected + * 1: RF4CE connected + */ +ssize_t proc_set_rf4ce_state(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 state; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx", &state); + + if (num >= 1) + rtw_btcoex_SetRf4ceLinkState(adapter, state); + } + + return count; +} +#endif /* CONFIG_RF4CE_COEXIST */ +#endif /* CONFIG_BT_COEXIST */ + +#if defined(DBG_CONFIG_ERROR_DETECT) +int proc_get_sreset(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + + if (psrtpriv->dbg_sreset_ctrl == _TRUE) { + RTW_PRINT_SEL(m, "self_dect_tx_cnt:%llu\n", psrtpriv->self_dect_tx_cnt); + RTW_PRINT_SEL(m, "self_dect_rx_cnt:%llu\n", psrtpriv->self_dect_rx_cnt); + RTW_PRINT_SEL(m, "self_dect_fw_cnt:%llu\n", psrtpriv->self_dect_fw_cnt); + RTW_PRINT_SEL(m, "tx_dma_status_cnt:%llu\n", psrtpriv->tx_dma_status_cnt); + RTW_PRINT_SEL(m, "rx_dma_status_cnt:%llu\n", psrtpriv->rx_dma_status_cnt); + RTW_PRINT_SEL(m, "self_dect_case:%d\n", psrtpriv->self_dect_case); + RTW_PRINT_SEL(m, "dbg_sreset_cnt:%d\n", pdbgpriv->dbg_sreset_cnt); + } + return 0; +} + +ssize_t proc_set_sreset(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + char tmp[32]; + s32 trigger_point; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d", &trigger_point); + + if (num < 1) + return count; + + if (trigger_point == SRESET_TGP_NULL) + rtw_hal_sreset_reset(padapter); + else if (trigger_point == SRESET_TGP_INFO) + psrtpriv->dbg_sreset_ctrl = _TRUE; + else + sreset_set_trigger_point(padapter, trigger_point); + } + + return count; + +} +#endif /* DBG_CONFIG_ERROR_DETECT */ + +#ifdef CONFIG_PCI_HCI + +ssize_t proc_set_pci_bridge_conf_space(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct pci_dev *pdev = pdvobjpriv->ppcidev; + struct pci_dev *bridge_pdev = pdev->bus->self; + + char tmp[32] = { 0 }; + int num; + + u32 reg = 0, value = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + num = sscanf(tmp, "%x %x", ®, &value); + if (num != 2) { + RTW_INFO("invalid parameter!\n"); + return count; + } + + if (reg >= 0x1000) { + RTW_INFO("invalid register!\n"); + return count; + } + + if (value > 0xFF) { + RTW_INFO("invalid value! Only one byte\n"); + return count; + } + + RTW_INFO(FUNC_ADPT_FMT ": register 0x%x value 0x%x\n", + FUNC_ADPT_ARG(padapter), reg, value); + + pci_write_config_byte(bridge_pdev, reg, value); + } + return count; +} + + +int proc_get_pci_bridge_conf_space(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *) rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct pci_dev *pdev = pdvobjpriv->ppcidev; + struct pci_dev *bridge_pdev = pdev->bus->self; + + u32 tmp[4] = { 0 }; + u32 i, j; + + RTW_PRINT_SEL(m, "\n***** PCI Host Device Configuration Space*****\n\n"); + + for (i = 0; i < 0x1000; i += 0x10) { + for (j = 0 ; j < 4 ; j++) + pci_read_config_dword(bridge_pdev, i + j * 4, tmp+j); + + RTW_PRINT_SEL(m, "%03x: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n", + i, tmp[0] & 0xFF, (tmp[0] >> 8) & 0xFF, (tmp[0] >> 16) & 0xFF, (tmp[0] >> 24) & 0xFF, + tmp[1] & 0xFF, (tmp[1] >> 8) & 0xFF, (tmp[1] >> 16) & 0xFF, (tmp[1] >> 24) & 0xFF, + tmp[2] & 0xFF, (tmp[2] >> 8) & 0xFF, (tmp[2] >> 16) & 0xFF, (tmp[2] >> 24) & 0xFF, + tmp[3] & 0xFF, (tmp[3] >> 8) & 0xFF, (tmp[3] >> 16) & 0xFF, (tmp[3] >> 24) & 0xFF); + } + return 0; +} + + +ssize_t proc_set_pci_conf_space(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct pci_dev *pdev = pdvobjpriv->ppcidev; + + char tmp[32] = { 0 }; + int num; + + u32 reg = 0, value = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + num = sscanf(tmp, "%x %x", ®, &value); + + if (num != 2) { + RTW_INFO("invalid parameter!\n"); + return count; + } + + + if (reg >= 0x1000) { + RTW_INFO("invalid register!\n"); + return count; + } + + if (value > 0xFF) { + RTW_INFO("invalid value! Only one byte\n"); + return count; + } + + RTW_INFO(FUNC_ADPT_FMT ": register 0x%x value 0x%x\n", + FUNC_ADPT_ARG(padapter), reg, value); + + pci_write_config_byte(pdev, reg, value); + + + } + return count; +} + + +int proc_get_pci_conf_space(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *) rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct pci_dev *pdev = pdvobjpriv->ppcidev; + struct pci_dev *bridge_pdev = pdev->bus->self; + + u32 tmp[4] = { 0 }; + u32 i, j; + + RTW_PRINT_SEL(m, "\n***** PCI Device Configuration Space *****\n\n"); + + for (i = 0; i < 0x1000; i += 0x10) { + for (j = 0 ; j < 4 ; j++) + pci_read_config_dword(pdev, i + j * 4, tmp+j); + + RTW_PRINT_SEL(m, "%03x: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n", + i, tmp[0] & 0xFF, (tmp[0] >> 8) & 0xFF, (tmp[0] >> 16) & 0xFF, (tmp[0] >> 24) & 0xFF, + tmp[1] & 0xFF, (tmp[1] >> 8) & 0xFF, (tmp[1] >> 16) & 0xFF, (tmp[1] >> 24) & 0xFF, + tmp[2] & 0xFF, (tmp[2] >> 8) & 0xFF, (tmp[2] >> 16) & 0xFF, (tmp[2] >> 24) & 0xFF, + tmp[3] & 0xFF, (tmp[3] >> 8) & 0xFF, (tmp[3] >> 16) & 0xFF, (tmp[3] >> 24) & 0xFF); + } + + return 0; +} + + +int proc_get_pci_aspm(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *) rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct pci_priv *pcipriv = &(pdvobjpriv->pcipriv); + u8 tmp8 = 0; + u16 tmp16 = 0; + u32 tmp32 = 0; + u8 l1_idle = 0; + + + RTW_PRINT_SEL(m, "***** ASPM Capability *****\n"); + + pci_read_config_dword(pdvobjpriv->ppcidev, pcipriv->pciehdr_offset + PCI_EXP_LNKCAP, &tmp32); + + RTW_PRINT_SEL(m, "CLK REQ: %s\n", (tmp32&PCI_EXP_LNKCAP_CLKPM) ? "Enable" : "Disable"); + RTW_PRINT_SEL(m, "ASPM L0s: %s\n", (tmp32&BIT10) ? "Enable" : "Disable"); + RTW_PRINT_SEL(m, "ASPM L1: %s\n", (tmp32&BIT11) ? "Enable" : "Disable"); + + tmp8 = rtw_hal_pci_l1off_capability(padapter); + RTW_PRINT_SEL(m, "ASPM L1OFF: %s\n", tmp8 ? "Enable" : "Disable"); + + RTW_PRINT_SEL(m, "***** ASPM CTRL Reg *****\n"); + + pci_read_config_word(pdvobjpriv->ppcidev, pcipriv->pciehdr_offset + PCI_EXP_LNKCTL, &tmp16); + + RTW_PRINT_SEL(m, "CLK REQ: %s\n", (tmp16&PCI_EXP_LNKCTL_CLKREQ_EN) ? "Enable" : "Disable"); + RTW_PRINT_SEL(m, "ASPM L0s: %s\n", (tmp16&BIT0) ? "Enable" : "Disable"); + RTW_PRINT_SEL(m, "ASPM L1: %s\n", (tmp16&BIT1) ? "Enable" : "Disable"); + + tmp8 = rtw_hal_pci_l1off_nic_support(padapter); + RTW_PRINT_SEL(m, "ASPM L1OFF: %s\n", tmp8 ? "Enable" : "Disable"); + + RTW_PRINT_SEL(m, "***** ASPM Backdoor *****\n"); + + tmp8 = rtw_hal_pci_dbi_read(padapter, 0x719); + RTW_PRINT_SEL(m, "CLK REQ: %s\n", (tmp8 & BIT4) ? "Enable" : "Disable"); + + tmp8 = rtw_hal_pci_dbi_read(padapter, 0x70f); + l1_idle = tmp8 & 0x38; + RTW_PRINT_SEL(m, "ASPM L0s: %s\n", (tmp8&BIT7) ? "Enable" : "Disable"); + + tmp8 = rtw_hal_pci_dbi_read(padapter, 0x719); + RTW_PRINT_SEL(m, "ASPM L1: %s\n", (tmp8 & BIT3) ? "Enable" : "Disable"); + + tmp8 = rtw_hal_pci_dbi_read(padapter, 0x718); + RTW_PRINT_SEL(m, "ASPM L1OFF: %s\n", (tmp8 & BIT5) ? "Enable" : "Disable"); + + RTW_PRINT_SEL(m, "********* MISC **********\n"); + RTW_PRINT_SEL(m, "ASPM L1 Idel Time: 0x%x\n", l1_idle>>3); + RTW_PRINT_SEL(m, "*************************\n"); + +#ifdef CONFIG_PCI_DYNAMIC_ASPM + RTW_PRINT_SEL(m, "Dynamic ASPM mode: %d (%s)\n", pcipriv->aspm_mode, + pcipriv->aspm_mode == ASPM_MODE_PERF ? "Perf" : + pcipriv->aspm_mode == ASPM_MODE_PS ? "PS" : "Und"); +#endif + + return 0; +} + +int proc_get_rx_ring(struct seq_file *m, void *v) +{ + _irqL irqL; + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *) rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct recv_priv *precvpriv = &padapter->recvpriv; + struct rtw_rx_ring *rx_ring = &precvpriv->rx_ring[RX_MPDU_QUEUE]; + int i, j; + + RTW_PRINT_SEL(m, "rx ring (%p)\n", rx_ring); + RTW_PRINT_SEL(m, " dma: 0x%08x\n", (int) rx_ring->dma); + RTW_PRINT_SEL(m, " idx: %d\n", rx_ring->idx); + + _enter_critical(&pdvobjpriv->irq_th_lock, &irqL); + for (i = 0; i < precvpriv->rxringcount; i++) { +#ifdef CONFIG_TRX_BD_ARCH + struct rx_buf_desc *entry = &rx_ring->buf_desc[i]; +#else + struct recv_stat *entry = &rx_ring->desc[i]; +#endif + struct sk_buff *skb = rx_ring->rx_buf[i]; + + RTW_PRINT_SEL(m, " desc[%03d]: %p, rx_buf[%03d]: 0x%08x\n", + i, entry, i, cpu_to_le32(*((dma_addr_t *)skb->cb))); + + for (j = 0; j < sizeof(*entry) / 4; j++) { + if ((j % 4) == 0) + RTW_PRINT_SEL(m, " 0x%03x", j); + + RTW_PRINT_SEL(m, " 0x%08x ", ((int *) entry)[j]); + + if ((j % 4) == 3) + RTW_PRINT_SEL(m, "\n"); + } + } + _exit_critical(&pdvobjpriv->irq_th_lock, &irqL); + + return 0; +} + +int proc_get_tx_ring(struct seq_file *m, void *v) +{ + _irqL irqL; + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *) rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + int i, j, k; + + _enter_critical(&pdvobjpriv->irq_th_lock, &irqL); + for (i = 0; i < PCI_MAX_TX_QUEUE_COUNT; i++) { + struct rtw_tx_ring *tx_ring = &pxmitpriv->tx_ring[i]; + + RTW_PRINT_SEL(m, "tx ring[%d] (%p)\n", i, tx_ring); + RTW_PRINT_SEL(m, " dma: 0x%08x\n", (int) tx_ring->dma); + RTW_PRINT_SEL(m, " idx: %d\n", tx_ring->idx); + RTW_PRINT_SEL(m, " entries: %d\n", tx_ring->entries); + /* RTW_PRINT_SEL(m, " queue: %d\n", tx_ring->queue); */ + RTW_PRINT_SEL(m, " qlen: %d\n", tx_ring->qlen); + + for (j = 0; j < pxmitpriv->txringcount[i]; j++) { +#ifdef CONFIG_TRX_BD_ARCH + struct tx_buf_desc *entry = &tx_ring->buf_desc[j]; + RTW_PRINT_SEL(m, " buf_desc[%03d]: %p\n", j, entry); +#else + struct tx_desc *entry = &tx_ring->desc[j]; + RTW_PRINT_SEL(m, " desc[%03d]: %p\n", j, entry); +#endif + + for (k = 0; k < sizeof(*entry) / 4; k++) { + if ((k % 4) == 0) + RTW_PRINT_SEL(m, " 0x%03x", k); + + RTW_PRINT_SEL(m, " 0x%08x ", ((int *) entry)[k]); + + if ((k % 4) == 3) + RTW_PRINT_SEL(m, "\n"); + } + } + } + _exit_critical(&pdvobjpriv->irq_th_lock, &irqL); + + return 0; +} + +#ifdef DBG_TXBD_DESC_DUMP +int proc_get_tx_ring_ext(struct seq_file *m, void *v) +{ + _irqL irqL; + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *) rtw_netdev_priv(dev); + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct rtw_tx_desc_backup *pbuf; + int i, j, k, idx; + + RTW_PRINT_SEL(m, "<<<< tx ring ext dump settings >>>>\n"); + RTW_PRINT_SEL(m, " - backup frame num: %d\n", TX_BAK_FRMAE_CNT); + RTW_PRINT_SEL(m, " - backup max. desc size: %d bytes\n", TX_BAK_DESC_LEN); + RTW_PRINT_SEL(m, " - backup data size: %d bytes\n\n", TX_BAK_DATA_LEN); + + if (!pxmitpriv->dump_txbd_desc) { + RTW_PRINT_SEL(m, "Dump function is disabled.\n"); + return 0; + } + + _enter_critical(&pdvobjpriv->irq_th_lock, &irqL); + for (i = 0; i < HW_QUEUE_ENTRY; i++) { + struct rtw_tx_ring *tx_ring = &pxmitpriv->tx_ring[i]; + + idx = rtw_get_tx_desc_backup(padapter, i, &pbuf); + + RTW_PRINT_SEL(m, "Tx ring[%d]", i); + switch (i) { + case 0: + RTW_PRINT_SEL(m, " (VO)\n"); + break; + case 1: + RTW_PRINT_SEL(m, " (VI)\n"); + break; + case 2: + RTW_PRINT_SEL(m, " (BE)\n"); + break; + case 3: + RTW_PRINT_SEL(m, " (BK)\n"); + break; + case 4: + RTW_PRINT_SEL(m, " (BCN)\n"); + break; + case 5: + RTW_PRINT_SEL(m, " (MGT)\n"); + break; + case 6: + RTW_PRINT_SEL(m, " (HIGH)\n"); + break; + case 7: + RTW_PRINT_SEL(m, " (TXCMD)\n"); + break; + default: + RTW_PRINT_SEL(m, " (?)\n"); + break; + } + + RTW_PRINT_SEL(m, " Entries: %d\n", TX_BAK_FRMAE_CNT); + RTW_PRINT_SEL(m, " Last idx: %d\n", idx); + + for (j = 0; j < TX_BAK_FRMAE_CNT; j++) { + RTW_PRINT_SEL(m, " desc[%03d]:\n", j); + + for (k = 0; k < (pbuf->tx_desc_size) / 4; k++) { + if ((k % 4) == 0) + RTW_PRINT_SEL(m, " 0x%03x", k); + + RTW_PRINT_SEL(m, " 0x%08x ", ((int *)pbuf->tx_bak_desc)[k]); + + if ((k % 4) == 3) + RTW_PRINT_SEL(m, "\n"); + } + +#if 1 /* data dump */ + if (pbuf->tx_desc_size) { + RTW_PRINT_SEL(m, " data[%03d]:\n", j); + + for (k = 0; k < (TX_BAK_DATA_LEN) / 4; k++) { + if ((k % 4) == 0) + RTW_PRINT_SEL(m, " 0x%03x", k); + + RTW_PRINT_SEL(m, " 0x%08x ", ((int *)pbuf->tx_bak_data_hdr)[k]); + + if ((k % 4) == 3) + RTW_PRINT_SEL(m, "\n"); + } + RTW_PRINT_SEL(m, "\n"); + } +#endif + + RTW_PRINT_SEL(m, " R/W pointer: %d/%d\n", pbuf->tx_bak_rp, pbuf->tx_bak_wp); + + pbuf = pbuf + 1; + } + RTW_PRINT_SEL(m, "\n"); + } + _exit_critical(&pdvobjpriv->irq_th_lock, &irqL); + + return 0; +} + +ssize_t proc_set_tx_ring_ext(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + _irqL irqL; + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + char tmp[32]; + u32 reset = 0; + u32 dump = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%u %u", &dump, &reset); + + if (num != 2) { + RTW_INFO("invalid parameter!\n"); + return count; + } + + _enter_critical(&pdvobjpriv->irq_th_lock, &irqL); + pxmitpriv->dump_txbd_desc = (BOOLEAN) dump; + + if (reset == 1) + rtw_tx_desc_backup_reset(); + + _exit_critical(&pdvobjpriv->irq_th_lock, &irqL); + + } + + return count; +} + +#endif + +#endif + +#ifdef CONFIG_WOWLAN +int proc_get_wow_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *registry_pair = &padapter->registrypriv; + + RTW_PRINT_SEL(m, "wow - %s\n", (registry_pair->wowlan_enable)? "enable" : "disable"); + return 0; +} + +ssize_t proc_set_wow_enable(struct file *file, const char __user *buffer, + size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *registry_pair = &padapter->registrypriv; + char tmp[8]; + int num = 0; + int mode = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) + num = sscanf(tmp, "%d", &mode); + else + return -EFAULT; + + if (num != 1) { + RTW_ERR("%s: %s - invalid parameter!\n", __func__, tmp); + return -EINVAL; + } + + if (mode == 1) { + RTW_PRINT("%s: wowlan - enable\n", __func__); + } else if (mode == 0) { + RTW_PRINT("%s: wowlan - disable\n", __func__); + } else { + RTW_ERR("%s: %s - invalid parameter!, mode=%d\n", + __func__, tmp, mode); + return -EINVAL; + } + + registry_pair->wowlan_enable = mode; + + return count; +} + +int proc_get_pattern_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 val8; + char str_1[128]; + char *p_str; + int i = 0 , j = 0, k = 0; + int len = 0, max_len = 0, total = 0; + + p_str = str_1; + max_len = sizeof(str_1); + + total = pwrpriv->wowlan_pattern_idx; + + rtw_set_default_pattern(padapter); + + /*show pattern*/ + RTW_PRINT_SEL(m, "\n======[Pattern Info.]======\n"); + RTW_PRINT_SEL(m, "pattern number: %d\n", total); + RTW_PRINT_SEL(m, "support default patterns: %c\n", + (pwrpriv->default_patterns_en) ? 'Y' : 'N'); + + for (k = 0; k < total ; k++) { + RTW_PRINT_SEL(m, "\npattern idx: %d\n", k); + RTW_PRINT_SEL(m, "pattern content:\n"); + + p_str = str_1; + max_len = sizeof(str_1); + for (i = 0 ; i < MAX_WKFM_PATTERN_SIZE / 8 ; i++) { + _rtw_memset(p_str, 0, max_len); + len = 0; + for (j = 0 ; j < 8 ; j++) { + val8 = pwrpriv->patterns[k].content[i * 8 + j]; + len += snprintf(p_str + len, max_len - len, + "%02x ", val8); + } + RTW_PRINT_SEL(m, "%s\n", p_str); + } + RTW_PRINT_SEL(m, "\npattern mask:\n"); + for (i = 0 ; i < MAX_WKFM_SIZE / 8 ; i++) { + _rtw_memset(p_str, 0, max_len); + len = 0; + for (j = 0 ; j < 8 ; j++) { + val8 = pwrpriv->patterns[k].mask[i * 8 + j]; + len += snprintf(p_str + len, max_len - len, + "%02x ", val8); + } + RTW_PRINT_SEL(m, "%s\n", p_str); + } + + RTW_PRINT_SEL(m, "\npriv_pattern_len:\n"); + RTW_PRINT_SEL(m, "pattern_len: %d\n", pwrpriv->patterns[k].len); + RTW_PRINT_SEL(m, "*****************\n"); + } + + return 0; +} + +ssize_t proc_set_pattern_info(struct file *file, const char __user *buffer, + size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct wowlan_ioctl_param poidparam; + u8 tmp[MAX_WKFM_PATTERN_STR_LEN + 1] = {0}; + int ret = 0; + u8 index = 0; + + poidparam.subcode = 0; + + if (count < 1) + return -EFAULT; + + if (count >= sizeof(tmp)) { + RTW_ERR("%s: pattern string is too long, count=%zu\n", + __func__, count); + return -EFAULT; + } + + if (pwrpriv->wowlan_pattern_idx >= MAX_WKFM_CAM_NUM) { + RTW_ERR("priv-pattern is full(idx: %d)\n", + pwrpriv->wowlan_pattern_idx); + RTW_ERR("please clean priv-pattern first\n"); + return -ENOMEM; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + if (strncmp(tmp, "clean", 5) == 0) { + poidparam.subcode = WOWLAN_PATTERN_CLEAN; + rtw_hal_set_hwreg(padapter, + HW_VAR_WOWLAN, (u8 *)&poidparam); + } else { + index = pwrpriv->wowlan_pattern_idx; + ret = rtw_wowlan_parser_pattern_cmd(tmp, + pwrpriv->patterns[index].content, + &pwrpriv->patterns[index].len, + pwrpriv->patterns[index].mask); + if (ret == _TRUE) + pwrpriv->wowlan_pattern_idx++; + } + } else { + rtw_warn_on(1); + return -EFAULT; + } + + return count; +} + +int proc_get_wakeup_event(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *registry_par = &padapter->registrypriv; + + RTW_PRINT_SEL(m, "wakeup event: %#02x\n", registry_par->wakeup_event); + return 0; +} + +ssize_t proc_set_wakeup_event(struct file *file, const char __user *buffer, + size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + struct registry_priv *registry_par = &padapter->registrypriv; + u32 wakeup_event = 0; + + u8 tmp[8] = {0}; + int num = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) + num = sscanf(tmp, "%u", &wakeup_event); + else + return -EFAULT; + + if (num == 1 && wakeup_event <= 0x0f) { + registry_par->wakeup_event = wakeup_event; + + if (wakeup_event & BIT(1)) + pwrctrlpriv->default_patterns_en = _TRUE; + else + pwrctrlpriv->default_patterns_en = _FALSE; + + rtw_wow_pattern_sw_reset(padapter); + + RTW_INFO("%s: wakeup_event: %#2x, default pattern: %d\n", + __func__, registry_par->wakeup_event, + pwrctrlpriv->default_patterns_en); + } else { + return -EINVAL; + } + + return count; +} + +int proc_get_wakeup_reason(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 val = pwrpriv->wowlan_last_wake_reason; + + RTW_PRINT_SEL(m, "last wake reason: %#02x\n", val); + return 0; +} +#ifdef CONFIG_WOW_KEEP_ALIVE_PATTERN +int proc_dump_wow_keep_alive_info(struct seq_file *m, void *v) { + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + int i; + + RTW_PRINT_SEL(m, "wowlan_keep_alive_mode: %d\n", pwrpriv->wowlan_keep_alive_mode); + RTW_PRINT_SEL(m,"LocKeepAlive: %d\n", pwrpriv->keep_alive_pattern_loc ); + RTW_PRINT_SEL(m, "keep_alive_pattern_len: %d\n", pwrpriv->keep_alive_pattern_len); + RTW_PRINT_SEL(m,"keep_alive_pattern= \n" ); + for (i=0 ; i < pwrpriv->keep_alive_pattern_len ; i++) { + RTW_PRINT_SEL(m,"[0x%x] ",pwrpriv->keep_alive_pattern[i]); + if(i%8 == 7) + RTW_PRINT_SEL(m,"\n"); + } + RTW_PRINT_SEL(m,"\n"); + RTW_PRINT_SEL(m," wowlan_keep_alive_period= %d ms\n", pwrpriv->wowlan_keep_alive_period*100); + RTW_PRINT_SEL(m," wowlan_keep_alive_retry_counter= %d\n", pwrpriv->wowlan_keep_alive_retry_counter); + RTW_PRINT_SEL(m," wowlan_keep_alive_retry_interval= %d ms\n", pwrpriv->wowlan_keep_alive_retry_interval*100); + return 0; +} +#endif /* CONFIG_WOW_KEEP_ALIVE_PATTERN*/ + +#endif /*CONFIG_WOWLAN*/ + +#ifdef CONFIG_GPIO_WAKEUP +int proc_get_wowlan_gpio_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 gpio_index = pwrpriv->wowlan_gpio_index; + u8 gpio_output_state = pwrpriv->wowlan_gpio_output_state; + u8 val = pwrpriv->is_high_active; + + RTW_PRINT_SEL(m, "wakeup_gpio_idx: %d\n", gpio_index); +#if (!defined(CONFIG_WAKEUP_GPIO_INPUT_MODE) && !defined(CONFIG_RTW_ONE_PIN_GPIO)) + RTW_PRINT_SEL(m, "current_gpio_output_state: %d\n", gpio_output_state); +#endif + RTW_PRINT_SEL(m, "high_active: %d\n", val); + + return 0; +} + +ssize_t proc_set_wowlan_gpio_info(struct file *file, const char __user *buffer, + size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + char tmp[32] = {0}; + int num = 0; + u32 is_high_active = 0; + u8 val8 = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + num = sscanf(tmp, "%u", &is_high_active); + + if (num != 1) { + RTW_INFO("Invalid format\n"); + return count; + } + + is_high_active = is_high_active == 0 ? 0 : 1; + + pwrpriv->is_high_active = is_high_active; + + rtw_ps_deny(padapter, PS_DENY_IOCTL); + LeaveAllPowerSaveModeDirect(padapter); + +#ifdef CONFIG_WAKEUP_GPIO_INPUT_MODE + if (pwrpriv->is_high_active == 0) + rtw_hal_set_input_gpio(padapter, pwrpriv->wowlan_gpio_index); + else + rtw_hal_set_output_gpio(padapter, pwrpriv->wowlan_gpio_index, + GPIO_OUTPUT_LOW); +#else + val8 = (pwrpriv->is_high_active == 0) ? 1 : 0; + rtw_hal_switch_gpio_wl_ctrl(padapter, pwrpriv->wowlan_gpio_index, _TRUE); + rtw_hal_set_output_gpio(padapter, pwrpriv->wowlan_gpio_index, val8); +#endif + rtw_ps_deny_cancel(padapter, PS_DENY_IOCTL); + + RTW_INFO("%s set GPIO_%d to %s_ACTIVE\n", __func__, + pwrpriv->wowlan_gpio_index, + pwrpriv->is_high_active ? "HIGH" : "LOW"); + } + + return count; +} +#endif /* CONFIG_GPIO_WAKEUP */ + +#ifdef CONFIG_P2P_WOWLAN +int proc_get_p2p_wowlan_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + struct p2p_wowlan_info peerinfo = pwdinfo->p2p_wow_info; + if (_TRUE == peerinfo.is_trigger) { + RTW_PRINT_SEL(m, "is_trigger: TRUE\n"); + switch (peerinfo.wowlan_recv_frame_type) { + case P2P_WOWLAN_RECV_NEGO_REQ: + RTW_PRINT_SEL(m, "Frame Type: Nego Request\n"); + break; + case P2P_WOWLAN_RECV_INVITE_REQ: + RTW_PRINT_SEL(m, "Frame Type: Invitation Request\n"); + break; + case P2P_WOWLAN_RECV_PROVISION_REQ: + RTW_PRINT_SEL(m, "Frame Type: Provision Request\n"); + break; + default: + break; + } + RTW_PRINT_SEL(m, "Peer Addr: "MAC_FMT"\n", MAC_ARG(peerinfo.wowlan_peer_addr)); + RTW_PRINT_SEL(m, "Peer WPS Config: %x\n", peerinfo.wowlan_peer_wpsconfig); + RTW_PRINT_SEL(m, "Persistent Group: %d\n", peerinfo.wowlan_peer_is_persistent); + RTW_PRINT_SEL(m, "Intivation Type: %d\n", peerinfo.wowlan_peer_invitation_type); + } else + RTW_PRINT_SEL(m, "is_trigger: False\n"); + return 0; +} +#endif /* CONFIG_P2P_WOWLAN */ +#ifdef CONFIG_BCN_CNT_CONFIRM_HDL +int proc_get_new_bcn_max(struct seq_file *m, void *v) +{ + extern int new_bcn_max; + + RTW_PRINT_SEL(m, "%d", new_bcn_max); + return 0; +} + +ssize_t proc_set_new_bcn_max(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + extern int new_bcn_max; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) + sscanf(tmp, "%d ", &new_bcn_max); + + return count; +} +#endif +#ifdef CONFIG_POWER_SAVING +int proc_get_ps_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 ips_mode = pwrpriv->ips_mode_req; + u8 lps_mode = pwrpriv->power_mgnt; + u8 lps_level = pwrpriv->lps_level; +#ifdef CONFIG_LPS_1T1R + u8 lps_1t1r = pwrpriv->lps_1t1r; +#endif +#ifdef CONFIG_WOWLAN + u8 wow_lps_mode = pwrpriv->wowlan_power_mgmt; + u8 wow_lps_level = pwrpriv->wowlan_lps_level; + #ifdef CONFIG_LPS_1T1R + u8 wow_lps_1t1r = pwrpriv->wowlan_lps_1t1r; + #endif +#endif /* CONFIG_WOWLAN */ + char *str = ""; + + RTW_PRINT_SEL(m, "======Power Saving Info:======\n"); + RTW_PRINT_SEL(m, "*IPS:\n"); + + if (ips_mode == IPS_NORMAL) { +#ifdef CONFIG_FWLPS_IN_IPS + str = "FW_LPS_IN_IPS"; +#else + str = "Card Disable"; +#endif + } else if (ips_mode == IPS_NONE) + str = "NO IPS"; + else if (ips_mode == IPS_LEVEL_2) + str = "IPS_LEVEL_2"; + else + str = "invalid ips_mode"; + + RTW_PRINT_SEL(m, " IPS mode: %s\n", str); + RTW_PRINT_SEL(m, " IPS enter count:%d, IPS leave count:%d\n", + pwrpriv->ips_enter_cnts, pwrpriv->ips_leave_cnts); + RTW_PRINT_SEL(m, "------------------------------\n"); + RTW_PRINT_SEL(m, "*LPS:\n"); + + if (lps_mode == PS_MODE_ACTIVE) + str = "NO LPS"; + else if (lps_mode == PS_MODE_MIN) + str = "MIN"; + else if (lps_mode == PS_MODE_MAX) + str = "MAX"; + else if (lps_mode == PS_MODE_DTIM) + str = "DTIM"; + else + sprintf(str, "%d", lps_mode); + + RTW_PRINT_SEL(m, " LPS mode: %s\n", str); + + if (pwrpriv->dtim != 0) + RTW_PRINT_SEL(m, " DTIM: %d\n", pwrpriv->dtim); + RTW_PRINT_SEL(m, " LPS enter count:%d, LPS leave count:%d\n", + pwrpriv->lps_enter_cnts, pwrpriv->lps_leave_cnts); + + if (lps_level == LPS_LCLK) + str = "LPS_LCLK"; + else if (lps_level == LPS_PG) + str = "LPS_PG"; + else + str = "LPS_NORMAL"; + RTW_PRINT_SEL(m, " LPS level: %s\n", str); + +#ifdef CONFIG_LPS_1T1R + RTW_PRINT_SEL(m, " LPS 1T1R: %d\n", lps_1t1r); +#endif + +#ifdef CONFIG_WOWLAN + RTW_PRINT_SEL(m, "------------------------------\n"); + RTW_PRINT_SEL(m, "*WOW LPS:\n"); + + if (wow_lps_mode == PS_MODE_ACTIVE) + str = "NO LPS"; + else if (wow_lps_mode == PS_MODE_MIN) + str = "MIN"; + else if (wow_lps_mode == PS_MODE_MAX) + str = "MAX"; + else if (wow_lps_mode == PS_MODE_DTIM) + str = "DTIM"; + else + sprintf(str, "%d", wow_lps_mode); + + RTW_PRINT_SEL(m, " WOW LPS mode: %s\n", str); + + if (wow_lps_level == LPS_LCLK) + str = "LPS_LCLK"; + else if (wow_lps_level == LPS_PG) + str = "LPS_PG"; + else + str = "LPS_NORMAL"; + RTW_PRINT_SEL(m, " WOW LPS level: %s\n", str); + + #ifdef CONFIG_LPS_1T1R + RTW_PRINT_SEL(m, " WOW LPS 1T1R: %d\n", wow_lps_1t1r); + #endif +#endif /* CONFIG_WOWLAN */ + + RTW_PRINT_SEL(m, "=============================\n"); + return 0; +} + +ssize_t proc_set_ps_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + struct _ADAPTER *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[8]; + int num = 0; + int mode = 0; + int en = 0; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (!buffer || copy_from_user(tmp, buffer, count)) + goto exit; + + num = sscanf(tmp, "%d %d", &mode, &en); + if (num > 2) { + RTW_ERR("%s: invalid parameter!\n", __FUNCTION__); + goto exit; + } + + if (num == 1 && mode == 0) { + /* back to original LPS/IPS Mode */ + RTW_INFO("%s: back to original LPS/IPS Mode\n", __FUNCTION__); + + rtw_pm_set_lps(adapter, adapter->registrypriv.power_mgnt); + + rtw_pm_set_ips(adapter, adapter->registrypriv.ips_mode); + +#ifdef CONFIG_WOWLAN + RTW_INFO("%s: back to original WOW LPS Mode\n", __FUNCTION__); + + rtw_pm_set_wow_lps(adapter, adapter->registrypriv.wow_power_mgnt); +#endif /* CONFIG_WOWLAN */ + + goto exit; + } + + if (mode == 1) { + /* LPS */ + RTW_INFO("%s: LPS: %s, en=%d\n", __FUNCTION__, (en == 0) ? "disable":"enable", en); + if (rtw_pm_set_lps(adapter, en) != 0 ) + RTW_ERR("%s: invalid parameter, mode=%d, level=%d\n", __FUNCTION__, mode, en); + + } else if (mode == 2) { + /* IPS */ + RTW_INFO("%s: IPS: %s, en=%d\n", __FUNCTION__, (en == 0) ? "disable":"enable", en); + if (rtw_pm_set_ips(adapter, en) != 0 ) + RTW_ERR("%s: invalid parameter, mode=%d, level=%d\n", __FUNCTION__, mode, en); + } +#ifdef CONFIG_WOWLAN + else if (mode == 3) { + /* WOW LPS */ + RTW_INFO("%s: WOW LPS: %s, en=%d\n", __FUNCTION__, (en == 0) ? "disable":"enable", en); + if (rtw_pm_set_wow_lps(adapter, en) != 0 ) + RTW_ERR("%s: invalid parameter, mode=%d, level=%d\n", __FUNCTION__, mode, en); + } +#endif /* CONFIG_WOWLAN */ + else + RTW_ERR("%s: invalid parameter, mode = %d!\n", __FUNCTION__, mode); + +exit: + return count; +} + +#ifdef CONFIG_WMMPS_STA +int proc_get_wmmps_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char *uapsd_max_sp_str=""; + + if (pregpriv){ + switch(pregpriv->uapsd_max_sp_len) { + case 0: + uapsd_max_sp_str = "NO_LIMIT"; + break; + case 1: + uapsd_max_sp_str = "TWO_MSDU"; + break; + case 2: + uapsd_max_sp_str = "FOUR_MSDU"; + break; + case 3: + uapsd_max_sp_str = "SIX_MSDU"; + break; + default: + uapsd_max_sp_str = "UNSPECIFIED"; + break; + } + + RTW_PRINT_SEL(m, "====== WMMPS_STA Info:======\n"); + RTW_PRINT_SEL(m, "uapsd_max_sp_len=0x%02x (%s)\n", pregpriv->uapsd_max_sp_len, uapsd_max_sp_str); + RTW_PRINT_SEL(m, "uapsd_ac_enable=0x%02x\n", pregpriv->uapsd_ac_enable); + RTW_PRINT_SEL(m, "BIT0 - AC_VO UAPSD: %s\n", (pregpriv->uapsd_ac_enable & DRV_CFG_UAPSD_VO) ? "Enabled" : "Disabled"); + RTW_PRINT_SEL(m, "BIT1 - AC_VI UAPSD: %s\n", (pregpriv->uapsd_ac_enable & DRV_CFG_UAPSD_VI) ? "Enabled" : "Disabled"); + RTW_PRINT_SEL(m, "BIT2 - AC_BK UAPSD: %s\n", (pregpriv->uapsd_ac_enable & DRV_CFG_UAPSD_BK) ? "Enabled" : "Disabled"); + RTW_PRINT_SEL(m, "BIT3 - AC_BE UAPSD: %s\n", (pregpriv->uapsd_ac_enable & DRV_CFG_UAPSD_BE) ? "Enabled" : "Disabled"); + RTW_PRINT_SEL(m, "============================\n"); + } + + return 0; +} + +ssize_t proc_set_wmmps_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u8 uapsd_ac_setting; + u8 uapsd_max_sp_len_setting; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhu %hhx", &uapsd_max_sp_len_setting, &uapsd_ac_setting); + + if (pregpriv) { + if (num >= 1) { + pregpriv->uapsd_max_sp_len = uapsd_max_sp_len_setting; + RTW_INFO("uapsd_max_sp_len = %d\n", pregpriv->uapsd_max_sp_len); + } + + if (num >= 2) { + pregpriv->uapsd_ac_enable = uapsd_ac_setting; + RTW_INFO("uapsd_ac_enable = 0x%02x\n", pregpriv->uapsd_ac_enable); + } + } + } + + return count; +} +#endif /* CONFIG_WMMPS_STA */ +#endif /* CONFIG_POWER_SAVING */ + +#ifdef CONFIG_TDLS +int proc_get_tdls_enable(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + + if (pregpriv) + RTW_PRINT_SEL(m, "TDLS is %s !\n", (rtw_is_tdls_enabled(padapter) == _TRUE) ? "enabled" : "disabled"); + + return 0; +} + +ssize_t proc_set_tdls_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct registry_priv *pregpriv = &padapter->registrypriv; + char tmp[32]; + u32 en_tdls = 0; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d ", &en_tdls); + + if (num == 1 && pregpriv) { + if (en_tdls > 0) + rtw_enable_tdls_func(padapter); + else + rtw_disable_tdls_func(padapter, _TRUE); + } + } + + return count; +} + +static int proc_tdls_display_tdls_function_info(struct seq_file *m) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + u8 SpaceBtwnItemAndValue = TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE; + u8 SpaceBtwnItemAndValueTmp = 0; + BOOLEAN FirstMatchFound = _FALSE; + int j = 0; + + RTW_PRINT_SEL(m, "============[TDLS Function Info]============\n"); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Enable", (rtw_is_tdls_enabled(padapter) == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Driver Setup", (ptdlsinfo->driver_setup == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Prohibited", (ptdlsinfo->ap_prohibited == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Channel Switch Prohibited", (ptdlsinfo->ch_switch_prohibited == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Link Established", (ptdlsinfo->link_established == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %d/%d\n", SpaceBtwnItemAndValue, "TDLS STA Num (Linked/Allowed)", ptdlsinfo->sta_cnt, MAX_ALLOWED_TDLS_STA_NUM); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Allowed STA Num Reached", (ptdlsinfo->sta_maximum == _TRUE) ? "_TRUE" : "_FALSE"); + +#ifdef CONFIG_TDLS_CH_SW + RTW_PRINT_SEL(m, "%-*s =", SpaceBtwnItemAndValue, "TDLS CH SW State"); + if (ptdlsinfo->chsw_info.ch_sw_state == TDLS_STATE_NONE) + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_STATE_NONE"); + else { + for (j = 0; j < 32; j++) { + if (ptdlsinfo->chsw_info.ch_sw_state & BIT(j)) { + if (FirstMatchFound == _FALSE) { + SpaceBtwnItemAndValueTmp = 1; + FirstMatchFound = _TRUE; + } else + SpaceBtwnItemAndValueTmp = SpaceBtwnItemAndValue + 3; + switch (BIT(j)) { + case TDLS_INITIATOR_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_INITIATOR_STATE"); + break; + case TDLS_RESPONDER_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_RESPONDER_STATE"); + break; + case TDLS_LINKED_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_LINKED_STATE"); + break; + case TDLS_WAIT_PTR_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_WAIT_PTR_STATE"); + break; + case TDLS_ALIVE_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_ALIVE_STATE"); + break; + case TDLS_CH_SWITCH_ON_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SWITCH_ON_STATE"); + break; + case TDLS_PEER_AT_OFF_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_PEER_AT_OFF_STATE"); + break; + case TDLS_CH_SW_INITIATOR_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SW_INITIATOR_STATE"); + break; + case TDLS_WAIT_CH_RSP_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValue, " ", "TDLS_WAIT_CH_RSP_STATE"); + break; + default: + RTW_PRINT_SEL(m, "%-*sBIT(%d)\n", SpaceBtwnItemAndValueTmp, " ", j); + break; + } + } + } + } + + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS CH SW On", (ATOMIC_READ(&ptdlsinfo->chsw_info.chsw_on) == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Off-Channel Num", ptdlsinfo->chsw_info.off_ch_num); + RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Channel Offset", ptdlsinfo->chsw_info.ch_offset); + RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Current Time", ptdlsinfo->chsw_info.cur_time); + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS CH SW Delay Switch Back", (ptdlsinfo->chsw_info.delay_switch_back == _TRUE) ? "_TRUE" : "_FALSE"); + RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Dump Back", ptdlsinfo->chsw_info.dump_stack); +#endif + + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Device Discovered", (ptdlsinfo->dev_discovered == _TRUE) ? "_TRUE" : "_FALSE"); + + return 0; +} + +static int proc_tdls_display_network_info(struct seq_file *m) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + int i = 0; + u8 SpaceBtwnItemAndValue = TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE; + + /* Display the linked AP/GO info */ + RTW_PRINT_SEL(m, "============[Associated AP/GO Info]============\n"); + + if ((pmlmepriv->fw_state & WIFI_STATION_STATE) && (pmlmepriv->fw_state & WIFI_ASOC_STATE)) { + RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "BSSID", cur_network->network.Ssid.Ssid); + RTW_PRINT_SEL(m, "%-*s = "MAC_FMT"\n", SpaceBtwnItemAndValue, "Mac Address", MAC_ARG(cur_network->network.MacAddress)); + + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Wireless Mode"); + for (i = 0; i < 8; i++) { + if (pmlmeext->cur_wireless_mode & BIT(i)) { + switch (BIT(i)) { + case WIRELESS_11B: + RTW_PRINT_SEL(m, "%4s", "11B "); + break; + case WIRELESS_11G: + RTW_PRINT_SEL(m, "%4s", "11G "); + break; + case WIRELESS_11A: + RTW_PRINT_SEL(m, "%4s", "11A "); + break; + case WIRELESS_11_24N: + RTW_PRINT_SEL(m, "%7s", "11_24N "); + break; + case WIRELESS_11_5N: + RTW_PRINT_SEL(m, "%6s", "11_5N "); + break; + case WIRELESS_AUTO: + RTW_PRINT_SEL(m, "%5s", "AUTO "); + break; + case WIRELESS_11AC: + RTW_PRINT_SEL(m, "%5s", "11AC "); + break; + } + } + } + RTW_PRINT_SEL(m, "\n"); + + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Privacy"); + switch (padapter->securitypriv.dot11PrivacyAlgrthm) { + case _NO_PRIVACY_: + RTW_PRINT_SEL(m, "%s\n", "NO PRIVACY"); + break; + case _WEP40_: + RTW_PRINT_SEL(m, "%s\n", "WEP 40"); + break; + case _TKIP_: + RTW_PRINT_SEL(m, "%s\n", "TKIP"); + break; + case _TKIP_WTMIC_: + RTW_PRINT_SEL(m, "%s\n", "TKIP WTMIC"); + break; + case _AES_: + RTW_PRINT_SEL(m, "%s\n", "AES"); + break; + case _WEP104_: + RTW_PRINT_SEL(m, "%s\n", "WEP 104"); + break; +#if 0 /* no this setting */ + case _WEP_WPA_MIXED_: + RTW_PRINT_SEL(m, "%s\n", "WEP/WPA Mixed"); + break; +#endif + case _SMS4_: + RTW_PRINT_SEL(m, "%s\n", "SMS4"); + break; +#ifdef CONFIG_IEEE80211W + case _BIP_CMAC_128_: + RTW_PRINT_SEL(m, "%s\n", "BIP"); + break; +#endif /* CONFIG_IEEE80211W */ + } + + RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "Channel", pmlmeext->cur_channel); + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Channel Offset"); + switch (pmlmeext->cur_ch_offset) { + case HAL_PRIME_CHNL_OFFSET_DONT_CARE: + RTW_PRINT_SEL(m, "%s\n", "N/A"); + break; + case HAL_PRIME_CHNL_OFFSET_LOWER: + RTW_PRINT_SEL(m, "%s\n", "Lower"); + break; + case HAL_PRIME_CHNL_OFFSET_UPPER: + RTW_PRINT_SEL(m, "%s\n", "Upper"); + break; + } + + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Bandwidth Mode"); + switch (pmlmeext->cur_bwmode) { + case CHANNEL_WIDTH_20: + RTW_PRINT_SEL(m, "%s\n", "20MHz"); + break; + case CHANNEL_WIDTH_40: + RTW_PRINT_SEL(m, "%s\n", "40MHz"); + break; + case CHANNEL_WIDTH_80: + RTW_PRINT_SEL(m, "%s\n", "80MHz"); + break; + case CHANNEL_WIDTH_160: + RTW_PRINT_SEL(m, "%s\n", "160MHz"); + break; + case CHANNEL_WIDTH_80_80: + RTW_PRINT_SEL(m, "%s\n", "80MHz + 80MHz"); + break; + } + } else + RTW_PRINT_SEL(m, "No association with AP/GO exists!\n"); + + return 0; +} + +static int proc_tdls_display_tdls_sta_info(struct seq_file *m) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct sta_priv *pstapriv = &padapter->stapriv; + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct sta_info *psta; + int i = 0, j = 0; + _irqL irqL; + _list *plist, *phead; + u8 SpaceBtwnItemAndValue = TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE; + u8 SpaceBtwnItemAndValueTmp = 0; + u8 NumOfTdlsStaToShow = 0; + BOOLEAN FirstMatchFound = _FALSE; + + /* Search for TDLS sta info to display */ + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + if (psta->tdls_sta_state != TDLS_STATE_NONE) { + /* We got one TDLS sta info to show */ + RTW_PRINT_SEL(m, "============[TDLS Peer STA Info: STA %d]============\n", ++NumOfTdlsStaToShow); + RTW_PRINT_SEL(m, "%-*s = "MAC_FMT"\n", SpaceBtwnItemAndValue, "Mac Address", MAC_ARG(psta->cmn.mac_addr)); + RTW_PRINT_SEL(m, "%-*s =", SpaceBtwnItemAndValue, "TDLS STA State"); + SpaceBtwnItemAndValueTmp = 0; + FirstMatchFound = _FALSE; + for (j = 0; j < 32; j++) { + if (psta->tdls_sta_state & BIT(j)) { + if (FirstMatchFound == _FALSE) { + SpaceBtwnItemAndValueTmp = 1; + FirstMatchFound = _TRUE; + } else + SpaceBtwnItemAndValueTmp = SpaceBtwnItemAndValue + 3; + switch (BIT(j)) { + case TDLS_INITIATOR_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_INITIATOR_STATE"); + break; + case TDLS_RESPONDER_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_RESPONDER_STATE"); + break; + case TDLS_LINKED_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_LINKED_STATE"); + break; + case TDLS_WAIT_PTR_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_WAIT_PTR_STATE"); + break; + case TDLS_ALIVE_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_ALIVE_STATE"); + break; + case TDLS_CH_SWITCH_ON_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SWITCH_ON_STATE"); + break; + case TDLS_PEER_AT_OFF_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_PEER_AT_OFF_STATE"); + break; + case TDLS_CH_SW_INITIATOR_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SW_INITIATOR_STATE"); + break; + case TDLS_WAIT_CH_RSP_STATE: + RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValue, " ", "TDLS_WAIT_CH_RSP_STATE"); + break; + default: + RTW_PRINT_SEL(m, "%-*sBIT(%d)\n", SpaceBtwnItemAndValueTmp, " ", j); + break; + } + } + } + + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Wireless Mode"); + for (j = 0; j < 8; j++) { + if (psta->wireless_mode & BIT(j)) { + switch (BIT(j)) { + case WIRELESS_11B: + RTW_PRINT_SEL(m, "%4s", "11B "); + break; + case WIRELESS_11G: + RTW_PRINT_SEL(m, "%4s", "11G "); + break; + case WIRELESS_11A: + RTW_PRINT_SEL(m, "%4s", "11A "); + break; + case WIRELESS_11_24N: + RTW_PRINT_SEL(m, "%7s", "11_24N "); + break; + case WIRELESS_11_5N: + RTW_PRINT_SEL(m, "%6s", "11_5N "); + break; + case WIRELESS_AUTO: + RTW_PRINT_SEL(m, "%5s", "AUTO "); + break; + case WIRELESS_11AC: + RTW_PRINT_SEL(m, "%5s", "11AC "); + break; + } + } + } + RTW_PRINT_SEL(m, "\n"); + + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Bandwidth Mode"); + switch (psta->cmn.bw_mode) { + case CHANNEL_WIDTH_20: + RTW_PRINT_SEL(m, "%s\n", "20MHz"); + break; + case CHANNEL_WIDTH_40: + RTW_PRINT_SEL(m, "%s\n", "40MHz"); + break; + case CHANNEL_WIDTH_80: + RTW_PRINT_SEL(m, "%s\n", "80MHz"); + break; + case CHANNEL_WIDTH_160: + RTW_PRINT_SEL(m, "%s\n", "160MHz"); + break; + case CHANNEL_WIDTH_80_80: + RTW_PRINT_SEL(m, "%s\n", "80MHz + 80MHz"); + break; + case CHANNEL_WIDTH_5: + RTW_PRINT_SEL(m, "%s\n", "5MHz"); + break; + case CHANNEL_WIDTH_10: + RTW_PRINT_SEL(m, "%s\n", "10MHz"); + break; + default: + RTW_PRINT_SEL(m, "(%d)%s\n", psta->cmn.bw_mode, "invalid"); + break; + } + + RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Privacy"); + switch (psta->dot118021XPrivacy) { + case _NO_PRIVACY_: + RTW_PRINT_SEL(m, "%s\n", "NO PRIVACY"); + break; + case _WEP40_: + RTW_PRINT_SEL(m, "%s\n", "WEP 40"); + break; + case _TKIP_: + RTW_PRINT_SEL(m, "%s\n", "TKIP"); + break; + case _TKIP_WTMIC_: + RTW_PRINT_SEL(m, "%s\n", "TKIP WTMIC"); + break; + case _AES_: + RTW_PRINT_SEL(m, "%s\n", "AES"); + break; + case _WEP104_: + RTW_PRINT_SEL(m, "%s\n", "WEP 104"); + break; +#if 0 /* no this setting */ + case _WEP_WPA_MIXED_: + RTW_PRINT_SEL(m, "%s\n", "WEP/WPA Mixed"); + break; +#endif + case _SMS4_: + RTW_PRINT_SEL(m, "%s\n", "SMS4"); + break; +#ifdef CONFIG_IEEE80211W + case _BIP_CMAC_128_: + RTW_PRINT_SEL(m, "%s\n", "BIP"); + break; +#endif /* CONFIG_IEEE80211W */ + } + + RTW_PRINT_SEL(m, "%-*s = %d sec/%d sec\n", SpaceBtwnItemAndValue, "TPK Lifetime (Current/Expire)", psta->TPK_count, psta->TDLS_PeerKey_Lifetime); + RTW_PRINT_SEL(m, "%-*s = %llu\n", SpaceBtwnItemAndValue, "Tx Packets Over Direct Link", psta->sta_stats.tx_pkts); + RTW_PRINT_SEL(m, "%-*s = %llu\n", SpaceBtwnItemAndValue, "Rx Packets Over Direct Link", psta->sta_stats.rx_data_pkts); + } + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + if (NumOfTdlsStaToShow == 0) { + RTW_PRINT_SEL(m, "============[TDLS Peer STA Info]============\n"); + RTW_PRINT_SEL(m, "No TDLS direct link exists!\n"); + } + + return 0; +} + +int proc_get_tdls_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct sta_priv *pstapriv = &padapter->stapriv; + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct sta_info *psta; + int i = 0, j = 0; + _irqL irqL; + _list *plist, *phead; + u8 SpaceBtwnItemAndValue = 41; + u8 SpaceBtwnItemAndValueTmp = 0; + u8 NumOfTdlsStaToShow = 0; + BOOLEAN FirstMatchFound = _FALSE; + + if (hal_chk_wl_func(padapter, WL_FUNC_TDLS) == _FALSE) { + RTW_PRINT_SEL(m, "No tdls info can be shown since hal doesn't support tdls\n"); + return 0; + } + + proc_tdls_display_tdls_function_info(m); + proc_tdls_display_network_info(m); + proc_tdls_display_tdls_sta_info(m); + + return 0; +} +#endif + +int proc_get_monitor(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + if (MLME_IS_MONITOR(padapter)) { + RTW_PRINT_SEL(m, "Monitor mode : Enable\n"); + RTW_PRINT_SEL(m, "Device type : %u\n", dev->type); + + RTW_PRINT_SEL(m, "ch=%d, ch_offset=%d, bw=%d\n", + rtw_get_oper_ch(padapter), + rtw_get_oper_choffset(padapter), + rtw_get_oper_bw(padapter)); + } else + RTW_PRINT_SEL(m, "Monitor mode : Disable\n"); + + return 0; +} + +ssize_t proc_set_monitor(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + u16 target_type; + u8 target_ch, target_offset, target_bw; + + if (count < 3) { + RTW_INFO("argument size is less than 3\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = 0; + + num = sscanf(tmp, "type %hu", &target_type); + if ((num == 1) && + ((target_type != ARPHRD_IEEE80211) && + (target_type != ARPHRD_IEEE80211_RADIOTAP))) { + dev->type = ARPHRD_IEEE80211_RADIOTAP; + return count; + } + + num = sscanf(tmp, "%hhu %hhu %hhu", &target_ch, &target_offset, &target_bw); + if (num != 3) { + RTW_INFO("invalid write_reg parameter!\n"); + return count; + } + + padapter->mlmeextpriv.cur_channel = target_ch; + set_channel_bwmode(padapter, target_ch, target_offset, target_bw); + } + + return count; +} + +#ifdef RTW_SIMPLE_CONFIG +/* For RtwSimleConfig */ +int proc_get_simple_config(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "RTW Simple Config : %s\n", padapter->rtw_simple_config ? "Enable" : "Disable"); + + return 0; +} + +ssize_t proc_set_simple_config(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + char tmp[32]; + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + u8 ret; + + if (count < 1) { + RTW_INFO("argument size is less than 1\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%hhd", &ret); + + padapter->rtw_simple_config = ret ? _TRUE : _FALSE; + } + + return count; +} +#endif + +#ifdef DBG_XMIT_BLOCK +int proc_get_xmit_block(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + dump_xmit_block(m, padapter); + + return 0; +} + +ssize_t proc_set_xmit_block(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u8 xb_mode, xb_reason; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%hhx %hhx", &xb_mode, &xb_reason); + + if (num != 2) { + RTW_INFO("invalid parameter!\n"); + return count; + } + + if (xb_mode == 0)/*set*/ + rtw_set_xmit_block(padapter, xb_reason); + else if (xb_mode == 1)/*clear*/ + rtw_clr_xmit_block(padapter, xb_reason); + else + RTW_INFO("invalid parameter!\n"); + } + + return count; +} +#endif + +#include +int proc_get_efuse_map(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal; + int i, j; + u8 ips_mode = IPS_NUM; + u16 mapLen; + + EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, _FALSE); + if (mapLen > EFUSE_MAX_MAP_LEN) + mapLen = EFUSE_MAX_MAP_LEN; + + ips_mode = pwrctrlpriv->ips_mode; + rtw_pm_set_ips(padapter, IPS_NONE); +#ifdef CONFIG_EFUSE_CONFIG_FILE + if (pHalData->efuse_file_status == EFUSE_FILE_LOADED) { + RTW_PRINT_SEL(m, "File eFuse Map loaded! file path:%s\nDriver eFuse Map From File\n", EFUSE_MAP_PATH); + if (pHalData->bautoload_fail_flag) + RTW_PRINT_SEL(m, "File Autoload fail!!!\n"); + } else if (pHalData->efuse_file_status == EFUSE_FILE_FAILED) { + RTW_PRINT_SEL(m, "Open File eFuse Map Fail ! file path:%s\nDriver eFuse Map From Default\n", EFUSE_MAP_PATH); + if (pHalData->bautoload_fail_flag) + RTW_PRINT_SEL(m, "HW Autoload fail!!!\n"); + } else +#endif + + { +#ifdef CONFIG_MP_INCLUDED + if (rtw_mp_mode_check(padapter)) { + PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal; + + if (GET_EFUSE_UPDATE_ON(padapter)) + RTW_PRINT_SEL(m, "Driver eFuse Map From fake\n"); + else + RTW_PRINT_SEL(m, "Driver eFuse Map From HW\n"); + } else +#endif + { + RTW_PRINT_SEL(m, "Driver eFuse Map From HW\n"); + + if (pHalData->bautoload_fail_flag) + RTW_PRINT_SEL(m, "HW Autoload fail!!!\n"); + } + } + for (i = 0; i < mapLen; i += 16) { + RTW_PRINT_SEL(m, "0x%02x\t", i); + for (j = 0; j < 8; j++) + RTW_PRINT_SEL(m, "%02X ", pHalData->efuse_eeprom_data[i + j]); + RTW_PRINT_SEL(m, "\t"); + for (; j < 16; j++) + RTW_PRINT_SEL(m, "%02X ", pHalData->efuse_eeprom_data[i + j]); + RTW_PRINT_SEL(m, "\n"); + } + + if (rtw_efuse_map_read(padapter, 0, mapLen, pEfuseHal->fakeEfuseInitMap) == _FAIL) { + RTW_PRINT_SEL(m, "WARN - Read Realmap Failed\n"); + return 0; + } + + RTW_PRINT_SEL(m, "\n"); + RTW_PRINT_SEL(m, "HW eFuse Map\n"); + for (i = 0; i < mapLen; i += 16) { + RTW_PRINT_SEL(m, "0x%02x\t", i); + for (j = 0; j < 8; j++) + RTW_PRINT_SEL(m, "%02X ", pEfuseHal->fakeEfuseInitMap[i + j]); + RTW_PRINT_SEL(m, "\t"); + for (; j < 16; j++) + RTW_PRINT_SEL(m, "%02X ", pEfuseHal->fakeEfuseInitMap[i + j]); + RTW_PRINT_SEL(m, "\n"); + } + + rtw_pm_set_ips(padapter, ips_mode); + + return 0; +} + +ssize_t proc_set_efuse_map(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ +#if 0 + char tmp[256] = {0}; + u32 addr, cnts; + u8 efuse_data; + + int jj, kk; + + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + u8 ips_mode = IPS_NUM; + + if (count < 3) { + RTW_INFO("argument size is less than 3\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%x %d %x", &addr, &cnts, &efuse_data); + + if (num != 3) { + RTW_INFO("invalid write_reg parameter!\n"); + return count; + } + } + ips_mode = pwrctrlpriv->ips_mode; + rtw_pm_set_ips(padapter, IPS_NONE); + if (rtw_efuse_map_write(padapter, addr, cnts, &efuse_data) == _FAIL) + RTW_INFO("WARN - rtw_efuse_map_write error!!\n"); + rtw_pm_set_ips(padapter, ips_mode); +#endif + return count; +} + +#ifdef CONFIG_IEEE80211W +ssize_t proc_set_tx_sa_query(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct sta_info *psta; + _list *plist, *phead; + _irqL irqL; + char tmp[16]; + u8 mac_addr[NUM_STA][ETH_ALEN]; + u32 key_type; + u8 index; + + if (count > 2) { + RTW_INFO("argument size is more than 2\n"); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%x", &key_type); + + if (num != 1) { + RTW_INFO("invalid read_reg parameter!\n"); + return count; + } + RTW_INFO("0: set sa query request , key_type=%d\n", key_type); + } + + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) + && (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) && SEC_IS_BIP_KEY_INSTALLED(&padapter->securitypriv) == _TRUE) { + RTW_INFO("STA:"MAC_FMT"\n", MAC_ARG(get_my_bssid(&(pmlmeinfo->network)))); + /* TX unicast sa_query to AP */ + issue_action_SA_Query(padapter, get_my_bssid(&(pmlmeinfo->network)), 0, 0, (u8)key_type); + } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE && SEC_IS_BIP_KEY_INSTALLED(&padapter->securitypriv) == _TRUE) { + /* TX unicast sa_query to every client STA */ + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (index = 0; index < NUM_STA; index++) { + psta = NULL; + + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + _rtw_memcpy(&mac_addr[psta->cmn.mac_id][0], psta->cmn.mac_addr, ETH_ALEN); + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (index = 0; index < macid_ctl->num && index < NUM_STA; index++) { + if (rtw_macid_is_used(macid_ctl, index) && !rtw_macid_is_bmc(macid_ctl, index)) { + if (!_rtw_memcmp(get_my_bssid(&(pmlmeinfo->network)), &mac_addr[index][0], ETH_ALEN) + && !IS_MCAST(&mac_addr[index][0])) { + issue_action_SA_Query(padapter, &mac_addr[index][0], 0, 0, (u8)key_type); + RTW_INFO("STA[%u]:"MAC_FMT"\n", index , MAC_ARG(&mac_addr[index][0])); + } + } + } + } + + return count; +} + +int proc_get_tx_sa_query(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "%s\n", __func__); + return 0; +} + +ssize_t proc_set_tx_deauth(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct sta_info *psta; + _list *plist, *phead; + _irqL irqL; + char tmp[16]; + u8 mac_addr[NUM_STA][ETH_ALEN]; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u32 key_type; + u8 index; + + + if (count > 2) { + RTW_INFO("argument size is more than 2\n"); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%x", &key_type); + + if (num != 1) { + RTW_INFO("invalid read_reg parameter!\n"); + return count; + } + RTW_INFO("key_type=%d\n", key_type); + } + if (key_type < 0 || key_type > 4) + return count; + + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) + && (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE)) { + if (key_type == 3) /* key_type 3 only for AP mode */ + return count; + /* TX unicast deauth to AP */ + issue_deauth_11w(padapter, get_my_bssid(&(pmlmeinfo->network)), 0, (u8)key_type); +#ifdef CONFIG_AP_MODE + } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE) { + u8 updated = _FALSE; + + if (key_type == 3) + issue_deauth_11w(padapter, bc_addr, 0, IEEE80211W_RIGHT_KEY); + + /* TX unicast deauth to every client STA */ + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (index = 0; index < NUM_STA; index++) { + psta = NULL; + + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + _rtw_memcpy(&mac_addr[psta->cmn.mac_id][0], psta->cmn.mac_addr, ETH_ALEN); + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (index = 0; index < macid_ctl->num && index < NUM_STA; index++) { + if (rtw_macid_is_used(macid_ctl, index) && !rtw_macid_is_bmc(macid_ctl, index)) { + if (!_rtw_memcmp(get_my_bssid(&(pmlmeinfo->network)), &mac_addr[index][0], ETH_ALEN)) { + if (key_type != 3) + issue_deauth_11w(padapter, &mac_addr[index][0], 0, (u8)key_type); + + psta = rtw_get_stainfo(pstapriv, &mac_addr[index][0]); + if (psta && key_type != IEEE80211W_WRONG_KEY && key_type != IEEE80211W_NO_KEY) { + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&psta->asoc_list) == _FALSE) { + rtw_list_delete(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + updated |= ap_free_sta(padapter, psta, _FALSE, WLAN_REASON_PREV_AUTH_NOT_VALID, _TRUE); + + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + } + + RTW_INFO("STA[%u]:"MAC_FMT"\n", index , MAC_ARG(&mac_addr[index][0])); + } + } + } + + associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL); +#endif /* CONFIG_AP_MODE */ + } + + return count; +} + +int proc_get_tx_deauth(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "%s\n", __func__); + return 0; +} + +ssize_t proc_set_tx_auth(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct sta_info *psta; + _list *plist, *phead; + _irqL irqL; + char tmp[16]; + u8 mac_addr[NUM_STA][ETH_ALEN]; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u32 tx_auth; + u8 index; + + + if (count > 2) { + RTW_INFO("argument size is more than 2\n"); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) { + + int num = sscanf(tmp, "%x", &tx_auth); + + if (num != 1) { + RTW_INFO("invalid read_reg parameter!\n"); + return count; + } + RTW_INFO("1: setnd auth, 2: send assoc request. tx_auth=%d\n", tx_auth); + } + + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) + && (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE)) { + if (tx_auth == 1) { + /* TX unicast auth to AP */ + issue_auth(padapter, NULL, 0); + } else if (tx_auth == 2) { + /* TX unicast auth to AP */ + issue_assocreq(padapter); + } + } + + return count; +} + +int proc_get_tx_auth(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "%s\n", __func__); + return 0; +} +#endif /* CONFIG_IEEE80211W */ + +#ifdef CONFIG_CUSTOMER01_SMART_ANTENNA +static u32 phase_idx; +int proc_get_pathb_phase(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "PathB phase index =%d\n", phase_idx); + return 0; +} + +ssize_t proc_set_pathb_phase(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + int num; + u32 tmp_idx; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + num = sscanf(tmp, "%u", &tmp_idx); + if ((tmp_idx < 0) || (tmp_idx > 11)) { + RTW_INFO(FUNC_ADPT_FMT "Invalid input value\n", FUNC_ADPT_ARG(padapter)); + return count; + } + phase_idx = tmp_idx; + rtw_hal_set_pathb_phase(padapter, phase_idx); + } + return count; +} +#endif + +#ifdef CONFIG_MCC_MODE +int proc_get_mcc_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + dump_adapters_status(m, adapter_to_dvobj(adapter)); + rtw_hal_dump_mcc_info(m, adapter_to_dvobj(adapter)); + return 0; +} + +int proc_get_mcc_policy_table(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + rtw_hal_dump_mcc_policy_table(m); + return 0; +} + +ssize_t proc_set_mcc_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 en_mcc = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + _adapter *iface = NULL; + u8 i = 0; + int num = sscanf(tmp, "%u", &en_mcc); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: en_mcc = %d\n", __func__, en_mcc); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + iface->registrypriv.en_mcc = en_mcc; + } + } + + return count; +} + +ssize_t proc_set_mcc_duration(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 enable_runtime_duration = 0, mcc_duration = 0, type = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u %u %u", &enable_runtime_duration, &type, &mcc_duration); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + if (num > 3) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters > 2\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + if (num == 2) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters > 2\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + if (num >= 1) { + SET_MCC_RUNTIME_DURATION(padapter, enable_runtime_duration); + RTW_INFO("runtime duration:%s\n", enable_runtime_duration ? "enable":"disable"); + } + + if (num == 3) { + RTW_INFO("type:%d, mcc duration:%d\n", type, mcc_duration); + rtw_set_mcc_duration_cmd(padapter, type, mcc_duration); + } + } + + return count; +} + +#ifdef CONFIG_MCC_PHYDM_OFFLOAD +ssize_t proc_set_mcc_phydm_offload_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_phydm_enable = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + u8 i = 0; + int num = sscanf(tmp, "%u", &mcc_phydm_enable); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc phydm enable = %d\n", __func__, mcc_phydm_enable); + rtw_set_mcc_phydm_offload_enable_cmd(padapter, mcc_phydm_enable, _TRUE); + } + + return count; +} +#endif + +ssize_t proc_set_mcc_single_tx_criteria(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_single_tx_criteria = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + _adapter *iface = NULL; + u8 i = 0; + int num = sscanf(tmp, "%u", &mcc_single_tx_criteria); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_single_tx_criteria = %d\n", __func__, mcc_single_tx_criteria); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + iface->registrypriv.rtw_mcc_single_tx_cri = mcc_single_tx_criteria; + } + + + } + + return count; +} + + +ssize_t proc_set_mcc_ap_bw20_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_ap_bw20_target_tp = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &mcc_ap_bw20_target_tp); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_ap_bw20_target_tp = %d\n", __func__, mcc_ap_bw20_target_tp); + + padapter->registrypriv.rtw_mcc_ap_bw20_target_tx_tp = mcc_ap_bw20_target_tp; + + + } + + return count; +} + +ssize_t proc_set_mcc_ap_bw40_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_ap_bw40_target_tp = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &mcc_ap_bw40_target_tp); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_ap_bw40_target_tp = %d\n", __func__, mcc_ap_bw40_target_tp); + + padapter->registrypriv.rtw_mcc_ap_bw40_target_tx_tp = mcc_ap_bw40_target_tp; + + + } + + return count; +} + +ssize_t proc_set_mcc_ap_bw80_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_ap_bw80_target_tp = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &mcc_ap_bw80_target_tp); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_ap_bw80_target_tp = %d\n", __func__, mcc_ap_bw80_target_tp); + + padapter->registrypriv.rtw_mcc_ap_bw80_target_tx_tp = mcc_ap_bw80_target_tp; + + + } + + return count; +} + +ssize_t proc_set_mcc_sta_bw20_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_sta_bw20_target_tp = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &mcc_sta_bw20_target_tp); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_sta_bw20_target_tp = %d\n", __func__, mcc_sta_bw20_target_tp); + + padapter->registrypriv.rtw_mcc_sta_bw20_target_tx_tp = mcc_sta_bw20_target_tp; + + + } + + return count; +} + +ssize_t proc_set_mcc_sta_bw40_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_sta_bw40_target_tp = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &mcc_sta_bw40_target_tp); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_sta_bw40_target_tp = %d\n", __func__, mcc_sta_bw40_target_tp); + + padapter->registrypriv.rtw_mcc_sta_bw40_target_tx_tp = mcc_sta_bw40_target_tp; + + + } + + return count; +} + +ssize_t proc_set_mcc_sta_bw80_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[255]; + u32 mcc_sta_bw80_target_tp = 0; + + if (NULL == buffer) { + RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter)); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter)); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &mcc_sta_bw80_target_tp); + + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } + + RTW_INFO("%s: mcc_sta_bw80_target_tp = %d\n", __func__, mcc_sta_bw80_target_tp); + + padapter->registrypriv.rtw_mcc_sta_bw80_target_tx_tp = mcc_sta_bw80_target_tp; + + + } + + return count; +} +#endif /* CONFIG_MCC_MODE */ + +int proc_get_ack_timeout(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + u8 ack_timeout_val; +#ifdef CONFIG_RTL8821C + u8 ack_timeout_val_cck; +#endif + + ack_timeout_val = rtw_read8(padapter, REG_ACKTO); + +#ifdef CONFIG_RTL8821C + ack_timeout_val_cck = rtw_read8(padapter, REG_ACKTO_CCK_8821C); + RTW_PRINT_SEL(m, "Current CCK packet ACK Timeout = %d us (0x%x).\n", ack_timeout_val_cck, ack_timeout_val_cck); + RTW_PRINT_SEL(m, "Current non-CCK packet ACK Timeout = %d us (0x%x).\n", ack_timeout_val, ack_timeout_val); +#else + RTW_PRINT_SEL(m, "Current ACK Timeout = %d us (0x%x).\n", ack_timeout_val, ack_timeout_val); +#endif + + return 0; +} + +ssize_t proc_set_ack_timeout(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 ack_timeout_ms, ack_timeout_ms_cck; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u %u", &ack_timeout_ms, &ack_timeout_ms_cck); + +#ifdef CONFIG_RTL8821C + if (num < 2) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 2\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } +#else + if (num < 1) { + RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter)); + return -EINVAL; + } +#endif + /* This register sets the Ack time out value after Tx unicast packet. It is in units of us. */ + rtw_write8(padapter, REG_ACKTO, (u8)ack_timeout_ms); + +#ifdef CONFIG_RTL8821C + /* This register sets the Ack time out value after Tx unicast CCK packet. It is in units of us. */ + rtw_write8(padapter, REG_ACKTO_CCK_8821C, (u8)ack_timeout_ms_cck); + RTW_INFO("Set CCK packet ACK Timeout to %d us.\n", ack_timeout_ms_cck); + RTW_INFO("Set non-CCK packet ACK Timeout to %d us.\n", ack_timeout_ms); +#else + RTW_INFO("Set ACK Timeout to %d us.\n", ack_timeout_ms); +#endif + } + + return count; +} + +ssize_t proc_set_fw_offload(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + _adapter *pri_adapter = GET_PRIMARY_ADAPTER(adapter); + HAL_DATA_TYPE *hal = GET_HAL_DATA(adapter); + char tmp[32]; + u32 iqk_offload_enable = 0, ch_switch_offload_enable = 0; + + if (buffer == NULL) { + RTW_INFO("input buffer is NULL!\n"); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO("input length is 0!\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO("input length is too large\n"); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%d %d", &iqk_offload_enable, &ch_switch_offload_enable); + + if (num < 2) { + RTW_INFO("input parameters < 1\n"); + return -EINVAL; + } + + if (hal->RegIQKFWOffload != iqk_offload_enable) { + hal->RegIQKFWOffload = iqk_offload_enable; + rtw_run_in_thread_cmd(pri_adapter, ((void *)(rtw_hal_update_iqk_fw_offload_cap)), pri_adapter); + } + + if (hal->ch_switch_offload != ch_switch_offload_enable) + hal->ch_switch_offload = ch_switch_offload_enable; + } + + return count; +} + +int proc_get_fw_offload(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + HAL_DATA_TYPE *hal = GET_HAL_DATA(adapter); + + + RTW_PRINT_SEL(m, "IQK FW offload:%s\n", hal->RegIQKFWOffload?"enable":"disable"); + RTW_PRINT_SEL(m, "Channel switch FW offload:%s\n", hal->ch_switch_offload?"enable":"disable"); + return 0; +} +#ifdef CONFIG_FW_HANDLE_TXBCN +extern void rtw_hal_set_fw_ap_bcn_offload_cmd(_adapter *adapter, bool fw_bcn_en, u8 tbtt_rpt_map); +ssize_t proc_set_fw_tbtt_rpt(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 fw_tbtt_rpt, fw_bcn_offload; + + + if (buffer == NULL) { + RTW_INFO("input buffer is NULL!\n"); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO("input length is 0!\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO("input length is too large\n"); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%d %x",&fw_bcn_offload, &fw_tbtt_rpt); + + if (num < 2) { + RTW_INFO("input parameters < 2\n"); + return -EINVAL; + } + rtw_hal_set_fw_ap_bcn_offload_cmd(adapter, fw_bcn_offload, fw_tbtt_rpt); + } + + return count; +} + +int proc_get_fw_tbtt_rpt(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + + RTW_PRINT_SEL(m, "FW BCN offload:%s\n", dvobj->fw_bcn_offload ? "enable" : "disable"); + RTW_PRINT_SEL(m, "FW TBTT RPT:%x\n", dvobj->vap_tbtt_rpt_map); + return 0; +} + +#endif + +#ifdef CONFIG_CTRL_TXSS_BY_TP +ssize_t proc_set_txss_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + char tmp[32]; + u32 enable = 0; + u32 txss_tx_tp = 0; + int txss_chk_cnt = 0; + + if (buffer == NULL) { + RTW_INFO("input buffer is NULL!\n"); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO("input length is 0!\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO("input length is too large\n"); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u %u %d", + &enable, &txss_tx_tp, &txss_chk_cnt); + + if (num < 1) { + RTW_INFO("input parameters < 1\n"); + return -EINVAL; + } + pmlmeext->txss_ctrl_en = enable; + + if (txss_tx_tp) + pmlmeext->txss_tp_th = txss_tx_tp; + if (txss_chk_cnt) + pmlmeext->txss_tp_chk_cnt = txss_chk_cnt; + + RTW_INFO("%s txss_ctl_en :%s , txss_tp_th:%d, tp_chk_cnt:%d\n", + __func__, pmlmeext->txss_tp_th ? "Y" : "N", + pmlmeext->txss_tp_th, pmlmeext->txss_tp_chk_cnt); + + } + + return count; +} + +int proc_get_txss_tp(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + RTW_PRINT_SEL(m, "TXSS Control - %s\n", pmlmeext->txss_ctrl_en ? "enable" : "disable"); + RTW_PRINT_SEL(m, "TXSS Tx TP TH - %d\n", pmlmeext->txss_tp_th); + RTW_PRINT_SEL(m, "TXSS check cnt - %d\n", pmlmeext->txss_tp_chk_cnt); + + return 0; +} +#ifdef DBG_CTRL_TXSS +ssize_t proc_set_txss_ctrl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + char tmp[32]; + u32 tx_1ss = 0; + + if (buffer == NULL) { + RTW_INFO("input buffer is NULL!\n"); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO("input length is 0!\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO("input length is too large\n"); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u", &tx_1ss); + + if (num < 1) { + RTW_INFO("input parameters < 1\n"); + return -EINVAL; + } + + pmlmeext->txss_ctrl_en = _FALSE; + + dbg_ctrl_txss(adapter, tx_1ss); + + RTW_INFO("%s set tx to 1ss :%s\n", __func__, tx_1ss ? "Y" : "N"); + } + + return count; +} + +int proc_get_txss_ctrl(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + RTW_PRINT_SEL(m, "TXSS 1ss - %s\n", pmlmeext->txss_1ss ? "Y" : "N"); + + return 0; +} +#endif +#endif + +#ifdef CONFIG_DBG_RF_CAL +int proc_get_iqk_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + return 0; +} + +ssize_t proc_set_iqk(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 recovery, clear, segment; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d %d %d", &recovery, &clear, &segment); + + if (num != 3) { + RTW_INFO("Invalid format\n"); + return count; + } + + rtw_hal_iqk_test(padapter, recovery, clear, segment); + } + + return count; + +} + +int proc_get_lck_info(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + + return 0; +} + +ssize_t proc_set_lck(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + char tmp[32]; + u32 trigger; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + + int num = sscanf(tmp, "%d", &trigger); + + if (num != 1) { + RTW_INFO("Invalid format\n"); + return count; + } + + rtw_hal_lck_test(padapter); + } + + return count; +} +#endif /* CONFIG_DBG_RF_CAL */ + +#ifdef CONFIG_LPS_CHK_BY_TP +ssize_t proc_set_lps_chk_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + char tmp[32]; + u32 enable = 0; + u32 lps_tx_tp = 0, lps_rx_tp = 0, lps_bi_tp = 0; + int lps_chk_cnt_th = 0; + u32 lps_tx_pkts = 0, lps_rx_pkts = 0; + + if (buffer == NULL) { + RTW_INFO("input buffer is NULL!\n"); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO("input length is 0!\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO("input length is too large\n"); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u %u %u %u %d %u %u", + &enable, &lps_tx_tp, &lps_rx_tp, &lps_bi_tp, + &lps_chk_cnt_th, &lps_tx_pkts, &lps_rx_pkts); + + if (num < 1) { + RTW_INFO("input parameters < 1\n"); + return -EINVAL; + } + pwrpriv->lps_chk_by_tp = enable; + + if (lps_tx_tp) { + pwrpriv->lps_tx_tp_th = lps_tx_tp; + pwrpriv->lps_rx_tp_th = lps_tx_tp; + pwrpriv->lps_bi_tp_th = lps_tx_tp; + } + if (lps_rx_tp) + pwrpriv->lps_rx_tp_th = lps_rx_tp; + if (lps_bi_tp) + pwrpriv->lps_bi_tp_th = lps_bi_tp; + + if (lps_chk_cnt_th) + pwrpriv->lps_chk_cnt_th = lps_chk_cnt_th; + + if (lps_tx_pkts) + pwrpriv->lps_tx_pkts = lps_tx_pkts; + + if (lps_rx_pkts) + pwrpriv->lps_rx_pkts = lps_rx_pkts; + + RTW_INFO("%s lps_chk_by_tp:%s , lps_tx_tp_th:%d, lps_tx_tp_th:%d, lps_bi_tp:%d\n", + __func__, pwrpriv->lps_chk_by_tp ? "Y" : "N", + pwrpriv->lps_tx_tp_th, pwrpriv->lps_tx_tp_th, pwrpriv->lps_bi_tp_th); + RTW_INFO("%s lps_chk_cnt_th:%d , lps_tx_pkts:%d, lps_rx_pkts:%d\n", + __func__, pwrpriv->lps_chk_cnt_th, pwrpriv->lps_tx_pkts, pwrpriv->lps_rx_pkts); + } + + return count; +} + +int proc_get_lps_chk_tp(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + + RTW_PRINT_SEL(m, "LPS chk by tp - %s\n", pwrpriv->lps_chk_by_tp ? "enable" : "disable"); + RTW_PRINT_SEL(m, "LPS Tx TP TH - %d(Mbps)\n", pwrpriv->lps_tx_tp_th); + RTW_PRINT_SEL(m, "LPS Rx TP TH - %d(Mbps)\n", pwrpriv->lps_rx_tp_th); + RTW_PRINT_SEL(m, "LPS BI TP TH - %d(Mbps)\n", pwrpriv->lps_bi_tp_th); + + RTW_PRINT_SEL(m, "LPS CHK CNT - %d\n", pwrpriv->lps_chk_cnt_th); + RTW_PRINT_SEL(m, "LPS Tx PKTs - %d\n", pwrpriv->lps_tx_pkts); + RTW_PRINT_SEL(m, "LPS Rx PKTs - %d\n", pwrpriv->lps_rx_pkts); + return 0; +} +#endif /*CONFIG_LPS_CHK_BY_TP*/ +#ifdef CONFIG_SUPPORT_STATIC_SMPS +ssize_t proc_set_smps(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + char tmp[32]; + u32 enable = 0; + u32 smps_en, smps_tx_tp = 0, smps_rx_tp = 0; + u32 smps_test = 0, smps_test_en = 0; + + if (buffer == NULL) { + RTW_INFO("input buffer is NULL!\n"); + return -EFAULT; + } + + if (count < 1) { + RTW_INFO("input length is 0!\n"); + return -EFAULT; + } + + if (count > sizeof(tmp)) { + RTW_INFO("input length is too large\n"); + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%u %u %u %u %u", &smps_en, &smps_tx_tp, &smps_rx_tp, + &smps_test, &smps_test_en); + + if (num < 1) { + RTW_INFO("input parameters < 1\n"); + return -EINVAL; + } + + pmlmeext->ssmps_en = smps_en; + if (smps_tx_tp) { + pmlmeext->ssmps_tx_tp_th= smps_tx_tp; + pmlmeext->ssmps_rx_tp_th= smps_tx_tp; + } + if (smps_rx_tp) + pmlmeext->ssmps_rx_tp_th = smps_rx_tp; + + #ifdef DBG_STATIC_SMPS + if (num > 3) { + pmlmeext->ssmps_test = smps_test; + pmlmeext->ssmps_test_en = smps_test_en; + } + #endif + RTW_INFO("SM PS : %s tx_tp_th:%d, rx_tp_th:%d\n", + (smps_en) ? "Enable" : "Disable", + pmlmeext->ssmps_tx_tp_th, + pmlmeext->ssmps_rx_tp_th); + #ifdef DBG_STATIC_SMPS + RTW_INFO("SM PS : %s ssmps_test_en:%d\n", + (smps_test) ? "Enable" : "Disable", + pmlmeext->ssmps_test_en); + #endif + } + + return count; +} + +int proc_get_smps(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + RTW_PRINT_SEL(m, "Static SMPS %s\n", pmlmeext->ssmps_en ? "enable" : "disable"); + RTW_PRINT_SEL(m, "Tx TP TH %d\n", pmlmeext->ssmps_tx_tp_th); + RTW_PRINT_SEL(m, "Rx TP TH %d\n", pmlmeext->ssmps_rx_tp_th); + #ifdef DBG_STATIC_SMPS + RTW_PRINT_SEL(m, "test %d, test_en:%d\n", pmlmeext->ssmps_test, pmlmeext->ssmps_test_en); + #endif + return 0; +} +#endif /*CONFIG_SUPPORT_STATIC_SMPS*/ + +#endif /* CONFIG_PROC_DEBUG */ +#define RTW_BUFDUMP_BSIZE 16 +#if 1 +inline void RTW_BUF_DUMP_SEL(uint _loglevel, void *sel, u8 *_titlestring, + bool _idx_show, const u8 *_hexdata, int _hexdatalen) +{ +#ifdef CONFIG_RTW_DEBUG + int __i; + u8 *ptr = (u8 *)_hexdata; + + if (_loglevel <= rtw_drv_log_level) { + if (_titlestring) { + if (sel == RTW_DBGDUMP) + RTW_PRINT(""); + _RTW_PRINT_SEL(sel, "%s", _titlestring); + if (_hexdatalen >= RTW_BUFDUMP_BSIZE) + _RTW_PRINT_SEL(sel, "\n"); + } + + for (__i = 0; __i < _hexdatalen; __i++) { + if (((__i % RTW_BUFDUMP_BSIZE) == 0) && (_hexdatalen >= RTW_BUFDUMP_BSIZE)) { + if (sel == RTW_DBGDUMP) + RTW_PRINT(""); + if (_idx_show) + _RTW_PRINT_SEL(sel, "0x%03X: ", __i); + } + _RTW_PRINT_SEL(sel, "%02X%s", ptr[__i], (((__i + 1) % 4) == 0) ? " " : " "); + if ((__i + 1 < _hexdatalen) && ((__i + 1) % RTW_BUFDUMP_BSIZE) == 0) + _RTW_PRINT_SEL(sel, "\n"); + } + _RTW_PRINT_SEL(sel, "\n"); + } +#endif +} +#else +inline void _RTW_STR_DUMP_SEL(void *sel, char *str_out) +{ + if (sel == RTW_DBGDUMP) + _dbgdump("%s\n", str_out); + #if defined(_seqdump) + else + _seqdump(sel, "%s\n", str_out); + #endif /*_seqdump*/ +} +inline void RTW_BUF_DUMP_SEL(uint _loglevel, void *sel, u8 *_titlestring, + bool _idx_show, u8 *_hexdata, int _hexdatalen) +{ + int __i, len; + int __j, idx; + int block_num, remain_byte; + char str_out[128] = {'\0'}; + char str_val[32] = {'\0'}; + char *p = NULL; + u8 *ptr = (u8 *)_hexdata; + + if (_loglevel <= rtw_drv_log_level) { + /*dump title*/ + p = &str_out[0]; + if (_titlestring) { + if (sel == RTW_DBGDUMP) { + len = snprintf(str_val, sizeof(str_val), "%s", DRIVER_PREFIX); + strncpy(p, str_val, len); + p += len; + } + len = snprintf(str_val, sizeof(str_val), "%s", _titlestring); + strncpy(p, str_val, len); + p += len; + } + if (p != &str_out[0]) { + _RTW_STR_DUMP_SEL(sel, str_out); + _rtw_memset(&str_out, '\0', sizeof(str_out)); + } + + /*dump buffer*/ + block_num = _hexdatalen / RTW_BUFDUMP_BSIZE; + remain_byte = _hexdatalen % RTW_BUFDUMP_BSIZE; + for (__i = 0; __i < block_num; __i++) { + p = &str_out[0]; + if (sel == RTW_DBGDUMP) { + len = snprintf(str_val, sizeof(str_val), "%s", DRIVER_PREFIX); + strncpy(p, str_val, len); + p += len; + } + if (_idx_show) { + len = snprintf(str_val, sizeof(str_val), "0x%03X: ", __i * RTW_BUFDUMP_BSIZE); + strncpy(p, str_val, len); + p += len; + } + for (__j =0; __j < RTW_BUFDUMP_BSIZE; __j++) { + idx = __i * RTW_BUFDUMP_BSIZE + __j; + len = snprintf(str_val, sizeof(str_val), "%02X%s", ptr[idx], (((__j + 1) % 4) == 0) ? " " : " "); + strncpy(p, str_val, len); + p += len; + } + _RTW_STR_DUMP_SEL(sel, str_out); + _rtw_memset(&str_out, '\0', sizeof(str_out)); + } + + p = &str_out[0]; + if ((sel == RTW_DBGDUMP) && remain_byte) { + len = snprintf(str_val, sizeof(str_val), "%s", DRIVER_PREFIX); + strncpy(p, str_val, len); + p += len; + } + if (_idx_show && remain_byte) { + len = snprintf(str_val, sizeof(str_val), "0x%03X: ", block_num * RTW_BUFDUMP_BSIZE); + strncpy(p, str_val, len); + p += len; + } + for (__i = 0; __i < remain_byte; __i++) { + idx = block_num * RTW_BUFDUMP_BSIZE + __i; + len = snprintf(str_val, sizeof(str_val), "%02X%s", ptr[idx], (((__i + 1) % 4) == 0) ? " " : " "); + strncpy(p, str_val, len); + p += len; + } + _RTW_STR_DUMP_SEL(sel, str_out); + } +} + +#endif diff --git a/core/rtw_eeprom.c b/core/rtw_eeprom.c new file mode 100644 index 0000000..62c0be0 --- /dev/null +++ b/core/rtw_eeprom.c @@ -0,0 +1,329 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_EEPROM_C_ + +#include +#include +#include + +void up_clk(_adapter *padapter, u16 *x) +{ + *x = *x | _EESK; + rtw_write8(padapter, EE_9346CR, (u8)*x); + rtw_udelay_os(CLOCK_RATE); + + +} + +void down_clk(_adapter *padapter, u16 *x) +{ + *x = *x & ~_EESK; + rtw_write8(padapter, EE_9346CR, (u8)*x); + rtw_udelay_os(CLOCK_RATE); +} + +void shift_out_bits(_adapter *padapter, u16 data, u16 count) +{ + u16 x, mask; + + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + mask = 0x01 << (count - 1); + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDO | _EEDI); + + do { + x &= ~_EEDI; + if (data & mask) + x |= _EEDI; + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + rtw_write8(padapter, EE_9346CR, (u8)x); + rtw_udelay_os(CLOCK_RATE); + up_clk(padapter, &x); + down_clk(padapter, &x); + mask = mask >> 1; + } while (mask); + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + x &= ~_EEDI; + rtw_write8(padapter, EE_9346CR, (u8)x); +out: + return; +} + +u16 shift_in_bits(_adapter *padapter) +{ + u16 x, d = 0, i; + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDO | _EEDI); + d = 0; + + for (i = 0; i < 16; i++) { + d = d << 1; + up_clk(padapter, &x); + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDI); + if (x & _EEDO) + d |= 1; + + down_clk(padapter, &x); + } +out: + + return d; +} + +void standby(_adapter *padapter) +{ + u8 x; + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EECS | _EESK); + rtw_write8(padapter, EE_9346CR, x); + + rtw_udelay_os(CLOCK_RATE); + x |= _EECS; + rtw_write8(padapter, EE_9346CR, x); + rtw_udelay_os(CLOCK_RATE); +} + +u16 wait_eeprom_cmd_done(_adapter *padapter) +{ + u8 x; + u16 i, res = _FALSE; + standby(padapter); + for (i = 0; i < 200; i++) { + x = rtw_read8(padapter, EE_9346CR); + if (x & _EEDO) { + res = _TRUE; + goto exit; + } + rtw_udelay_os(CLOCK_RATE); + } +exit: + return res; +} + +void eeprom_clean(_adapter *padapter) +{ + u16 x; + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + x = rtw_read8(padapter, EE_9346CR); + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + x &= ~(_EECS | _EEDI); + rtw_write8(padapter, EE_9346CR, (u8)x); + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + up_clk(padapter, &x); + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + down_clk(padapter, &x); +out: + return; +} + +void eeprom_write16(_adapter *padapter, u16 reg, u16 data) +{ + u8 x; + x = rtw_read8(padapter, EE_9346CR); + + x &= ~(_EEDI | _EEDO | _EESK | _EEM0); + x |= _EEM1 | _EECS; + rtw_write8(padapter, EE_9346CR, x); + + shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5); + + if (padapter->EepromAddressSize == 8) /* CF+ and SDIO */ + shift_out_bits(padapter, 0, 6); + else /* USB */ + shift_out_bits(padapter, 0, 4); + + standby(padapter); + + /* Commented out by rcnjko, 2004.0 + * Erase this particular word. Write the erase opcode and register + * number in that order. The opcode is 3bits in length; reg is 6 bits long. */ +/* shift_out_bits(Adapter, EEPROM_ERASE_OPCODE, 3); + * shift_out_bits(Adapter, reg, Adapter->EepromAddressSize); + * + * if (wait_eeprom_cmd_done(Adapter ) == FALSE) + * { + * return; + * } */ + + + standby(padapter); + + /* write the new word to the EEPROM */ + + /* send the write opcode the EEPORM */ + shift_out_bits(padapter, EEPROM_WRITE_OPCODE, 3); + + /* select which word in the EEPROM that we are writing to. */ + shift_out_bits(padapter, reg, padapter->EepromAddressSize); + + /* write the data to the selected EEPROM word. */ + shift_out_bits(padapter, data, 16); + + if (wait_eeprom_cmd_done(padapter) == _FALSE) + + goto exit; + + standby(padapter); + + shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5); + shift_out_bits(padapter, reg, 4); + + eeprom_clean(padapter); +exit: + return; +} + +u16 eeprom_read16(_adapter *padapter, u16 reg) /* ReadEEprom */ +{ + + u16 x; + u16 data = 0; + + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + /* select EEPROM, reset bits, set _EECS */ + x = rtw_read8(padapter, EE_9346CR); + + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + + x &= ~(_EEDI | _EEDO | _EESK | _EEM0); + x |= _EEM1 | _EECS; + rtw_write8(padapter, EE_9346CR, (unsigned char)x); + + /* write the read opcode and register number in that order */ + /* The opcode is 3bits in length, reg is 6 bits long */ + shift_out_bits(padapter, EEPROM_READ_OPCODE, 3); + shift_out_bits(padapter, reg, padapter->EepromAddressSize); + + /* Now read the data (16 bits) in from the selected EEPROM word */ + data = shift_in_bits(padapter); + + eeprom_clean(padapter); +out: + + return data; + + +} + + + + +/* From even offset */ +void eeprom_read_sz(_adapter *padapter, u16 reg, u8 *data, u32 sz) +{ + + u16 x, data16; + u32 i; + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + /* select EEPROM, reset bits, set _EECS */ + x = rtw_read8(padapter, EE_9346CR); + + if (rtw_is_surprise_removed(padapter)) { + goto out; + } + + x &= ~(_EEDI | _EEDO | _EESK | _EEM0); + x |= _EEM1 | _EECS; + rtw_write8(padapter, EE_9346CR, (unsigned char)x); + + /* write the read opcode and register number in that order */ + /* The opcode is 3bits in length, reg is 6 bits long */ + shift_out_bits(padapter, EEPROM_READ_OPCODE, 3); + shift_out_bits(padapter, reg, padapter->EepromAddressSize); + + + for (i = 0; i < sz; i += 2) { + data16 = shift_in_bits(padapter); + data[i] = data16 & 0xff; + data[i + 1] = data16 >> 8; + } + + eeprom_clean(padapter); +out: + return; +} + + +/* addr_off : address offset of the entry in eeprom (not the tuple number of eeprom (reg); that is addr_off !=reg) */ +u8 eeprom_read(_adapter *padapter, u32 addr_off, u8 sz, u8 *rbuf) +{ + u8 quotient, remainder, addr_2align_odd; + u16 reg, stmp , i = 0, idx = 0; + reg = (u16)(addr_off >> 1); + addr_2align_odd = (u8)(addr_off & 0x1); + + if (addr_2align_odd) { /* read that start at high part: e.g 1,3,5,7,9,... */ + stmp = eeprom_read16(padapter, reg); + rbuf[idx++] = (u8)((stmp >> 8) & 0xff); /* return hogh-part of the short */ + reg++; + sz--; + } + + quotient = sz >> 1; + remainder = sz & 0x1; + + for (i = 0 ; i < quotient; i++) { + stmp = eeprom_read16(padapter, reg + i); + rbuf[idx++] = (u8)(stmp & 0xff); + rbuf[idx++] = (u8)((stmp >> 8) & 0xff); + } + + reg = reg + i; + if (remainder) { /* end of read at lower part of short : 0,2,4,6,... */ + stmp = eeprom_read16(padapter, reg); + rbuf[idx] = (u8)(stmp & 0xff); + } + return _TRUE; +} + + + +void read_eeprom_content(_adapter *padapter) +{ + + + +} diff --git a/core/rtw_ft.c b/core/rtw_ft.c new file mode 100644 index 0000000..1ba55ce --- /dev/null +++ b/core/rtw_ft.c @@ -0,0 +1,668 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include + +#ifdef CONFIG_RTW_80211R + +#ifndef RTW_FT_DBG + #define RTW_FT_DBG 0 +#endif +#if RTW_FT_DBG + #define RTW_FT_INFO(fmt, arg...) \ + RTW_INFO(fmt, arg) + #define RTW_FT_DUMP(str, data, len) \ + RTW_INFO_DUMP(str, data, len) +#else + #define RTW_FT_INFO(fmt, arg...) do {} while (0) + #define RTW_FT_DUMP(str, data, len) do {} while (0) +#endif + +void rtw_ft_info_init(struct ft_roam_info *pft) +{ + _rtw_memset(pft, 0, sizeof(struct ft_roam_info)); + pft->ft_flags = 0 + | RTW_FT_EN + /* | RTW_FT_OTD_EN */ +#ifdef CONFIG_RTW_BTM_ROAM + | RTW_FT_BTM_ROAM +#endif + ; + pft->ft_updated_bcn = _FALSE; + RTW_FT_INFO("%s : ft_flags=0x%02x\n", __func__, pft->ft_flags); +} + +ssize_t rtw_ft_proc_flags_set(struct file *file, + const char __user *buffer, size_t count, loff_t *pos, void *data) +{ + struct net_device *dev = data; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + char tmp[32]; + u8 flags; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%hhx", &flags); + if (num == 1) + adapter->mlmepriv.ft_roam.ft_flags = flags; + } + + return count; + +} + +int rtw_ft_proc_flags_get(struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); + + RTW_PRINT_SEL(m, "0x%02x\n", adapter->mlmepriv.ft_roam.ft_flags); + + return 0; +} + +u8 rtw_ft_chk_roaming_candidate( + _adapter *padapter, struct wlan_network *competitor) +{ + u8 *pmdie; + u32 mdie_len = 0; + struct ft_roam_info *pft_roam = &(padapter->mlmepriv.ft_roam); + + if (!(pmdie = rtw_get_ie(&competitor->network.IEs[12], + _MDIE_, &mdie_len, competitor->network.IELength-12))) { + RTW_INFO("FT : MDIE not foud in competitor!\n"); + return _FALSE; + } + + if (!_rtw_memcmp(&pft_roam->mdid, (pmdie+2), 2)) { + RTW_INFO("FT : unmatched MDIE!\n"); + return _FALSE; + } + + /*The candidate don't support over-the-DS*/ + if (rtw_ft_valid_otd_candidate(padapter, pmdie)) { + RTW_INFO("FT: ignore the candidate(" + MAC_FMT ") for over-the-DS\n", + MAC_ARG(competitor->network.MacAddress)); + /* rtw_ft_clr_flags(padapter, RTW_FT_PEER_OTD_EN); */ + return _FALSE; + } + + if (rtw_ft_chk_flags(padapter, RTW_FT_TEST_RSSI_ROAM)) { + if (!_rtw_memcmp(padapter->mlmepriv.cur_network.network.MacAddress, + competitor->network.MacAddress, ETH_ALEN) ) { + competitor->network.Rssi +=20; + RTW_FT_INFO("%s : update "MAC_FMT" RSSI to %d for RTW_FT_TEST_RSSI_ROAM\n", + __func__, MAC_ARG(competitor->network.MacAddress), + (int)competitor->network.Rssi); + rtw_ft_clr_flags(padapter, RTW_FT_TEST_RSSI_ROAM); + } + } + + return _TRUE; +} + +void rtw_ft_update_stainfo(_adapter *padapter, WLAN_BSSID_EX *pnetwork) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + + psta = rtw_get_stainfo(pstapriv, pnetwork->MacAddress); + if (psta == NULL) + psta = rtw_alloc_stainfo(pstapriv, pnetwork->MacAddress); + + if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { + + padapter->securitypriv.binstallGrpkey = _FALSE; + padapter->securitypriv.busetkipkey = _FALSE; + padapter->securitypriv.bgrpkey_handshake = _FALSE; + + psta->ieee8021x_blocked = _TRUE; + psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm; + + _rtw_memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype)); + _rtw_memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype)); + _rtw_memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype)); + } + +} + +void rtw_ft_reassoc_event_callback(_adapter *padapter, u8 *pbuf) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct stassoc_event *pstassoc = (struct stassoc_event *)pbuf; + struct ft_roam_info *pft_roam = &(pmlmepriv->ft_roam); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&(pmlmeinfo->network); + struct cfg80211_ft_event_params ft_evt_parms; + _irqL irqL; + + _rtw_memset(&ft_evt_parms, 0, sizeof(ft_evt_parms)); + rtw_ft_update_stainfo(padapter, pnetwork); + ft_evt_parms.ies_len = pft_roam->ft_event.ies_len; + ft_evt_parms.ies = rtw_zmalloc(ft_evt_parms.ies_len); + if (ft_evt_parms.ies) + _rtw_memcpy((void *)ft_evt_parms.ies, pft_roam->ft_event.ies, ft_evt_parms.ies_len); + else + goto err_2; + + ft_evt_parms.target_ap = rtw_zmalloc(ETH_ALEN); + if (ft_evt_parms.target_ap) + _rtw_memcpy((void *)ft_evt_parms.target_ap, pstassoc->macaddr, ETH_ALEN); + else + goto err_1; + + ft_evt_parms.ric_ies = pft_roam->ft_event.ric_ies; + ft_evt_parms.ric_ies_len = pft_roam->ft_event.ric_ies_len; + + /* It's a KERNEL issue between v4.11 ~ v4.16, + * <= v4.10, NLMSG_DEFAULT_SIZE is used for nlmsg_new(). + * v4.11 ~ v4.16, only used "100 + >ric_ies_len" for nlmsg_new() + * even then DRIVER don't support RIC. + * >= v4.17, issue should correct as "100 + ies_len + ric_ies_len". + */ + #if ((LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0)) && \ + (LINUX_VERSION_CODE < KERNEL_VERSION(4, 17, 0))) + if (!ft_evt_parms.ric_ies_len) + ft_evt_parms.ric_ies_len = ft_evt_parms.ies_len; + else + ft_evt_parms.ric_ies_len += ft_evt_parms.ies_len; + #endif + + rtw_ft_lock_set_status(padapter, RTW_FT_AUTHENTICATED_STA, &irqL); + rtw_cfg80211_ft_event(padapter, &ft_evt_parms); + RTW_INFO("%s: to "MAC_FMT"\n", __func__, MAC_ARG(ft_evt_parms.target_ap)); + + rtw_mfree((u8 *)pft_roam->ft_event.target_ap, ETH_ALEN); +err_1: + rtw_mfree((u8 *)ft_evt_parms.ies, ft_evt_parms.ies_len); +err_2: + return; +} + +void rtw_ft_validate_akm_type(_adapter *padapter, + struct wlan_network *pnetwork) +{ + struct security_priv *psecuritypriv = &(padapter->securitypriv); + struct ft_roam_info *pft_roam = &(padapter->mlmepriv.ft_roam); + u32 tmp_len; + u8 *ptmp; + + /*IEEE802.11-2012 Std. Table 8-101-AKM suite selectors*/ + if (rtw_ft_valid_akm(padapter, psecuritypriv->rsn_akm_suite_type)) { + ptmp = rtw_get_ie(&pnetwork->network.IEs[12], + _MDIE_, &tmp_len, (pnetwork->network.IELength-12)); + if (ptmp) { + pft_roam->mdid = *(u16 *)(ptmp+2); + pft_roam->ft_cap = *(ptmp+4); + + RTW_INFO("FT: target " MAC_FMT " mdid=(0x%2x), capacity=(0x%2x)\n", + MAC_ARG(pnetwork->network.MacAddress), pft_roam->mdid, pft_roam->ft_cap); + rtw_ft_set_flags(padapter, RTW_FT_PEER_EN); + RTW_FT_INFO("%s : peer support FTOTA(0x%02x)\n", __func__, pft_roam->ft_flags); + + if (rtw_ft_otd_roam_en(padapter)) { + rtw_ft_set_flags(padapter, RTW_FT_PEER_OTD_EN); + RTW_FT_INFO("%s : peer support FTOTD(0x%02x)\n", __func__, pft_roam->ft_flags); + } + } else { + /* Don't use FT roaming if target AP cannot support FT */ + rtw_ft_clr_flags(padapter, (RTW_FT_PEER_EN|RTW_FT_PEER_OTD_EN)); + rtw_ft_reset_status(padapter); + } + } else { + /* It could be a non-FT connection */ + rtw_ft_clr_flags(padapter, (RTW_FT_PEER_EN|RTW_FT_PEER_OTD_EN)); + rtw_ft_reset_status(padapter); + } + + RTW_FT_INFO("%s : ft_flags=0x%02x\n", __func__, pft_roam->ft_flags); +} + +void rtw_ft_update_bcn(_adapter *padapter, union recv_frame *precv_frame) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + WLAN_BSSID_EX *pbss; + + if (rtw_ft_chk_status(padapter,RTW_FT_ASSOCIATED_STA) + && (pmlmepriv->ft_roam.ft_updated_bcn == _FALSE)) { + pbss = (WLAN_BSSID_EX*)rtw_malloc(sizeof(WLAN_BSSID_EX)); + if (pbss) { + if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) { + struct beacon_keys recv_beacon; + + update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE); + /* Move into rtw_get_bcn_keys */ + /* rtw_get_bcn_info(&(pmlmepriv->cur_network)); */ + + /* update bcn keys */ + if (rtw_get_bcn_keys(padapter, pframe, len, &recv_beacon) == _TRUE) { + RTW_FT_INFO("%s: beacon keys ready\n", __func__); + _rtw_memcpy(&pmlmepriv->cur_beacon_keys, + &recv_beacon, sizeof(recv_beacon)); + if (is_hidden_ssid(recv_beacon.ssid, recv_beacon.ssid_len)) { + _rtw_memcpy(pmlmepriv->cur_beacon_keys.ssid, pmlmeinfo->network.Ssid.Ssid, IW_ESSID_MAX_SIZE); + pmlmepriv->cur_beacon_keys.ssid_len = pmlmeinfo->network.Ssid.SsidLength; + } + } else { + RTW_ERR("%s: get beacon keys failed\n", __func__); + _rtw_memset(&pmlmepriv->cur_beacon_keys, 0, sizeof(recv_beacon)); + } + #ifdef CONFIG_BCN_CNT_CONFIRM_HDL + pmlmepriv->new_beacon_cnts = 0; + #endif + } + rtw_mfree((u8*)pbss, sizeof(WLAN_BSSID_EX)); + } + + /* check the vendor of the assoc AP */ + pmlmeinfo->assoc_AP_vendor = + check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), + (len - sizeof(struct rtw_ieee80211_hdr_3addr))); + + /* update TSF Value */ + update_TSF(pmlmeext, pframe, len); + pmlmeext->bcn_cnt = 0; + pmlmeext->last_bcn_cnt = 0; + pmlmepriv->ft_roam.ft_updated_bcn = _TRUE; + } +} + +void rtw_ft_start_clnt_join(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ft_roam_info *pft_roam = &(pmlmepriv->ft_roam); + + if (rtw_ft_otd_roam(padapter)) { + pmlmeinfo->state = WIFI_FW_AUTH_SUCCESS | WIFI_FW_STATION_STATE; + pft_roam->ft_event.ies = + (pft_roam->ft_action + sizeof(struct rtw_ieee80211_hdr_3addr) + 16); + pft_roam->ft_event.ies_len = + (pft_roam->ft_action_len - sizeof(struct rtw_ieee80211_hdr_3addr)); + + /*Not support RIC*/ + pft_roam->ft_event.ric_ies = NULL; + pft_roam->ft_event.ric_ies_len = 0; + rtw_ft_report_evt(padapter); + return; + } + + pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE; + start_clnt_auth(padapter); +} + +u8 rtw_ft_update_rsnie( + _adapter *padapter, u8 bwrite, + struct pkt_attrib *pattrib, u8 **pframe) +{ + struct ft_roam_info *pft_roam = &(padapter->mlmepriv.ft_roam); + u8 *pie; + u32 len; + + pie = rtw_get_ie(pft_roam->updated_ft_ies, EID_WPA2, &len, + pft_roam->updated_ft_ies_len); + + if (!bwrite) + return (pie)?_SUCCESS:_FAIL; + + if (pie) { + *pframe = rtw_set_ie(((u8 *)*pframe), EID_WPA2, len, + pie+2, &(pattrib->pktlen)); + } else + return _FAIL; + + return _SUCCESS; +} + +static u8 rtw_ft_update_mdie( + _adapter *padapter, struct pkt_attrib *pattrib, u8 **pframe) +{ + struct ft_roam_info *pft_roam = &(padapter->mlmepriv.ft_roam); + u8 *pie, mdie[3]; + u32 len = 3; + + if (rtw_ft_roam(padapter)) { + if ((pie = rtw_get_ie(pft_roam->updated_ft_ies, _MDIE_, + &len, pft_roam->updated_ft_ies_len))) { + pie = (pie + 2); /* ignore md-id & length */ + } else + return _FAIL; + } else { + *((u16 *)&mdie[0]) = pft_roam->mdid; + mdie[2] = pft_roam->ft_cap; + pie = &mdie[0]; + } + + *pframe = rtw_set_ie(((u8 *)*pframe), _MDIE_, len , pie, &(pattrib->pktlen)); + return _SUCCESS; +} + +static u8 rtw_ft_update_ftie( + _adapter *padapter, struct pkt_attrib *pattrib, u8 **pframe) +{ + struct ft_roam_info *pft_roam = &(padapter->mlmepriv.ft_roam); + u8 *pie; + u32 len; + + if ((pie = rtw_get_ie(pft_roam->updated_ft_ies, _FTIE_, &len, + pft_roam->updated_ft_ies_len)) != NULL) { + *pframe = rtw_set_ie(*pframe, _FTIE_, len , + (pie+2), &(pattrib->pktlen)); + } else + return _FAIL; + + return _SUCCESS; +} + +void rtw_ft_build_auth_req_ies(_adapter *padapter, + struct pkt_attrib *pattrib, u8 **pframe) +{ + u8 ftie_append = _TRUE; + + if (!pattrib || !(*pframe)) + return; + + if (!rtw_ft_roam(padapter)) + return; + + ftie_append = rtw_ft_update_rsnie(padapter, _TRUE, pattrib, pframe); + rtw_ft_update_mdie(padapter, pattrib, pframe); + if (ftie_append) + rtw_ft_update_ftie(padapter, pattrib, pframe); +} + +void rtw_ft_build_assoc_req_ies(_adapter *padapter, + u8 is_reassoc, struct pkt_attrib *pattrib, u8 **pframe) +{ + if (!pattrib || !(*pframe)) + return; + + if (rtw_ft_chk_flags(padapter, RTW_FT_PEER_EN)) + rtw_ft_update_mdie(padapter, pattrib, pframe); + + if ((!is_reassoc) || (!rtw_ft_roam(padapter))) + return; + + if (rtw_ft_update_rsnie(padapter, _FALSE, pattrib, pframe)) + rtw_ft_update_ftie(padapter, pattrib, pframe); +} + +u8 rtw_ft_update_auth_rsp_ies(_adapter *padapter, u8 *pframe, u32 len) +{ + u8 ret = _SUCCESS; + u8 target_ap_addr[ETH_ALEN] = {0}; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ft_roam_info *pft_roam = &(pmlmepriv->ft_roam); + + if (!rtw_ft_roam(padapter)) + return _FAIL; + + /*rtw_ft_report_reassoc_evt already, + * and waiting for cfg80211_rtw_update_ft_ies */ + if (rtw_ft_authed_sta(padapter)) + return ret; + + if (!pframe || !len) + return _FAIL; + + rtw_buf_update(&pmlmepriv->auth_rsp, + &pmlmepriv->auth_rsp_len, pframe, len); + pft_roam->ft_event.ies = + (pmlmepriv->auth_rsp + sizeof(struct rtw_ieee80211_hdr_3addr) + 6); + pft_roam->ft_event.ies_len = + (pmlmepriv->auth_rsp_len - sizeof(struct rtw_ieee80211_hdr_3addr) - 6); + + /*Not support RIC*/ + pft_roam->ft_event.ric_ies = NULL; + pft_roam->ft_event.ric_ies_len = 0; + _rtw_memcpy(target_ap_addr, pmlmepriv->assoc_bssid, ETH_ALEN); + rtw_ft_report_reassoc_evt(padapter, target_ap_addr); + + return ret; +} + +static void rtw_ft_start_clnt_action(_adapter *padapter, u8 *pTargetAddr) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + rtw_ft_set_status(padapter, RTW_FT_REQUESTING_STA); + rtw_ft_issue_action_req(padapter, pTargetAddr); + _set_timer(&pmlmeext->ft_link_timer, REASSOC_TO); +} + +void rtw_ft_start_roam(_adapter *padapter, u8 *pTargetAddr) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (rtw_ft_otd_roam(padapter)) { + RTW_FT_INFO("%s : try OTD roaming\n", __func__); + rtw_ft_start_clnt_action(padapter, pTargetAddr); + } else { + /*wait a little time to retrieve packets buffered in the current ap while scan*/ + RTW_FT_INFO("%s : start roaming timer\n", __func__); + _set_timer(&pmlmeext->ft_roam_timer, 30); + } +} + +void rtw_ft_issue_action_req(_adapter *padapter, u8 *pTargetAddr) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct xmit_frame *pmgntframe; + struct rtw_ieee80211_hdr *pwlanhdr; + struct pkt_attrib *pattrib; + u8 *pframe; + u8 category = RTW_WLAN_CATEGORY_FT; + u8 action = RTW_WLAN_ACTION_FT_REQ; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + pwlanhdr->frame_ctl = 0; + + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + _rtw_memcpy(pframe, adapter_mac_addr(padapter), ETH_ALEN); + pframe += ETH_ALEN; + pattrib->pktlen += ETH_ALEN; + + _rtw_memcpy(pframe, pTargetAddr, ETH_ALEN); + pframe += ETH_ALEN; + pattrib->pktlen += ETH_ALEN; + + rtw_ft_update_mdie(padapter, pattrib, &pframe); + if (rtw_ft_update_rsnie(padapter, _TRUE, pattrib, &pframe)) + rtw_ft_update_ftie(padapter, pattrib, &pframe); + + RTW_INFO("FT : issue RTW_WLAN_ACTION_FT_REQ\n"); + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(padapter, pmgntframe); +} + +void rtw_ft_report_evt(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ft_roam_info *pft_roam = &(pmlmepriv->ft_roam); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&(pmlmeinfo->network); + struct cfg80211_ft_event_params ft_evt_parms; + _irqL irqL; + + _rtw_memset(&ft_evt_parms, 0, sizeof(ft_evt_parms)); + rtw_ft_update_stainfo(padapter, pnetwork); + + if (!pnetwork) + goto err_2; + + ft_evt_parms.ies_len = pft_roam->ft_event.ies_len; + ft_evt_parms.ies = rtw_zmalloc(ft_evt_parms.ies_len); + if (ft_evt_parms.ies) + _rtw_memcpy((void *)ft_evt_parms.ies, pft_roam->ft_event.ies, ft_evt_parms.ies_len); + else + goto err_2; + + ft_evt_parms.target_ap = rtw_zmalloc(ETH_ALEN); + if (ft_evt_parms.target_ap) + _rtw_memcpy((void *)ft_evt_parms.target_ap, pnetwork->MacAddress, ETH_ALEN); + else + goto err_1; + + ft_evt_parms.ric_ies = pft_roam->ft_event.ric_ies; + ft_evt_parms.ric_ies_len = pft_roam->ft_event.ric_ies_len; + + /* It's a KERNEL issue between v4.11 ~ v4.16, + * <= v4.10, NLMSG_DEFAULT_SIZE is used for nlmsg_new(). + * v4.11 ~ v4.16, only used "100 + >ric_ies_len" for nlmsg_new() + * even then DRIVER don't support RIC. + * >= v4.17, issue should correct as "100 + ies_len + ric_ies_len". + */ + #if ((LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0)) && \ + (LINUX_VERSION_CODE < KERNEL_VERSION(4, 17, 0))) + ft_evt_parms.ric_ies_len = (ft_evt_parms.ies_len <= 100 )? + (0):(ft_evt_parms.ies_len - 100); + #endif + + rtw_ft_lock_set_status(padapter, RTW_FT_AUTHENTICATED_STA, &irqL); + rtw_cfg80211_ft_event(padapter, &ft_evt_parms); + RTW_INFO("FT: rtw_ft_report_evt\n"); + rtw_mfree((u8 *)pft_roam->ft_event.target_ap, ETH_ALEN); +err_1: + rtw_mfree((u8 *)ft_evt_parms.ies, ft_evt_parms.ies_len); +err_2: + return; +} + +void rtw_ft_report_reassoc_evt(_adapter *padapter, u8 *pMacAddr) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + struct cmd_obj *pcmd_obj = NULL; + struct stassoc_event *passoc_sta_evt = NULL; + struct rtw_evt_header *evt_hdr = NULL; + u8 *pevtcmd = NULL; + u32 cmdsz = 0; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct stassoc_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + return; + } + + _rtw_init_listhead(&pcmd_obj->list); + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct stassoc_event); + evt_hdr->id = EVT_FT_REASSOC; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + passoc_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + _rtw_memcpy((unsigned char *)(&(passoc_sta_evt->macaddr)), pMacAddr, ETH_ALEN); + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); +} + +void rtw_ft_link_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct ft_roam_info *pft_roam = &(pmlmepriv->ft_roam); + + if (rtw_ft_chk_status(padapter, RTW_FT_REQUESTING_STA)) { + if (pft_roam->ft_req_retry_cnt < RTW_FT_ACTION_REQ_LMT) { + pft_roam->ft_req_retry_cnt++; + rtw_ft_issue_action_req(padapter, (u8 *)pmlmepriv->roam_network->network.MacAddress); + _set_timer(&pmlmeext->ft_link_timer, REASSOC_TO); + } else { + pft_roam->ft_req_retry_cnt = 0; + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + rtw_ft_set_status(padapter, RTW_FT_ASSOCIATED_STA); + else + rtw_ft_reset_status(padapter); + } + } +} + +void rtw_ft_roam_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + RTW_FT_INFO("%s : try roaming\n", __func__); + receive_disconnect(padapter, pmlmepriv->cur_network.network.MacAddress + , WLAN_REASON_ACTIVE_ROAM, _FALSE); +} + +void rtw_ft_roam_status_reset(_adapter *padapter) +{ + struct ft_roam_info *pft_roam = &(padapter->mlmepriv.ft_roam); + + if ((rtw_to_roam(padapter) > 0) && + (!rtw_ft_chk_status(padapter, RTW_FT_REQUESTED_STA))) { + rtw_ft_reset_status(padapter); + } + + padapter->mlmepriv.ft_roam.ft_updated_bcn = _FALSE; +} + +#endif /* CONFIG_RTW_80211R */ diff --git a/core/rtw_ieee80211.c b/core/rtw_ieee80211.c new file mode 100644 index 0000000..91787b8 --- /dev/null +++ b/core/rtw_ieee80211.c @@ -0,0 +1,3212 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _IEEE80211_C + +#ifdef CONFIG_PLATFORM_INTEL_BYT + #include +#endif +#include + + +u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 }; +u16 RTW_WPA_VERSION = 1; +u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 }; +u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 }; +u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 }; +u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 }; +u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 }; +u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 }; +u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 }; +u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 }; +u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 }; + +u16 RSN_VERSION_BSD = 1; +u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 }; +u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 }; +u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 }; +u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 }; +u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 }; +u8 RSN_CIPHER_SUITE_AES_128_CMAC[] = { 0x00, 0x0f, 0xac, 6 }; +u8 RSN_CIPHER_SUITE_GCMP[] = { 0x00, 0x0f, 0xac, 8 }; +u8 RSN_CIPHER_SUITE_GCMP_256[] = { 0x00, 0x0f, 0xac, 9 }; +u8 RSN_CIPHER_SUITE_CCMP_256[] = { 0x00, 0x0f, 0xac, 10 }; +u8 RSN_CIPHER_SUITE_BIP_GMAC_128[] = { 0x00, 0x0f, 0xac, 11 }; +u8 RSN_CIPHER_SUITE_BIP_GMAC_256[] = { 0x00, 0x0f, 0xac, 12 }; +u8 RSN_CIPHER_SUITE_BIP_CMAC_256[] = { 0x00, 0x0f, 0xac, 13 }; +u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 }; + +u8 WLAN_AKM_8021X[] = {0x00, 0x0f, 0xac, 1}; +u8 WLAN_AKM_PSK[] = {0x00, 0x0f, 0xac, 2}; +u8 WLAN_AKM_FT_8021X[] = {0x00, 0x0f, 0xac, 3}; +u8 WLAN_AKM_FT_PSK[] = {0x00, 0x0f, 0xac, 4}; +u8 WLAN_AKM_8021X_SHA256[] = {0x00, 0x0f, 0xac, 5}; +u8 WLAN_AKM_PSK_SHA256[] = {0x00, 0x0f, 0xac, 6}; +u8 WLAN_AKM_TDLS[] = {0x00, 0x0f, 0xac, 7}; +u8 WLAN_AKM_SAE[] = {0x00, 0x0f, 0xac, 8}; +u8 WLAN_AKM_FT_OVER_SAE[] = {0x00, 0x0f, 0xac, 9}; +u8 WLAN_AKM_8021X_SUITE_B[] = {0x00, 0x0f, 0xac, 11}; +u8 WLAN_AKM_8021X_SUITE_B_192[] = {0x00, 0x0f, 0xac, 12}; +u8 WLAN_AKM_FILS_SHA256[] = {0x00, 0x0f, 0xac, 14}; +u8 WLAN_AKM_FILS_SHA384[] = {0x00, 0x0f, 0xac, 15}; +u8 WLAN_AKM_FT_FILS_SHA256[] = {0x00, 0x0f, 0xac, 16}; +u8 WLAN_AKM_FT_FILS_SHA384[] = {0x00, 0x0f, 0xac, 17}; +/* ----------------------------------------------------------- + * for adhoc-master to generate ie and provide supported-rate to fw + * ----------------------------------------------------------- */ + +u8 WIFI_CCKRATES[] = { + (IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK), + (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK), + (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK), + (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK) +}; + +u8 WIFI_OFDMRATES[] = { + (IEEE80211_OFDM_RATE_6MB), + (IEEE80211_OFDM_RATE_9MB), + (IEEE80211_OFDM_RATE_12MB), + (IEEE80211_OFDM_RATE_18MB), + (IEEE80211_OFDM_RATE_24MB), + IEEE80211_OFDM_RATE_36MB, + IEEE80211_OFDM_RATE_48MB, + IEEE80211_OFDM_RATE_54MB +}; + +const char *MGN_RATE_STR(enum MGN_RATE rate) +{ + u8 hw_rate; + + if (rate == MGN_MCS32) + return "MCS32"; + + hw_rate = MRateToHwRate(rate); + if (hw_rate == DESC_RATE1M && rate != MGN_1M) + hw_rate = DESC_RATE_NUM; /* invalid case */ + + return HDATA_RATE(hw_rate); +} + +u8 mgn_rates_cck[4] = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M}; +u8 mgn_rates_ofdm[8] = {MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M}; +u8 mgn_rates_mcs0_7[8] = {MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7}; +u8 mgn_rates_mcs8_15[8] = {MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15}; +u8 mgn_rates_mcs16_23[8] = {MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23}; +u8 mgn_rates_mcs24_31[8] = {MGN_MCS24, MGN_MCS25, MGN_MCS26, MGN_MCS27, MGN_MCS28, MGN_MCS29, MGN_MCS30, MGN_MCS31}; +u8 mgn_rates_vht1ss[10] = {MGN_VHT1SS_MCS0, MGN_VHT1SS_MCS1, MGN_VHT1SS_MCS2, MGN_VHT1SS_MCS3, MGN_VHT1SS_MCS4 + , MGN_VHT1SS_MCS5, MGN_VHT1SS_MCS6, MGN_VHT1SS_MCS7, MGN_VHT1SS_MCS8, MGN_VHT1SS_MCS9 + }; +u8 mgn_rates_vht2ss[10] = {MGN_VHT2SS_MCS0, MGN_VHT2SS_MCS1, MGN_VHT2SS_MCS2, MGN_VHT2SS_MCS3, MGN_VHT2SS_MCS4 + , MGN_VHT2SS_MCS5, MGN_VHT2SS_MCS6, MGN_VHT2SS_MCS7, MGN_VHT2SS_MCS8, MGN_VHT2SS_MCS9 + }; +u8 mgn_rates_vht3ss[10] = {MGN_VHT3SS_MCS0, MGN_VHT3SS_MCS1, MGN_VHT3SS_MCS2, MGN_VHT3SS_MCS3, MGN_VHT3SS_MCS4 + , MGN_VHT3SS_MCS5, MGN_VHT3SS_MCS6, MGN_VHT3SS_MCS7, MGN_VHT3SS_MCS8, MGN_VHT3SS_MCS9 + }; +u8 mgn_rates_vht4ss[10] = {MGN_VHT4SS_MCS0, MGN_VHT4SS_MCS1, MGN_VHT4SS_MCS2, MGN_VHT4SS_MCS3, MGN_VHT4SS_MCS4 + , MGN_VHT4SS_MCS5, MGN_VHT4SS_MCS6, MGN_VHT4SS_MCS7, MGN_VHT4SS_MCS8, MGN_VHT4SS_MCS9 + }; + +RATE_SECTION mgn_rate_to_rs(enum MGN_RATE rate) +{ + RATE_SECTION rs = RATE_SECTION_NUM; + + if (IS_CCK_RATE(rate)) + rs = CCK; + else if (IS_OFDM_RATE(rate)) + rs = OFDM; + else if (IS_HT1SS_RATE(rate)) + rs = HT_1SS; + else if (IS_HT2SS_RATE(rate)) + rs = HT_2SS; + else if (IS_HT3SS_RATE(rate)) + rs = HT_3SS; + else if (IS_HT4SS_RATE(rate)) + rs = HT_4SS; + else if (IS_VHT1SS_RATE(rate)) + rs = VHT_1SS; + else if (IS_VHT2SS_RATE(rate)) + rs = VHT_2SS; + else if (IS_VHT3SS_RATE(rate)) + rs = VHT_3SS; + else if (IS_VHT4SS_RATE(rate)) + rs = VHT_4SS; + + return rs; +} + +static const char *const _rate_section_str[] = { + "CCK", + "OFDM", + "HT_1SS", + "HT_2SS", + "HT_3SS", + "HT_4SS", + "VHT_1SS", + "VHT_2SS", + "VHT_3SS", + "VHT_4SS", + "RATE_SECTION_UNKNOWN", +}; + +const char *rate_section_str(u8 section) +{ + section = (section >= RATE_SECTION_NUM) ? RATE_SECTION_NUM : section; + return _rate_section_str[section]; +} + +struct rate_section_ent rates_by_sections[RATE_SECTION_NUM] = { + {RF_1TX, 4, mgn_rates_cck}, + {RF_1TX, 8, mgn_rates_ofdm}, + {RF_1TX, 8, mgn_rates_mcs0_7}, + {RF_2TX, 8, mgn_rates_mcs8_15}, + {RF_3TX, 8, mgn_rates_mcs16_23}, + {RF_4TX, 8, mgn_rates_mcs24_31}, + {RF_1TX, 10, mgn_rates_vht1ss}, + {RF_2TX, 10, mgn_rates_vht2ss}, + {RF_3TX, 10, mgn_rates_vht3ss}, + {RF_4TX, 10, mgn_rates_vht4ss}, +}; + +int rtw_get_bit_value_from_ieee_value(u8 val) +{ + unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /* last element must be zero!! */ + + int i = 0; + while (dot11_rate_table[i] != 0) { + if (dot11_rate_table[i] == val) + return BIT(i); + i++; + } + return 0; +} +uint rtw_get_cckrate_size(u8 *rate, u32 rate_length) +{ + int i = 0; + while(i < rate_length){ + RTW_DBG("%s, rate[%d]=%u\n", __FUNCTION__, i, rate[i]); + if (((rate[i] & 0x7f) == 2) || ((rate[i] & 0x7f) == 4) || + ((rate[i] & 0x7f) == 11) || ((rate[i] & 0x7f) == 22)) + i++; + else + break; + } + return i; +} + +uint rtw_is_cckrates_included(u8 *rate) +{ + u32 i = 0; + + while (rate[i] != 0) { + if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) || + (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22)) + return _TRUE; + i++; + } + + return _FALSE; +} + +uint rtw_is_cckratesonly_included(u8 *rate) +{ + u32 i = 0; + + + while (rate[i] != 0) { + if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) && + (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22)) + return _FALSE; + + i++; + } + + return _TRUE; + +} + +int rtw_check_network_type(unsigned char *rate, int ratelen, int channel) +{ + if (channel > 14) { + if ((rtw_is_cckrates_included(rate)) == _TRUE) + return WIRELESS_INVALID; + else + return WIRELESS_11A; + } else { /* could be pure B, pure G, or B/G */ + if ((rtw_is_cckratesonly_included(rate)) == _TRUE) + return WIRELESS_11B; + else if ((rtw_is_cckrates_included(rate)) == _TRUE) + return WIRELESS_11BG; + else + return WIRELESS_11G; + } + +} + +u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source, + unsigned int *frlen) +{ + _rtw_memcpy((void *)pbuf, (void *)source, len); + *frlen = *frlen + len; + return pbuf + len; +} + +/* rtw_set_ie will update frame length */ +u8 *rtw_set_ie +( + u8 *pbuf, + sint index, + uint len, + const u8 *source, + uint *frlen /* frame length */ +) +{ + *pbuf = (u8)index; + + *(pbuf + 1) = (u8)len; + + if (len > 0) + _rtw_memcpy((void *)(pbuf + 2), (void *)source, len); + + if (frlen) + *frlen = *frlen + (len + 2); + + return pbuf + len + 2; +} + +inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode, + u8 new_ch, u8 ch_switch_cnt) +{ + u8 ie_data[3]; + + ie_data[0] = ch_switch_mode; + ie_data[1] = new_ch; + ie_data[2] = ch_switch_cnt; + return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH, 3, ie_data, buf_len); +} + +inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset) +{ + if (ch_offset == SCN) + return HAL_PRIME_CHNL_OFFSET_DONT_CARE; + else if (ch_offset == SCA) + return HAL_PRIME_CHNL_OFFSET_LOWER; + else if (ch_offset == SCB) + return HAL_PRIME_CHNL_OFFSET_UPPER; + + return HAL_PRIME_CHNL_OFFSET_DONT_CARE; +} + +inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset) +{ + if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) + return SCN; + else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER) + return SCA; + else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER) + return SCB; + + return SCN; +} + +inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset) +{ + return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET, 1, &secondary_ch_offset, buf_len); +} + +inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl, + u8 flags, u16 reason, u16 precedence) +{ + u8 ie_data[6]; + + ie_data[0] = ttl; + ie_data[1] = flags; + RTW_PUT_LE16((u8 *)&ie_data[2], reason); + RTW_PUT_LE16((u8 *)&ie_data[4], precedence); + + return rtw_set_ie(buf, 0x118, 6, ie_data, buf_len); +} + +/*---------------------------------------------------------------------------- +index: the information element id index, limit is the limit for search +-----------------------------------------------------------------------------*/ +u8 *rtw_get_ie(const u8 *pbuf, sint index, sint *len, sint limit) +{ + sint tmp, i; + const u8 *p; + if (limit < 1) { + return NULL; + } + + p = pbuf; + i = 0; + *len = 0; + while (1) { + if (*p == index) { + *len = *(p + 1); + return (u8 *)p; + } else { + tmp = *(p + 1); + p += (tmp + 2); + i += (tmp + 2); + } + if (i >= limit) + break; + } + return NULL; +} + +/** + * rtw_get_ie_ex - Search specific IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @eid: Element ID to match + * @oui: OUI to match + * @oui_len: OUI length + * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE + * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE + * + * Returns: The address of the specific IE found, or NULL + */ +u8 *rtw_get_ie_ex(const u8 *in_ie, uint in_len, u8 eid, const u8 *oui, u8 oui_len, u8 *ie, uint *ielen) +{ + uint cnt; + const u8 *target_ie = NULL; + + + if (ielen) + *ielen = 0; + + if (!in_ie || in_len <= 0) + return (u8 *)target_ie; + + cnt = 0; + + while (cnt < in_len) { + if (eid == in_ie[cnt] + && (!oui || _rtw_memcmp(&in_ie[cnt + 2], oui, oui_len) == _TRUE)) { + target_ie = &in_ie[cnt]; + + if (ie) + _rtw_memcpy(ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + if (ielen) + *ielen = in_ie[cnt + 1] + 2; + + break; + } else { + cnt += in_ie[cnt + 1] + 2; /* goto next */ + } + + } + + return (u8 *)target_ie; +} + +/** + * rtw_ies_remove_ie - Find matching IEs and remove + * @ies: Address of IEs to search + * @ies_len: Pointer of length of ies, will update to new length + * @offset: The offset to start scarch + * @eid: Element ID to match + * @oui: OUI to match + * @oui_len: OUI length + * + * Returns: _SUCCESS: ies is updated, _FAIL: not updated + */ +int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len) +{ + int ret = _FAIL; + u8 *target_ie; + u32 target_ielen; + u8 *start; + uint search_len; + + if (!ies || !ies_len || *ies_len <= offset) + goto exit; + + start = ies + offset; + search_len = *ies_len - offset; + + while (1) { + target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen); + if (target_ie && target_ielen) { + u8 *remain_ies = target_ie + target_ielen; + uint remain_len = search_len - (remain_ies - start); + + _rtw_memmove(target_ie, remain_ies, remain_len); + *ies_len = *ies_len - target_ielen; + ret = _SUCCESS; + + start = target_ie; + search_len = remain_len; + } else + break; + } +exit: + return ret; +} + +void rtw_set_supported_rate(u8 *SupportedRates, uint mode) +{ + + _rtw_memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX); + + switch (mode) { + case WIRELESS_11B: + _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN); + break; + + case WIRELESS_11G: + case WIRELESS_11A: + case WIRELESS_11_5N: + case WIRELESS_11A_5N: /* Todo: no basic rate for ofdm ? */ + case WIRELESS_11_5AC: + _rtw_memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN); + break; + + case WIRELESS_11BG: + case WIRELESS_11G_24N: + case WIRELESS_11_24N: + case WIRELESS_11BG_24N: + _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN); + _rtw_memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN); + break; + + } +} + +void rtw_filter_suppport_rateie(WLAN_BSSID_EX *pbss_network, u8 keep) +{ + u8 i, idx = 0, new_rate[NDIS_802_11_LENGTH_RATES_EX], *p; + uint iscck, isofdm, ie_orilen = 0, remain_len; + u8 *remain_ies; + + p = rtw_get_ie(pbss_network->IEs + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_orilen, (pbss_network->IELength - _BEACON_IE_OFFSET_)); + if (!p) + return; + + _rtw_memset(new_rate, 0, NDIS_802_11_LENGTH_RATES_EX); + for (i=0; i < ie_orilen; i++) { + iscck = rtw_is_cck_rate(p[i+2]); + isofdm= rtw_is_ofdm_rate(p[i+2]); + if (((keep == CCK) && iscck) + || ((keep == OFDM) && isofdm)) + new_rate[idx++]= rtw_is_basic_rate_ofdm(p[i+2]) ? p[i+2]|IEEE80211_BASIC_RATE_MASK : p[i+2]; + } + /* update rate ie */ + p[1] = idx; + _rtw_memcpy(p+2, new_rate, idx); + /* update remain ie & IELength*/ + remain_ies = p + 2 + ie_orilen; + remain_len = pbss_network->IELength - (remain_ies - pbss_network->IEs); + _rtw_memmove(p+2+idx, remain_ies, remain_len); + pbss_network->IELength -= (ie_orilen - idx); +} + + +/* + Adjust those items by given wireless_mode + 1. pbss_network->IELength + 2. pbss_network->IE (SUPPORTRATE & EXT_SUPPORTRATE) + 3. pbss_network->SupportedRates +*/ + +u8 rtw_update_rate_bymode(WLAN_BSSID_EX *pbss_network, u32 mode) +{ + u8 network_type, *p, *ie = pbss_network->IEs; + sint ie_len; + uint network_ielen = pbss_network->IELength; + + if (mode == WIRELESS_11B) { + /*only keep CCK in support_rate IE and remove whole ext_support_rate IE*/ + rtw_filter_suppport_rateie(pbss_network, CCK); + p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_); + if (p) { + rtw_ies_remove_ie(ie , &network_ielen, _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, NULL, 0); + pbss_network->IELength -= ie_len; + } + network_type = WIRELESS_11B; + } else { + if (pbss_network->Configuration.DSConfig > 14) { + /* Remove CCK in support_rate IE */ + rtw_filter_suppport_rateie(pbss_network, OFDM); + network_type = WIRELESS_11A; + } else { + if ((mode & WIRELESS_11B) == 0) { + /* Remove CCK in support_rate IE */ + rtw_filter_suppport_rateie(pbss_network, OFDM); + network_type = WIRELESS_11G; + } else { + network_type = WIRELESS_11BG; + } + } + } + + rtw_set_supported_rate(pbss_network->SupportedRates, network_type); + + return network_type; +} + +uint rtw_get_rateset_len(u8 *rateset) +{ + uint i = 0; + while (1) { + if ((rateset[i]) == 0) + break; + + if (i > 12) + break; + + i++; + } + return i; +} + +int rtw_generate_ie(struct registry_priv *pregistrypriv) +{ + u8 wireless_mode; + int sz = 0, rateLen; + WLAN_BSSID_EX *pdev_network = &pregistrypriv->dev_network; + u8 *ie = pdev_network->IEs; + + + /* timestamp will be inserted by hardware */ + sz += 8; + ie += sz; + + /* beacon interval : 2bytes */ + *(u16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod); /* BCN_INTERVAL; */ + sz += 2; + ie += 2; + + /* capability info */ + *(u16 *)ie = 0; + + *(u16 *)ie |= cpu_to_le16(cap_IBSS); + + if (pregistrypriv->preamble == PREAMBLE_SHORT) + *(u16 *)ie |= cpu_to_le16(cap_ShortPremble); + + if (pdev_network->Privacy) + *(u16 *)ie |= cpu_to_le16(cap_Privacy); + + sz += 2; + ie += 2; + + /* SSID */ + ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz); + + /* supported rates */ + if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) { + if (pdev_network->Configuration.DSConfig > 14) + wireless_mode = WIRELESS_11A_5N; + else + wireless_mode = WIRELESS_11BG_24N; + } else if (pregistrypriv->wireless_mode == WIRELESS_MODE_MAX) { /* WIRELESS_11ABGN | WIRELESS_11AC */ + if (pdev_network->Configuration.DSConfig > 14) + wireless_mode = WIRELESS_11_5AC; + else + wireless_mode = WIRELESS_11BG_24N; + } else + wireless_mode = pregistrypriv->wireless_mode; + + rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode) ; + + rateLen = rtw_get_rateset_len(pdev_network->SupportedRates); + + if (rateLen > 8) { + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz); + /* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */ + } else + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz); + + /* DS parameter set */ + ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz); + + + /* IBSS Parameter Set */ + + ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz); + + if (rateLen > 8) + ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); + +#ifdef CONFIG_80211N_HT + /* HT Cap. */ + if (is_supported_ht(pregistrypriv->wireless_mode) + && (pregistrypriv->ht_enable == _TRUE)) { + /* todo: */ + } +#endif /* CONFIG_80211N_HT */ + + /* pdev_network->IELength = sz; */ /* update IELength */ + + + /* return _SUCCESS; */ + + return sz; + +} + +unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit) +{ + int len; + u16 val16; + unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01}; + u8 *pbuf = pie; + int limit_new = limit; + + while (1) { + pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new); + + if (pbuf) { + + /* check if oui matches... */ + if (_rtw_memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)) == _FALSE) + + goto check_next_ie; + + /* check version... */ + _rtw_memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16)); + + val16 = le16_to_cpu(val16); + if (val16 != 0x0001) + goto check_next_ie; + + *wpa_ie_len = *(pbuf + 1); + + return pbuf; + + } else { + + *wpa_ie_len = 0; + return NULL; + } + +check_next_ie: + + limit_new = limit - (pbuf - pie) - 2 - len; + + if (limit_new <= 0) + break; + + pbuf += (2 + len); + + } + + *wpa_ie_len = 0; + + return NULL; + +} + +unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit) +{ + + return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit); + +} + +int rtw_get_wpa_cipher_suite(u8 *s) +{ + if (_rtw_memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_NONE; + if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_WEP40; + if (_rtw_memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_TKIP; + if (_rtw_memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_CCMP; + if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_WEP104; + + return 0; +} + +int rtw_get_rsn_cipher_suite(u8 *s) +{ + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_NONE; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_WEP40; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_TKIP; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_CCMP; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_GCMP, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_GCMP; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_GCMP_256, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_GCMP_256; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP_256, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_CCMP_256; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_WEP104; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_AES_128_CMAC, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_BIP_CMAC_128; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_BIP_GMAC_128, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_BIP_GMAC_128; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_BIP_GMAC_256, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_BIP_GMAC_256; + if (_rtw_memcmp(s, RSN_CIPHER_SUITE_BIP_CMAC_256, RSN_SELECTOR_LEN) == _TRUE) + return WPA_CIPHER_BIP_CMAC_256; + return 0; +} + +u32 rtw_get_akm_suite_bitmap(u8 *s) +{ + if (_rtw_memcmp(s, WLAN_AKM_8021X, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_8021X; + if (_rtw_memcmp(s, WLAN_AKM_PSK, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_PSK; + if (_rtw_memcmp(s, WLAN_AKM_FT_8021X, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FT_8021X; + if (_rtw_memcmp(s, WLAN_AKM_FT_PSK, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FT_PSK; + if (_rtw_memcmp(s, WLAN_AKM_8021X_SHA256, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_8021X_SHA256; + if (_rtw_memcmp(s, WLAN_AKM_PSK_SHA256, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_PSK_SHA256; + if (_rtw_memcmp(s, WLAN_AKM_TDLS, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_TDLS; + if (_rtw_memcmp(s, WLAN_AKM_SAE, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_SAE; + if (_rtw_memcmp(s, WLAN_AKM_FT_OVER_SAE, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FT_OVER_SAE; + if (_rtw_memcmp(s, WLAN_AKM_8021X_SUITE_B, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_8021X_SUITE_B; + if (_rtw_memcmp(s, WLAN_AKM_8021X_SUITE_B_192, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_8021X_SUITE_B_192; + if (_rtw_memcmp(s, WLAN_AKM_FILS_SHA256, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FILS_SHA256; + if (_rtw_memcmp(s, WLAN_AKM_FILS_SHA384, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FILS_SHA384; + if (_rtw_memcmp(s, WLAN_AKM_FT_FILS_SHA256, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FT_FILS_SHA256; + if (_rtw_memcmp(s, WLAN_AKM_FT_FILS_SHA384, RSN_SELECTOR_LEN) == _TRUE) + return WLAN_AKM_TYPE_FT_FILS_SHA384; + + return 0; +} + +int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, + int *pairwise_cipher, u32 *akm) +{ + int i, ret = _SUCCESS; + int left, count; + u8 *pos; + u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1}; + + if (wpa_ie_len <= 0) { + /* No WPA IE - fail silently */ + return _FAIL; + } + + + if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie + 1) != (u8)(wpa_ie_len - 2)) || + (_rtw_memcmp(wpa_ie + 2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != _TRUE)) + return _FAIL; + + pos = wpa_ie; + + pos += 8; + left = wpa_ie_len - 8; + + + /* group_cipher */ + if (left >= WPA_SELECTOR_LEN) { + + *group_cipher = rtw_get_wpa_cipher_suite(pos); + + pos += WPA_SELECTOR_LEN; + left -= WPA_SELECTOR_LEN; + + } else if (left > 0) { + + return _FAIL; + } + + + /* pairwise_cipher */ + if (left >= 2) { + /* count = le16_to_cpu(*(u16*)pos); */ + count = RTW_GET_LE16(pos); + pos += 2; + left -= 2; + + if (count == 0 || left < count * WPA_SELECTOR_LEN) { + return _FAIL; + } + + for (i = 0; i < count; i++) { + *pairwise_cipher |= rtw_get_wpa_cipher_suite(pos); + + pos += WPA_SELECTOR_LEN; + left -= WPA_SELECTOR_LEN; + } + + } else if (left == 1) { + return _FAIL; + } + + if (akm) { + if (left >= 6) { + pos += 2; + if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) { + *akm = WLAN_AKM_TYPE_8021X; + } + } + } + + return ret; + +} + +int rtw_rsne_info_parse(const u8 *ie, uint ie_len, struct rsne_info *info) +{ + const u8 *pos = ie; + u16 ver; + u16 cnt; + + _rtw_memset(info, 0, sizeof(struct rsne_info)); + + if (ie + ie_len < pos + 4) + goto err; + + if (*ie != WLAN_EID_RSN || *(ie + 1) != ie_len - 2) + goto err; + pos += 2; + + /* Version */ + ver = RTW_GET_LE16(pos); + if(1 != ver) + goto err; + pos += 2; + + /* Group CS */ + if (ie + ie_len < pos + 4) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + info->gcs = (u8 *)pos; + pos += 4; + + /* Pairwise CS */ + if (ie + ie_len < pos + 2) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + cnt = RTW_GET_LE16(pos); + pos += 2; + if (ie + ie_len < pos + 4 * cnt) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + info->pcs_cnt = cnt; + info->pcs_list = (u8 *)pos; + pos += 4 * cnt; + + /* AKM */ + if (ie + ie_len < pos + 2) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + cnt = RTW_GET_LE16(pos); + pos += 2; + if (ie + ie_len < pos + 4 * cnt) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + info->akm_cnt = cnt; + info->akm_list = (u8 *)pos; + pos += 4 * cnt; + + /* RSN cap */ + if (ie + ie_len < pos + 2) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + info->cap = (u8 *)pos; + pos += 2; + + /* PMKID */ + if (ie + ie_len < pos + 2) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + cnt = RTW_GET_LE16(pos); + pos += 2; + if (ie + ie_len < pos + 16 * cnt) + goto err; + info->pmkid_cnt = cnt; + info->pmkid_list = (u8 *)pos; + pos += 16 * cnt; + + /* Group Mgmt CS */ + if (ie + ie_len < pos + 4) { + if (ie + ie_len != pos) + goto err; + goto exit; + } + info->gmcs = (u8 *)pos; + +exit: + return _SUCCESS; + +err: + info->err = 1; + return _FAIL; +} + +int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, + int *pairwise_cipher, int *gmcs, u32 *akm, u8 *mfp_opt, u8 *spp_opt) +{ + struct rsne_info info; + int i, ret = _SUCCESS; + + ret = rtw_rsne_info_parse(rsn_ie, rsn_ie_len, &info); + if (ret != _SUCCESS) + goto exit; + + if (group_cipher) { + if (info.gcs) + *group_cipher = rtw_get_rsn_cipher_suite(info.gcs); + else + *group_cipher = 0; + } + + if (pairwise_cipher) { + *pairwise_cipher = 0; + if (info.pcs_list) { + for (i = 0; i < info.pcs_cnt; i++) + *pairwise_cipher |= rtw_get_rsn_cipher_suite(info.pcs_list + 4 * i); + } + } + + if (gmcs) { + if (info.gmcs) + *gmcs = rtw_get_rsn_cipher_suite(info.gmcs); + else + *gmcs = WPA_CIPHER_BIP_CMAC_128; /* default value when absent */ + } + + if (akm) { + *akm = 0; + if (info.akm_list) { + for (i = 0; i < info.akm_cnt; i++) + *akm |= rtw_get_akm_suite_bitmap(info.akm_list + 4 * i); + } + } + + if (mfp_opt) { + *mfp_opt = MFP_NO; + if (info.cap) + *mfp_opt = GET_RSN_CAP_MFP_OPTION(info.cap); + } + + if (spp_opt) { + *spp_opt = 0; + if (info.cap) + *spp_opt = GET_RSN_CAP_SPP_OPT(info.cap); + } + +exit: + return ret; +} + +/* #ifdef CONFIG_WAPI_SUPPORT */ +int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len) +{ + int len = 0; + u8 authmode; + uint cnt; + u8 wapi_oui1[4] = {0x0, 0x14, 0x72, 0x01}; + u8 wapi_oui2[4] = {0x0, 0x14, 0x72, 0x02}; + + + if (wapi_len) + *wapi_len = 0; + + if (!in_ie || in_len <= 0) + return len; + + cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_); + + while (cnt < in_len) { + authmode = in_ie[cnt]; + + /* if(authmode==_WAPI_IE_) */ + if (authmode == _WAPI_IE_ && (_rtw_memcmp(&in_ie[cnt + 6], wapi_oui1, 4) == _TRUE || + _rtw_memcmp(&in_ie[cnt + 6], wapi_oui2, 4) == _TRUE)) { + if (wapi_ie) + _rtw_memcpy(wapi_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + if (wapi_len) + *wapi_len = in_ie[cnt + 1] + 2; + + cnt += in_ie[cnt + 1] + 2; /* get next */ + } else { + cnt += in_ie[cnt + 1] + 2; /* get next */ + } + } + + if (wapi_len) + len = *wapi_len; + + + return len; + +} +/* #endif */ + +int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len) +{ + u8 authmode, sec_idx; + u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01}; + uint cnt; + + + /* Search required WPA or WPA2 IE and copy to sec_ie[ ] */ + + cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_); + + sec_idx = 0; + + while (cnt < in_len) { + authmode = in_ie[cnt]; + + if ((authmode == _WPA_IE_ID_) && (_rtw_memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4) == _TRUE)) { + + if (wpa_ie) + _rtw_memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + *wpa_len = in_ie[cnt + 1] + 2; + cnt += in_ie[cnt + 1] + 2; /* get next */ + } else { + if (authmode == _WPA2_IE_ID_) { + + if (rsn_ie) + _rtw_memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + *rsn_len = in_ie[cnt + 1] + 2; + cnt += in_ie[cnt + 1] + 2; /* get next */ + } else { + cnt += in_ie[cnt + 1] + 2; /* get next */ + } + } + + } + + + return *rsn_len + *wpa_len; + +} + +u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen) +{ + u8 match = _FALSE; + u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04}; + + if (ie_ptr == NULL) + return match; + + eid = ie_ptr[0]; + + if ((eid == _WPA_IE_ID_) && (_rtw_memcmp(&ie_ptr[2], wps_oui, 4) == _TRUE)) { + /* RTW_INFO("==> found WPS_IE.....\n"); */ + *wps_ielen = ie_ptr[1] + 2; + match = _TRUE; + } + return match; +} + +u8 *rtw_get_wps_ie_from_scan_queue(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen, enum bss_type frame_type) +{ + u8 *wps = NULL; + + RTW_INFO("[%s] frame_type = %d\n", __FUNCTION__, frame_type); + switch (frame_type) { + case BSS_TYPE_BCN: + case BSS_TYPE_PROB_RSP: { + /* Beacon or Probe Response */ + wps = rtw_get_wps_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, wps_ie, wps_ielen); + break; + } + case BSS_TYPE_PROB_REQ: { + /* Probe Request */ + wps = rtw_get_wps_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , wps_ie, wps_ielen); + break; + } + default: + case BSS_TYPE_UNDEF: + break; + } + return wps; +} + +/** + * rtw_get_wps_ie - Search WPS IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie + * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE + * + * Returns: The address of the WPS IE found, or NULL + */ +u8 *rtw_get_wps_ie(const u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen) +{ + uint cnt; + const u8 *wpsie_ptr = NULL; + u8 eid, wps_oui[4] = {0x00, 0x50, 0xf2, 0x04}; + + if (wps_ielen) + *wps_ielen = 0; + + if (!in_ie) { + rtw_warn_on(1); + return (u8 *)wpsie_ptr; + } + + if (in_len <= 0) + return (u8 *)wpsie_ptr; + + cnt = 0; + + while (cnt + 1 + 4 < in_len) { + eid = in_ie[cnt]; + + if (cnt + 1 + 4 >= MAX_IE_SZ) { + rtw_warn_on(1); + return NULL; + } + + if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], wps_oui, 4) == _TRUE) { + wpsie_ptr = in_ie + cnt; + + if (wps_ie) + _rtw_memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + if (wps_ielen) + *wps_ielen = in_ie[cnt + 1] + 2; + + break; + } else + cnt += in_ie[cnt + 1] + 2; + + } + + return (u8 *)wpsie_ptr; +} + +/** + * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE + * @wps_ie: Address of WPS IE to search + * @wps_ielen: Length limit from wps_ie + * @target_attr_id: The attribute ID of WPS attribute to search + * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr + * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute + * + * Returns: the address of the specific WPS attribute found, or NULL + */ +u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr) +{ + u8 *attr_ptr = NULL; + u8 *target_attr_ptr = NULL; + u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04}; + + if (len_attr) + *len_attr = 0; + + if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) || + (_rtw_memcmp(wps_ie + 2, wps_oui , 4) != _TRUE)) + return attr_ptr; + + /* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */ + attr_ptr = wps_ie + 6; /* goto first attr */ + + while (attr_ptr - wps_ie < wps_ielen) { + /* 4 = 2(Attribute ID) + 2(Length) */ + u16 attr_id = RTW_GET_BE16(attr_ptr); + u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2); + u16 attr_len = attr_data_len + 4; + + /* RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len); */ + if (attr_id == target_attr_id) { + target_attr_ptr = attr_ptr; + + if (buf_attr) + _rtw_memcpy(buf_attr, attr_ptr, attr_len); + + if (len_attr) + *len_attr = attr_len; + + break; + } else { + attr_ptr += attr_len; /* goto next */ + } + + } + + return target_attr_ptr; +} + +/** + * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE + * @wps_ie: Address of WPS IE to search + * @wps_ielen: Length limit from wps_ie + * @target_attr_id: The attribute ID of WPS attribute to search + * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content + * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content + * + * Returns: the address of the specific WPS attribute content found, or NULL + */ +u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_content, uint *len_content) +{ + u8 *attr_ptr; + u32 attr_len; + + if (len_content) + *len_content = 0; + + attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len); + + if (attr_ptr && attr_len) { + if (buf_content) + _rtw_memcpy(buf_content, attr_ptr + 4, attr_len - 4); + + if (len_content) + *len_content = attr_len - 4; + + return attr_ptr + 4; + } + + return NULL; +} + +/* OWE */ + +/** + * rtw_get_OWE_ie - Search OWE IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @wps_ie: If not NULL and OWE IE is found, OWE IE will be copied to the buf starting from owe_ie + * @wps_ielen: If not NULL and OWE IE is found, will set to the length of the entire OWE IE + * + * Returns: The address of the OWE IE found, or NULL + */ +u8 *rtw_get_owe_ie(const u8 *in_ie, uint in_len, u8 *owe_ie, uint *owe_ielen) +{ + uint cnt; + const u8 *oweie_ptr = NULL; + u8 eid; + + if (owe_ielen) + *owe_ielen = 0; + + if (!in_ie) { + rtw_warn_on(1); + return (u8 *)oweie_ptr; + } + + if (in_len <= 0) + return (u8 *)oweie_ptr; + + cnt = 0; + + while (cnt + 1 + 4 < in_len) { + eid = in_ie[cnt]; + + if (cnt + 1 + 4 >= MAX_IE_SZ) { + rtw_warn_on(1); + return NULL; + } + + if ((eid == WLAN_EID_EXTENSION) && (in_ie[cnt + 2] == WLAN_EID_EXT_OWE_DH_PARAM)) { + oweie_ptr = in_ie + cnt; + + if (owe_ie) + _rtw_memcpy(owe_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + if (owe_ielen) + *owe_ielen = in_ie[cnt + 1] + 2; + + break; + } else + cnt += in_ie[cnt + 1] + 2; + + } + + return (u8 *)oweie_ptr; +} + +static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen, + struct rtw_ieee802_11_elems *elems, + int show_errors) +{ + unsigned int oui; + + /* first 3 bytes in vendor specific information element are the IEEE + * OUI of the vendor. The following byte is used a vendor specific + * sub-type. */ + if (elen < 4) { + if (show_errors) { + RTW_INFO("short vendor specific " + "information element ignored (len=%lu)\n", + (unsigned long) elen); + } + return -1; + } + + oui = RTW_GET_BE24(pos); + switch (oui) { + case OUI_MICROSOFT: + /* Microsoft/Wi-Fi information elements are further typed and + * subtyped */ + switch (pos[3]) { + case 1: + /* Microsoft OUI (00:50:F2) with OUI Type 1: + * real WPA information element */ + elems->wpa_ie = pos; + elems->wpa_ie_len = elen; + break; + case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */ + if (elen < 5) { + RTW_DBG("short WME " + "information element ignored " + "(len=%lu)\n", + (unsigned long) elen); + return -1; + } + switch (pos[4]) { + case WME_OUI_SUBTYPE_INFORMATION_ELEMENT: + case WME_OUI_SUBTYPE_PARAMETER_ELEMENT: + elems->wme = pos; + elems->wme_len = elen; + break; + case WME_OUI_SUBTYPE_TSPEC_ELEMENT: + elems->wme_tspec = pos; + elems->wme_tspec_len = elen; + break; + default: + RTW_DBG("unknown WME " + "information element ignored " + "(subtype=%d len=%lu)\n", + pos[4], (unsigned long) elen); + return -1; + } + break; + case 4: + /* Wi-Fi Protected Setup (WPS) IE */ + elems->wps_ie = pos; + elems->wps_ie_len = elen; + break; + default: + RTW_DBG("Unknown Microsoft " + "information element ignored " + "(type=%d len=%lu)\n", + pos[3], (unsigned long) elen); + return -1; + } + break; + + case OUI_BROADCOM: + switch (pos[3]) { + case VENDOR_HT_CAPAB_OUI_TYPE: + elems->vendor_ht_cap = pos; + elems->vendor_ht_cap_len = elen; + break; + default: + RTW_DBG("Unknown Broadcom " + "information element ignored " + "(type=%d len=%lu)\n", + pos[3], (unsigned long) elen); + return -1; + } + break; +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + case OUI_REALTEK: + if (elen == 8) { // TBTX capable IE length is 8 + elems->tbtx_cap = pos; + elems->tbtx_cap_len = elen; + } + break; +#endif + default: + RTW_DBG("unknown vendor specific information " + "element ignored (vendor OUI %02x:%02x:%02x " + "len=%lu)\n", + pos[0], pos[1], pos[2], (unsigned long) elen); + return -1; + } + + return 0; + +} + +/** + * ieee802_11_parse_elems - Parse information elements in management frames + * @start: Pointer to the start of IEs + * @len: Length of IE buffer in octets + * @elems: Data structure for parsed elements + * @show_errors: Whether to show parsing errors in debug log + * Returns: Parsing result + */ +ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len, + struct rtw_ieee802_11_elems *elems, + int show_errors) +{ + uint left = len; + u8 *pos = start; + int unknown = 0; + + _rtw_memset(elems, 0, sizeof(*elems)); + + while (left >= 2) { + u8 id, elen; + + id = *pos++; + elen = *pos++; + left -= 2; + + if (elen > left) { + if (show_errors) { + RTW_INFO("IEEE 802.11 element " + "parse failed (id=%d elen=%d " + "left=%lu)\n", + id, elen, (unsigned long) left); + } + return ParseFailed; + } + + switch (id) { + case WLAN_EID_SSID: + elems->ssid = pos; + elems->ssid_len = elen; + break; + case WLAN_EID_SUPP_RATES: + elems->supp_rates = pos; + elems->supp_rates_len = elen; + break; + case WLAN_EID_FH_PARAMS: + elems->fh_params = pos; + elems->fh_params_len = elen; + break; + case WLAN_EID_DS_PARAMS: + elems->ds_params = pos; + elems->ds_params_len = elen; + break; + case WLAN_EID_CF_PARAMS: + elems->cf_params = pos; + elems->cf_params_len = elen; + break; + case WLAN_EID_TIM: + elems->tim = pos; + elems->tim_len = elen; + break; + case WLAN_EID_IBSS_PARAMS: + elems->ibss_params = pos; + elems->ibss_params_len = elen; + break; + case WLAN_EID_CHALLENGE: + elems->challenge = pos; + elems->challenge_len = elen; + break; + case WLAN_EID_ERP_INFO: + elems->erp_info = pos; + elems->erp_info_len = elen; + break; + case WLAN_EID_EXT_SUPP_RATES: + elems->ext_supp_rates = pos; + elems->ext_supp_rates_len = elen; + break; + case WLAN_EID_VENDOR_SPECIFIC: + if (rtw_ieee802_11_parse_vendor_specific(pos, elen, + elems, + show_errors)) + unknown++; + break; + case WLAN_EID_RSN: + elems->rsn_ie = pos; + elems->rsn_ie_len = elen; + break; + case WLAN_EID_PWR_CAPABILITY: + elems->power_cap = pos; + elems->power_cap_len = elen; + break; + case WLAN_EID_SUPPORTED_CHANNELS: + elems->supp_channels = pos; + elems->supp_channels_len = elen; + break; + case WLAN_EID_MOBILITY_DOMAIN: + elems->mdie = pos; + elems->mdie_len = elen; + break; + case WLAN_EID_FAST_BSS_TRANSITION: + elems->ftie = pos; + elems->ftie_len = elen; + break; + case WLAN_EID_TIMEOUT_INTERVAL: + elems->timeout_int = pos; + elems->timeout_int_len = elen; + break; + case WLAN_EID_HT_CAP: + elems->ht_capabilities = pos; + elems->ht_capabilities_len = elen; + break; + case WLAN_EID_HT_OPERATION: + elems->ht_operation = pos; + elems->ht_operation_len = elen; + break; + case WLAN_EID_VHT_CAPABILITY: + elems->vht_capabilities = pos; + elems->vht_capabilities_len = elen; + break; + case WLAN_EID_VHT_OPERATION: + elems->vht_operation = pos; + elems->vht_operation_len = elen; + break; + case WLAN_EID_VHT_OP_MODE_NOTIFY: + elems->vht_op_mode_notify = pos; + elems->vht_op_mode_notify_len = elen; + break; + case _EID_RRM_EN_CAP_IE_: + elems->rm_en_cap = pos; + elems->rm_en_cap_len = elen; + break; +#ifdef CONFIG_RTW_MESH + case WLAN_EID_PREQ: + elems->preq = pos; + elems->preq_len = elen; + break; + case WLAN_EID_PREP: + elems->prep = pos; + elems->prep_len = elen; + break; + case WLAN_EID_PERR: + elems->perr = pos; + elems->perr_len = elen; + break; + case WLAN_EID_RANN: + elems->rann = pos; + elems->rann_len = elen; + break; +#endif + default: + unknown++; + if (!show_errors) + break; + RTW_DBG("IEEE 802.11 element parse " + "ignored unknown element (id=%d elen=%d)\n", + id, elen); + break; + } + + left -= elen; + pos += elen; + } + + if (left) + return ParseFailed; + + return unknown ? ParseUnknown : ParseOK; + +} + +static u8 key_char2num(u8 ch); +static u8 key_char2num(u8 ch) +{ + if ((ch >= '0') && (ch <= '9')) + return ch - '0'; + else if ((ch >= 'a') && (ch <= 'f')) + return ch - 'a' + 10; + else if ((ch >= 'A') && (ch <= 'F')) + return ch - 'A' + 10; + else + return 0xff; +} + +u8 str_2char2num(u8 hch, u8 lch); +u8 str_2char2num(u8 hch, u8 lch) +{ + return (key_char2num(hch) * 10) + key_char2num(lch); +} + +u8 key_2char2num(u8 hch, u8 lch); +u8 key_2char2num(u8 hch, u8 lch) +{ + return (key_char2num(hch) << 4) | key_char2num(lch); +} + +void macstr2num(u8 *dst, u8 *src); +void macstr2num(u8 *dst, u8 *src) +{ + int jj, kk; + for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3) + dst[jj] = key_2char2num(src[kk], src[kk + 1]); +} + +u8 convert_ip_addr(u8 hch, u8 mch, u8 lch) +{ + return (key_char2num(hch) * 100) + (key_char2num(mch) * 10) + key_char2num(lch); +} + +#ifdef CONFIG_PLATFORM_INTEL_BYT +#define MAC_ADDRESS_LEN 12 + +int rtw_get_mac_addr_intel(unsigned char *buf) +{ + int ret = 0; + int i; + struct file *fp = NULL; + mm_segment_t oldfs; + unsigned char c_mac[MAC_ADDRESS_LEN]; + char fname[] = "/config/wifi/mac.txt"; + int jj, kk; + + RTW_INFO("%s Enter\n", __FUNCTION__); + + ret = rtw_retrieve_from_file(fname, c_mac, MAC_ADDRESS_LEN); + if (ret < MAC_ADDRESS_LEN) + return -1; + + for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 2) + buf[jj] = key_2char2num(c_mac[kk], c_mac[kk + 1]); + + RTW_INFO("%s: read from file mac address: "MAC_FMT"\n", + __FUNCTION__, MAC_ARG(buf)); + + return 0; +} +#endif /* CONFIG_PLATFORM_INTEL_BYT */ + +/* + * Description: + * rtw_check_invalid_mac_address: + * This is only used for checking mac address valid or not. + * + * Input: + * adapter: mac_address pointer. + * check_local_bit: check locally bit or not. + * + * Output: + * _TRUE: The mac address is invalid. + * _FALSE: The mac address is valid. + * + * Auther: Isaac.Li + */ +u8 rtw_check_invalid_mac_address(u8 *mac_addr, u8 check_local_bit) +{ + u8 null_mac_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0}; + u8 multi_mac_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 res = _FALSE; + + if (_rtw_memcmp(mac_addr, null_mac_addr, ETH_ALEN)) { + res = _TRUE; + goto func_exit; + } + + if (_rtw_memcmp(mac_addr, multi_mac_addr, ETH_ALEN)) { + res = _TRUE; + goto func_exit; + } + + if (mac_addr[0] & BIT0) { + res = _TRUE; + goto func_exit; + } + + if (check_local_bit == _TRUE) { + if (mac_addr[0] & BIT1) { + res = _TRUE; + goto func_exit; + } + } + +func_exit: + return res; +} + +extern char *rtw_initmac; +/** + * rtw_macaddr_cfg - Decide the mac address used + * @out: buf to store mac address decided + * @hw_mac_addr: mac address from efuse/epprom + */ +void rtw_macaddr_cfg(u8 *out, const u8 *hw_mac_addr) +{ +#define DEFAULT_RANDOM_MACADDR 1 + u8 mac[ETH_ALEN]; + + if (out == NULL) { + rtw_warn_on(1); + return; + } + + /* Users specify the mac address */ + if (rtw_initmac) { + int jj, kk; + + for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3) + mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]); + + goto err_chk; + } + + /* platform specified */ +#ifdef CONFIG_PLATFORM_INTEL_BYT + if (rtw_get_mac_addr_intel(mac) == 0) + goto err_chk; +#endif + + /* Use the mac address stored in the Efuse */ + if (hw_mac_addr) { + _rtw_memcpy(mac, hw_mac_addr, ETH_ALEN); + goto err_chk; + } + +err_chk: + if (rtw_check_invalid_mac_address(mac, _TRUE) == _TRUE) { +#if DEFAULT_RANDOM_MACADDR + RTW_ERR("invalid mac addr:"MAC_FMT", assign random MAC\n", MAC_ARG(mac)); + *((u32 *)(&mac[2])) = rtw_random32(); + mac[0] = 0x00; + mac[1] = 0xe0; + mac[2] = 0x4c; +#else + RTW_ERR("invalid mac addr:"MAC_FMT", assign default one\n", MAC_ARG(mac)); + mac[0] = 0x00; + mac[1] = 0xe0; + mac[2] = 0x4c; + mac[3] = 0x87; + mac[4] = 0x00; + mac[5] = 0x00; +#endif + } + + _rtw_memcpy(out, mac, ETH_ALEN); + RTW_INFO("%s mac addr:"MAC_FMT"\n", __func__, MAC_ARG(out)); +} + +#ifdef CONFIG_RTW_DEBUG +#ifdef CONFIG_80211N_HT +void dump_ht_cap_ie_content(void *sel, const u8 *buf, u32 buf_len) +{ + if (buf_len != HT_CAP_IE_LEN) { + RTW_PRINT_SEL(sel, "Invalid HT capability IE len:%d != %d\n", buf_len, HT_CAP_IE_LEN); + return; + } + + RTW_PRINT_SEL(sel, "cap_info:%02x%02x:%s\n", *(buf), *(buf + 1) + , GET_HT_CAP_ELE_CHL_WIDTH(buf) ? " 40MHz" : " 20MHz"); + RTW_PRINT_SEL(sel, "A-MPDU Parameters:"HT_AMPDU_PARA_FMT"\n" + , HT_AMPDU_PARA_ARG(HT_CAP_ELE_AMPDU_PARA(buf))); + RTW_PRINT_SEL(sel, "Supported MCS Set:"HT_SUP_MCS_SET_FMT"\n" + , HT_SUP_MCS_SET_ARG(HT_CAP_ELE_SUP_MCS_SET(buf))); +} + +void dump_ht_cap_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *ht_cap_ie; + sint ht_cap_ielen; + + ht_cap_ie = rtw_get_ie(ie, WLAN_EID_HT_CAP, &ht_cap_ielen, ie_len); + if (!ie || ht_cap_ie != ie) + return; + + dump_ht_cap_ie_content(sel, ht_cap_ie + 2, ht_cap_ielen); +} + +const char *const _ht_sc_offset_str[] = { + "SCN", + "SCA", + "SC-RSVD", + "SCB", +}; + +void dump_ht_op_ie_content(void *sel, const u8 *buf, u32 buf_len) +{ + if (buf_len != HT_OP_IE_LEN) { + RTW_PRINT_SEL(sel, "Invalid HT operation IE len:%d != %d\n", buf_len, HT_OP_IE_LEN); + return; + } + + RTW_PRINT_SEL(sel, "ch:%u%s %s\n" + , GET_HT_OP_ELE_PRI_CHL(buf) + , GET_HT_OP_ELE_STA_CHL_WIDTH(buf) ? "" : " 20MHz only" + , ht_sc_offset_str(GET_HT_OP_ELE_2ND_CHL_OFFSET(buf)) + ); +} + +void dump_ht_op_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *ht_op_ie; + sint ht_op_ielen; + + ht_op_ie = rtw_get_ie(ie, WLAN_EID_HT_OPERATION, &ht_op_ielen, ie_len); + if (!ie || ht_op_ie != ie) + return; + + dump_ht_op_ie_content(sel, ht_op_ie + 2, ht_op_ielen); +} +#endif /* CONFIG_80211N_HT */ + +void dump_wps_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *pos = ie; + u16 id; + u16 len; + + const u8 *wps_ie; + uint wps_ielen; + + wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen); + if (wps_ie != ie || wps_ielen == 0) + return; + + pos += 6; + while (pos - ie + 4 <= ie_len) { + id = RTW_GET_BE16(pos); + len = RTW_GET_BE16(pos + 2); + + RTW_PRINT_SEL(sel, "%s ID:0x%04x, LEN:%u%s\n", __func__, id, len + , ((pos - ie + 4 + len) <= ie_len) ? "" : "(exceed ie_len)"); + + pos += (4 + len); + } +} +#endif /* CONFIG_RTW_DEBUG */ +void dump_ies(void *sel, const u8 *buf, u32 buf_len) +{ +#ifdef CONFIG_RTW_DEBUG + const u8 *pos = buf; + u8 id, len; + + while (pos - buf + 1 < buf_len) { + id = *pos; + len = *(pos + 1); + + RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u\n", __FUNCTION__, id, len); +#ifdef CONFIG_80211N_HT + dump_ht_cap_ie(sel, pos, len + 2); + dump_ht_op_ie(sel, pos, len + 2); +#endif +#ifdef CONFIG_80211AC_VHT + dump_vht_cap_ie(sel, pos, len + 2); + dump_vht_op_ie(sel, pos, len + 2); +#endif + dump_wps_ie(sel, pos, len + 2); +#ifdef CONFIG_P2P + dump_p2p_ie(sel, pos, len + 2); +#ifdef CONFIG_WFD + dump_wfd_ie(sel, pos, len + 2); +#endif +#endif +#ifdef CONFIG_RTW_MULTI_AP + dump_multi_ap_ie(sel, pos, len + 2); +#endif + + pos += (2 + len); + } +#endif /* CONFIG_RTW_DEBUG */ +} + +/** + * rtw_ies_get_chbw - get operation ch, bw, offset from IEs of BSS. + * @ies: pointer of the first tlv IE + * @ies_len: length of @ies + * @ch: pointer of ch, used as output + * @bw: pointer of bw, used as output + * @offset: pointer of offset, used as output + * @ht: check HT IEs + * @vht: check VHT IEs, if true imply ht is true + */ +void rtw_ies_get_chbw(u8 *ies, int ies_len, u8 *ch, u8 *bw, u8 *offset, u8 ht, u8 vht) +{ + u8 *p; + int ie_len; + + *ch = 0; + *bw = CHANNEL_WIDTH_20; + *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + p = rtw_get_ie(ies, _DSSET_IE_, &ie_len, ies_len); + if (p && ie_len > 0) + *ch = *(p + 2); + +#ifdef CONFIG_80211N_HT + if (ht || vht) { + u8 *ht_cap_ie, *ht_op_ie; + int ht_cap_ielen, ht_op_ielen; + + ht_cap_ie = rtw_get_ie(ies, EID_HTCapability, &ht_cap_ielen, ies_len); + if (ht_cap_ie && ht_cap_ielen) { + if (GET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2)) + *bw = CHANNEL_WIDTH_40; + } + + ht_op_ie = rtw_get_ie(ies, EID_HTInfo, &ht_op_ielen, ies_len); + if (ht_op_ie && ht_op_ielen) { + if (*ch == 0) + *ch = GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2); + else if (*ch != 0 && *ch != GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2)) { + RTW_INFO("%s ch inconsistent, DSSS:%u, HT primary:%u\n" + , __func__, *ch, GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2)); + } + + if (!GET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2)) + *bw = CHANNEL_WIDTH_20; + + if (*bw == CHANNEL_WIDTH_40) { + switch (GET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2)) { + case SCA: + *offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + case SCB: + *offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + } + } + } + +#ifdef CONFIG_80211AC_VHT + if (vht) { + u8 *vht_op_ie; + int vht_op_ielen; + + vht_op_ie = rtw_get_ie(ies, EID_VHTOperation, &vht_op_ielen, ies_len); + if (vht_op_ie && vht_op_ielen) { + if (GET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2) >= 1) + *bw = CHANNEL_WIDTH_80; + } + } +#endif /* CONFIG_80211AC_VHT */ + + } +#endif /* CONFIG_80211N_HT */ +} + +void rtw_bss_get_chbw(WLAN_BSSID_EX *bss, u8 *ch, u8 *bw, u8 *offset, u8 ht, u8 vht) +{ + rtw_ies_get_chbw(bss->IEs + sizeof(NDIS_802_11_FIXED_IEs) + , bss->IELength - sizeof(NDIS_802_11_FIXED_IEs) + , ch, bw, offset, ht, vht); + + if (*ch == 0) + *ch = bss->Configuration.DSConfig; + else if (*ch != bss->Configuration.DSConfig) { + RTW_INFO("inconsistent ch - ies:%u bss->Configuration.DSConfig:%u\n" + , *ch, bss->Configuration.DSConfig); + *ch = bss->Configuration.DSConfig; + rtw_warn_on(1); + } +} + +/** + * rtw_is_chbw_grouped - test if the two ch settings can be grouped together + * @ch_a: ch of set a + * @bw_a: bw of set a + * @offset_a: offset of set a + * @ch_b: ch of set b + * @bw_b: bw of set b + * @offset_b: offset of set b + */ +bool rtw_is_chbw_grouped(u8 ch_a, u8 bw_a, u8 offset_a + , u8 ch_b, u8 bw_b, u8 offset_b) +{ + bool is_grouped = _FALSE; + + if (ch_a != ch_b) { + /* ch is different */ + goto exit; + } else if ((bw_a == CHANNEL_WIDTH_40 || bw_a == CHANNEL_WIDTH_80) + && (bw_b == CHANNEL_WIDTH_40 || bw_b == CHANNEL_WIDTH_80) + ) { + if (offset_a != offset_b) + goto exit; + } + + is_grouped = _TRUE; + +exit: + return is_grouped; +} + +/** + * rtw_sync_chbw - obey g_ch, adjust g_bw, g_offset, bw, offset + * @req_ch: pointer of the request ch, may be modified further + * @req_bw: pointer of the request bw, may be modified further + * @req_offset: pointer of the request offset, may be modified further + * @g_ch: pointer of the ongoing group ch + * @g_bw: pointer of the ongoing group bw, may be modified further + * @g_offset: pointer of the ongoing group offset, may be modified further + */ +void rtw_sync_chbw(u8 *req_ch, u8 *req_bw, u8 *req_offset + , u8 *g_ch, u8 *g_bw, u8 *g_offset) +{ + + *req_ch = *g_ch; + + if (*req_bw == CHANNEL_WIDTH_80 && *g_ch <= 14) { + /*2.4G ch, downgrade to 40Mhz */ + *req_bw = CHANNEL_WIDTH_40; + } + + switch (*req_bw) { + case CHANNEL_WIDTH_80: + if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80) + *req_offset = *g_offset; + else if (*g_bw == CHANNEL_WIDTH_20) + rtw_get_offset_by_chbw(*req_ch, *req_bw, req_offset); + + if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) { + RTW_ERR("%s req 80MHz BW without offset, down to 20MHz\n", __func__); + rtw_warn_on(1); + *req_bw = CHANNEL_WIDTH_20; + } + break; + case CHANNEL_WIDTH_40: + if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80) + *req_offset = *g_offset; + else if (*g_bw == CHANNEL_WIDTH_20) + rtw_get_offset_by_chbw(*req_ch, *req_bw, req_offset); + + if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) { + RTW_ERR("%s req 40MHz BW without offset, down to 20MHz\n", __func__); + rtw_warn_on(1); + *req_bw = CHANNEL_WIDTH_20; + } + break; + case CHANNEL_WIDTH_20: + *req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + default: + RTW_ERR("%s req unsupported BW:%u\n", __func__, *req_bw); + rtw_warn_on(1); + } + + if (*req_bw > *g_bw) { + *g_bw = *req_bw; + *g_offset = *req_offset; + } +} + +#ifdef CONFIG_P2P +/** + * rtw_get_p2p_merged_len - Get merged ie length from muitiple p2p ies. + * @in_ie: Pointer of the first p2p ie + * @in_len: Total len of muiltiple p2p ies + * Returns: Length of merged p2p ie length + */ +u32 rtw_get_p2p_merged_ies_len(u8 *in_ie, u32 in_len) +{ + PNDIS_802_11_VARIABLE_IEs pIE; + u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 }; + int i = 0; + int len = 0; + + while (i < in_len) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i); + + if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4)) { + len += pIE->Length - 4; /* 4 is P2P OUI length, don't count it in this loop */ + } + + i += (pIE->Length + 2); + } + + return len + 4; /* Append P2P OUI length at last. */ +} + +/** + * rtw_p2p_merge_ies - Merge muitiple p2p ies into one + * @in_ie: Pointer of the first p2p ie + * @in_len: Total len of muiltiple p2p ies + * @merge_ie: Pointer of merged ie + * Returns: Length of merged p2p ie + */ +int rtw_p2p_merge_ies(u8 *in_ie, u32 in_len, u8 *merge_ie) +{ + PNDIS_802_11_VARIABLE_IEs pIE; + u8 len = 0; + u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 }; + u8 ELOUI[6] = { 0xDD, 0x00, 0x50, 0x6f, 0x9a, 0x09 }; /* EID;Len;OUI, Len would copy at the end of function */ + int i = 0; + + if (merge_ie != NULL) { + /* Set first P2P OUI */ + _rtw_memcpy(merge_ie, ELOUI, 6); + merge_ie += 6; + + while (i < in_len) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i); + + /* Take out the rest of P2P OUIs */ + if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4)) { + _rtw_memcpy(merge_ie, pIE->data + 4, pIE->Length - 4); + len += pIE->Length - 4; + merge_ie += pIE->Length - 4; + } + + i += (pIE->Length + 2); + } + + return len + 4; /* 4 is for P2P OUI */ + + } + + return 0; +} + +void dump_p2p_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *pos = ie; + u8 id; + u16 len; + + const u8 *p2p_ie; + uint p2p_ielen; + + p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen); + if (p2p_ie != ie || p2p_ielen == 0) + return; + + pos += 6; + while (pos - ie + 3 <= ie_len) { + id = *pos; + len = RTW_GET_LE16(pos + 1); + + RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len + , ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)"); + + pos += (3 + len); + } +} + +/** + * rtw_get_p2p_ie - Search P2P IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie + * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE + * + * Returns: The address of the P2P IE found, or NULL + */ +u8 *rtw_get_p2p_ie(const u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen) +{ + uint cnt; + const u8 *p2p_ie_ptr = NULL; + u8 eid, p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09}; + + if (p2p_ielen) + *p2p_ielen = 0; + + if (!in_ie || in_len < 0) { + rtw_warn_on(1); + return (u8 *)p2p_ie_ptr; + } + + if (in_len <= 0) + return (u8 *)p2p_ie_ptr; + + cnt = 0; + + while (cnt + 1 + 4 < in_len) { + eid = in_ie[cnt]; + + if (cnt + 1 + 4 >= MAX_IE_SZ) { + rtw_warn_on(1); + return NULL; + } + + if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], p2p_oui, 4) == _TRUE) { + p2p_ie_ptr = in_ie + cnt; + + if (p2p_ie) + _rtw_memcpy(p2p_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + if (p2p_ielen) + *p2p_ielen = in_ie[cnt + 1] + 2; + + break; + } else + cnt += in_ie[cnt + 1] + 2; + + } + + return (u8 *)p2p_ie_ptr; +} + +/** + * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE + * @p2p_ie: Address of P2P IE to search + * @p2p_ielen: Length limit from p2p_ie + * @target_attr_id: The attribute ID of P2P attribute to search + * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr + * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute + * + * Returns: the address of the specific WPS attribute found, or NULL + */ +u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_attr, u32 *len_attr) +{ + u8 *attr_ptr = NULL; + u8 *target_attr_ptr = NULL; + u8 p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09}; + + if (len_attr) + *len_attr = 0; + + if (!p2p_ie + || p2p_ielen <= 6 + || (p2p_ie[0] != WLAN_EID_VENDOR_SPECIFIC) + || (_rtw_memcmp(p2p_ie + 2, p2p_oui, 4) != _TRUE)) + return attr_ptr; + + /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */ + attr_ptr = p2p_ie + 6; /* goto first attr */ + + while ((attr_ptr - p2p_ie + 3) <= p2p_ielen) { + /* 3 = 1(Attribute ID) + 2(Length) */ + u8 attr_id = *attr_ptr; + u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1); + u16 attr_len = attr_data_len + 3; + + if (0) + RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); + + if ((attr_ptr - p2p_ie + attr_len) > p2p_ielen) + break; + + if (attr_id == target_attr_id) { + target_attr_ptr = attr_ptr; + + if (buf_attr) + _rtw_memcpy(buf_attr, attr_ptr, attr_len); + + if (len_attr) + *len_attr = attr_len; + + break; + } else + attr_ptr += attr_len; + } + + return target_attr_ptr; +} + +/** + * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE + * @p2p_ie: Address of P2P IE to search + * @p2p_ielen: Length limit from p2p_ie + * @target_attr_id: The attribute ID of P2P attribute to search + * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content + * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content + * + * Returns: the address of the specific P2P attribute content found, or NULL + */ +u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_content, uint *len_content) +{ + u8 *attr_ptr; + u32 attr_len; + + if (len_content) + *len_content = 0; + + attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len); + + if (attr_ptr && attr_len) { + if (buf_content) + _rtw_memcpy(buf_content, attr_ptr + 3, attr_len - 3); + + if (len_content) + *len_content = attr_len - 3; + + return attr_ptr + 3; + } + + return NULL; +} + +u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr) +{ + u32 a_len; + + *pbuf = attr_id; + + /* *(u16*)(pbuf + 1) = cpu_to_le16(attr_len); */ + RTW_PUT_LE16(pbuf + 1, attr_len); + + if (pdata_attr) + _rtw_memcpy(pbuf + 3, pdata_attr, attr_len); + + a_len = attr_len + 3; + + return a_len; +} + +uint rtw_del_p2p_ie(u8 *ies, uint ies_len_ori, const char *msg) +{ +#define DBG_DEL_P2P_IE 0 + + u8 *target_ie; + u32 target_ie_len; + uint ies_len = ies_len_ori; + int index = 0; + + while (1) { + target_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &target_ie_len); + if (target_ie && target_ie_len) { + u8 *next_ie = target_ie + target_ie_len; + uint remain_len = ies_len - (next_ie - ies); + + if (DBG_DEL_P2P_IE && msg) { + RTW_INFO("%s %d before\n", __func__, index); + dump_ies(RTW_DBGDUMP, ies, ies_len); + + RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len); + RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len); + RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len); + } + + _rtw_memmove(target_ie, next_ie, remain_len); + _rtw_memset(target_ie + remain_len, 0, target_ie_len); + ies_len -= target_ie_len; + + if (DBG_DEL_P2P_IE && msg) { + RTW_INFO("%s %d after\n", __func__, index); + dump_ies(RTW_DBGDUMP, ies, ies_len); + } + + index++; + } else + break; + } + + return ies_len; +} + +uint rtw_del_p2p_attr(u8 *ie, uint ielen_ori, u8 attr_id) +{ +#define DBG_DEL_P2P_ATTR 0 + + u8 *target_attr; + u32 target_attr_len; + uint ielen = ielen_ori; + int index = 0; + + while (1) { + target_attr = rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len); + if (target_attr && target_attr_len) { + u8 *next_attr = target_attr + target_attr_len; + uint remain_len = ielen - (next_attr - ie); + + if (DBG_DEL_P2P_ATTR) { + RTW_INFO("%s %d before\n", __func__, index); + dump_ies(RTW_DBGDUMP, ie, ielen); + + RTW_INFO("ie:%p, ielen:%u\n", ie, ielen); + RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len); + RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len); + } + + _rtw_memmove(target_attr, next_attr, remain_len); + _rtw_memset(target_attr + remain_len, 0, target_attr_len); + *(ie + 1) -= target_attr_len; + ielen -= target_attr_len; + + if (DBG_DEL_P2P_ATTR) { + RTW_INFO("%s %d after\n", __func__, index); + dump_ies(RTW_DBGDUMP, ie, ielen); + } + + index++; + } else + break; + } + + return ielen; +} + +inline u8 *rtw_bss_ex_get_p2p_ie(WLAN_BSSID_EX *bss_ex, u8 *p2p_ie, uint *p2p_ielen) +{ + return rtw_get_p2p_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), p2p_ie, p2p_ielen); +} + +void rtw_bss_ex_del_p2p_ie(WLAN_BSSID_EX *bss_ex) +{ +#define DBG_BSS_EX_DEL_P2P_IE 0 + + u8 *ies = BSS_EX_TLV_IES(bss_ex); + uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex); + uint ies_len; + + ies_len = rtw_del_p2p_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_P2P_IE ? __func__ : NULL); + bss_ex->IELength -= ies_len_ori - ies_len; +} + +void rtw_bss_ex_del_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id) +{ +#define DBG_BSS_EX_DEL_P2P_ATTR 0 + + u8 *ies = BSS_EX_TLV_IES(bss_ex); + uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex); + + u8 *ie; + uint ie_len, ie_len_ori; + + int index = 0; + + while (1) { + ie = rtw_get_p2p_ie(ies, ies_len, NULL, &ie_len_ori); + if (ie) { + u8 *next_ie_ori = ie + ie_len_ori; + uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs); + u8 has_target_attr = 0; + + if (DBG_BSS_EX_DEL_P2P_ATTR) { + if (rtw_get_p2p_attr(ie, ie_len_ori, attr_id, NULL, NULL)) { + RTW_INFO("%s %d before\n", __func__, index); + dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex)); + + RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len); + RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori); + RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len); + has_target_attr = 1; + } + } + + ie_len = rtw_del_p2p_attr(ie, ie_len_ori, attr_id); + if (ie_len != ie_len_ori) { + u8 *next_ie = ie + ie_len; + + _rtw_memmove(next_ie, next_ie_ori, remain_len); + _rtw_memset(next_ie + remain_len, 0, ie_len_ori - ie_len); + bss_ex->IELength -= ie_len_ori - ie_len; + + ies = next_ie; + } else + ies = next_ie_ori; + + if (DBG_BSS_EX_DEL_P2P_ATTR) { + if (has_target_attr) { + RTW_INFO("%s %d after\n", __func__, index); + dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex)); + } + } + + ies_len = remain_len; + + index++; + } else + break; + } +} +#endif /* CONFIG_P2P */ + +/** + * rtw_get_wfd_ie - Search WFD IE from a series of IEs + * @in_ie: Address of IEs to search + * @in_len: Length limit from in_ie + * @wfd_ie: If not NULL and WFD IE is found, WFD IE will be copied to the buf starting from wfd_ie + * @wfd_ielen: If not NULL and WFD IE is found, will set to the length of the entire WFD IE + * + * Returns: The address of the P2P IE found, or NULL + */ +u8 *rtw_get_wfd_ie(const u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen) +{ + uint cnt; + const u8 *wfd_ie_ptr = NULL; + u8 eid, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A}; + + if (wfd_ielen) + *wfd_ielen = 0; + + if (!in_ie || in_len < 0) { + rtw_warn_on(1); + return (u8 *)wfd_ie_ptr; + } + + if (in_len <= 0) + return (u8 *)wfd_ie_ptr; + + cnt = 0; + + while (cnt + 1 + 4 < in_len) { + eid = in_ie[cnt]; + + if (cnt + 1 + 4 >= MAX_IE_SZ) { + rtw_warn_on(1); + return NULL; + } + + if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], wfd_oui, 4) == _TRUE) { + wfd_ie_ptr = in_ie + cnt; + + if (wfd_ie) + _rtw_memcpy(wfd_ie, &in_ie[cnt], in_ie[cnt + 1] + 2); + + if (wfd_ielen) + *wfd_ielen = in_ie[cnt + 1] + 2; + + break; + } else + cnt += in_ie[cnt + 1] + 2; + + } + + return (u8 *)wfd_ie_ptr; +} + +uint rtw_del_wfd_ie(u8 *ies, uint ies_len_ori, const char *msg) +{ +#define DBG_DEL_WFD_IE 0 + + u8 *target_ie; + u32 target_ie_len; + uint ies_len = ies_len_ori; + int index = 0; + + while (1) { + target_ie = rtw_get_wfd_ie(ies, ies_len, NULL, &target_ie_len); + if (target_ie && target_ie_len) { + u8 *next_ie = target_ie + target_ie_len; + uint remain_len = ies_len - (next_ie - ies); + + if (DBG_DEL_WFD_IE && msg) { + RTW_INFO("%s %d before\n", __func__, index); + dump_ies(RTW_DBGDUMP, ies, ies_len); + + RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len); + RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len); + RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len); + } + + _rtw_memmove(target_ie, next_ie, remain_len); + _rtw_memset(target_ie + remain_len, 0, target_ie_len); + ies_len -= target_ie_len; + + if (DBG_DEL_WFD_IE && msg) { + RTW_INFO("%s %d after\n", __func__, index); + dump_ies(RTW_DBGDUMP, ies, ies_len); + } + + index++; + } else + break; + } + + return ies_len; +} + +void rtw_bss_ex_del_wfd_ie(WLAN_BSSID_EX *bss_ex) +{ +#define DBG_BSS_EX_DEL_WFD_IE 0 + u8 *ies = BSS_EX_TLV_IES(bss_ex); + uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex); + uint ies_len; + + ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_WFD_IE ? __func__ : NULL); + bss_ex->IELength -= ies_len_ori - ies_len; +} + +#ifdef CONFIG_WFD +void dump_wfd_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *pos = ie; + u8 id; + u16 len; + + const u8 *wfd_ie; + uint wfd_ielen; + + wfd_ie = rtw_get_wfd_ie(ie, ie_len, NULL, &wfd_ielen); + if (wfd_ie != ie || wfd_ielen == 0) + return; + + pos += 6; + while (pos - ie + 3 <= ie_len) { + id = *pos; + len = RTW_GET_BE16(pos + 1); + + RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len + , ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)"); + + pos += (3 + len); + } +} + +/** + * rtw_get_wfd_attr - Search a specific WFD attribute from a given WFD IE + * @wfd_ie: Address of WFD IE to search + * @wfd_ielen: Length limit from wfd_ie + * @target_attr_id: The attribute ID of WFD attribute to search + * @buf_attr: If not NULL and the WFD attribute is found, WFD attribute will be copied to the buf starting from buf_attr + * @len_attr: If not NULL and the WFD attribute is found, will set to the length of the entire WFD attribute + * + * Returns: the address of the specific WPS attribute found, or NULL + */ +u8 *rtw_get_wfd_attr(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_attr, u32 *len_attr) +{ + u8 *attr_ptr = NULL; + u8 *target_attr_ptr = NULL; + u8 wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A}; + + if (len_attr) + *len_attr = 0; + + if (!wfd_ie + || wfd_ielen <= 6 + || (wfd_ie[0] != WLAN_EID_VENDOR_SPECIFIC) + || (_rtw_memcmp(wfd_ie + 2, wfd_oui, 4) != _TRUE)) + return attr_ptr; + + /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */ + attr_ptr = wfd_ie + 6; /* goto first attr */ + + while ((attr_ptr - wfd_ie + 3) <= wfd_ielen) { + /* 3 = 1(Attribute ID) + 2(Length) */ + u8 attr_id = *attr_ptr; + u16 attr_data_len = RTW_GET_BE16(attr_ptr + 1); + u16 attr_len = attr_data_len + 3; + + if (0) + RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); + + if ((attr_ptr - wfd_ie + attr_len) > wfd_ielen) + break; + + if (attr_id == target_attr_id) { + target_attr_ptr = attr_ptr; + + if (buf_attr) + _rtw_memcpy(buf_attr, attr_ptr, attr_len); + + if (len_attr) + *len_attr = attr_len; + + break; + } else + attr_ptr += attr_len; + } + + return target_attr_ptr; +} + +/** + * rtw_get_wfd_attr_content - Search a specific WFD attribute content from a given WFD IE + * @wfd_ie: Address of WFD IE to search + * @wfd_ielen: Length limit from wfd_ie + * @target_attr_id: The attribute ID of WFD attribute to search + * @buf_content: If not NULL and the WFD attribute is found, WFD attribute content will be copied to the buf starting from buf_content + * @len_content: If not NULL and the WFD attribute is found, will set to the length of the WFD attribute content + * + * Returns: the address of the specific WFD attribute content found, or NULL + */ +u8 *rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_content, uint *len_content) +{ + u8 *attr_ptr; + u32 attr_len; + + if (len_content) + *len_content = 0; + + attr_ptr = rtw_get_wfd_attr(wfd_ie, wfd_ielen, target_attr_id, NULL, &attr_len); + + if (attr_ptr && attr_len) { + if (buf_content) + _rtw_memcpy(buf_content, attr_ptr + 3, attr_len - 3); + + if (len_content) + *len_content = attr_len - 3; + + return attr_ptr + 3; + } + + return NULL; +} + +uint rtw_del_wfd_attr(u8 *ie, uint ielen_ori, u8 attr_id) +{ +#define DBG_DEL_WFD_ATTR 0 + + u8 *target_attr; + u32 target_attr_len; + uint ielen = ielen_ori; + int index = 0; + + while (1) { + target_attr = rtw_get_wfd_attr(ie, ielen, attr_id, NULL, &target_attr_len); + if (target_attr && target_attr_len) { + u8 *next_attr = target_attr + target_attr_len; + uint remain_len = ielen - (next_attr - ie); + + if (DBG_DEL_WFD_ATTR) { + RTW_INFO("%s %d before\n", __func__, index); + dump_ies(RTW_DBGDUMP, ie, ielen); + + RTW_INFO("ie:%p, ielen:%u\n", ie, ielen); + RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len); + RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len); + } + + _rtw_memmove(target_attr, next_attr, remain_len); + _rtw_memset(target_attr + remain_len, 0, target_attr_len); + *(ie + 1) -= target_attr_len; + ielen -= target_attr_len; + + if (DBG_DEL_WFD_ATTR) { + RTW_INFO("%s %d after\n", __func__, index); + dump_ies(RTW_DBGDUMP, ie, ielen); + } + + index++; + } else + break; + } + + return ielen; +} + +inline u8 *rtw_bss_ex_get_wfd_ie(WLAN_BSSID_EX *bss_ex, u8 *wfd_ie, uint *wfd_ielen) +{ + return rtw_get_wfd_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), wfd_ie, wfd_ielen); +} + +void rtw_bss_ex_del_wfd_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id) +{ +#define DBG_BSS_EX_DEL_WFD_ATTR 0 + + u8 *ies = BSS_EX_TLV_IES(bss_ex); + uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex); + + u8 *ie; + uint ie_len, ie_len_ori; + + int index = 0; + + while (1) { + ie = rtw_get_wfd_ie(ies, ies_len, NULL, &ie_len_ori); + if (ie) { + u8 *next_ie_ori = ie + ie_len_ori; + uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs); + u8 has_target_attr = 0; + + if (DBG_BSS_EX_DEL_WFD_ATTR) { + if (rtw_get_wfd_attr(ie, ie_len_ori, attr_id, NULL, NULL)) { + RTW_INFO("%s %d before\n", __func__, index); + dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex)); + + RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len); + RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori); + RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len); + has_target_attr = 1; + } + } + + ie_len = rtw_del_wfd_attr(ie, ie_len_ori, attr_id); + if (ie_len != ie_len_ori) { + u8 *next_ie = ie + ie_len; + + _rtw_memmove(next_ie, next_ie_ori, remain_len); + _rtw_memset(next_ie + remain_len, 0, ie_len_ori - ie_len); + bss_ex->IELength -= ie_len_ori - ie_len; + + ies = next_ie; + } else + ies = next_ie_ori; + + if (DBG_BSS_EX_DEL_WFD_ATTR) { + if (has_target_attr) { + RTW_INFO("%s %d after\n", __func__, index); + dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex)); + } + } + + ies_len = remain_len; + + index++; + } else + break; + } +} +#endif /* CONFIG_WFD */ + +#ifdef CONFIG_RTW_MULTI_AP +void dump_multi_ap_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *pos = ie; + u8 id; + u8 len; + + const u8 *multi_ap_ie; + uint multi_ap_ielen; + + multi_ap_ie = rtw_get_ie_ex(ie, ie_len, WLAN_EID_VENDOR_SPECIFIC, MULTI_AP_OUI, 4, NULL, &multi_ap_ielen); + if (multi_ap_ie != ie || multi_ap_ielen == 0) + return; + + pos += 6; + while (pos - ie + 2 <= ie_len) { + id = *pos; + len = *(pos + 1); + + RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len + , ((pos - ie + 2 + len) <= ie_len) ? "" : "(exceed ie_len)"); + RTW_DUMP_SEL(sel, pos + 2, len); + + pos += (2 + len); + } +} + +/** + * rtw_get_multi_ap_ext - Search Multi-AP IE from a series of IEs and return extension subelement value + * @ies: Address of IEs to search + * @ies_len: Length limit from in_ie + * + * Returns: The address of the target IE found, or NULL + */ +u8 rtw_get_multi_ap_ie_ext(const u8 *ies, int ies_len) +{ + u8 *ie; + uint ielen; + u8 val = 0; + + ie = rtw_get_ie_ex(ies, ies_len, WLAN_EID_VENDOR_SPECIFIC, MULTI_AP_OUI, 4, NULL, &ielen); + if (ielen < 9) + goto exit; + + if (ie[6] != MULTI_AP_SUB_ELEM_TYPE) + goto exit; + + val = ie[8]; + +exit: + return val; +} + +u8 *rtw_set_multi_ap_ie_ext(u8 *pbuf, uint *frlen, u8 val) +{ + u8 cont_len = 7; + + *pbuf++ = WLAN_EID_VENDOR_SPECIFIC; + *pbuf++ = cont_len; + _rtw_memcpy(pbuf, MULTI_AP_OUI, 4); + pbuf += 4; + *pbuf++ = MULTI_AP_SUB_ELEM_TYPE; + *pbuf++ = 1; /* len */ + *pbuf++ = val; + + if (frlen) + *frlen = *frlen + (cont_len + 2); + + return pbuf; +} +#endif /* CONFIG_RTW_MULTI_AP */ + +/* Baron adds to avoid FreeBSD warning */ +int ieee80211_is_empty_essid(const char *essid, int essid_len) +{ + /* Single white space is for Linksys APs */ + if (essid_len == 1 && essid[0] == ' ') + return 1; + + /* Otherwise, if the entire essid is 0, we assume it is hidden */ + while (essid_len) { + essid_len--; + if (essid[essid_len] != '\0') + return 0; + } + + return 1; +} + +int ieee80211_get_hdrlen(u16 fc) +{ + int hdrlen = 24; + + switch (WLAN_FC_GET_TYPE(fc)) { + case RTW_IEEE80211_FTYPE_DATA: + if (fc & RTW_IEEE80211_STYPE_QOS_DATA) + hdrlen += 2; + if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS)) + hdrlen += 6; /* Addr4 */ + break; + case RTW_IEEE80211_FTYPE_CTL: + switch (WLAN_FC_GET_STYPE(fc)) { + case RTW_IEEE80211_STYPE_CTS: + case RTW_IEEE80211_STYPE_ACK: + hdrlen = 10; + break; + default: + hdrlen = 16; + break; + } + break; + } + + return hdrlen; +} + +u8 rtw_ht_mcsset_to_nss(u8 *supp_mcs_set) +{ + u8 nss = 1; + + if (supp_mcs_set[3]) + nss = 4; + else if (supp_mcs_set[2]) + nss = 3; + else if (supp_mcs_set[1]) + nss = 2; + else if (supp_mcs_set[0]) + nss = 1; + else + RTW_INFO("%s,%d, warning! supp_mcs_set is zero\n", __func__, __LINE__); + /* RTW_INFO("%s HT: %dSS\n", __FUNCTION__, nss); */ + return nss; +} + +u32 rtw_ht_mcs_set_to_bitmap(u8 *mcs_set, u8 nss) +{ + u8 i; + u32 bitmap = 0; + + for (i = 0; i < nss; i++) + bitmap |= mcs_set[i] << (i * 8); + + RTW_INFO("ht_mcs_set=%02x %02x %02x %02x, nss=%u, bitmap=%08x\n" + , mcs_set[0], mcs_set[1], mcs_set[2], mcs_set[3], nss, bitmap); + + return bitmap; +} + +/* show MCS rate, unit: 100Kbps */ +u16 rtw_ht_mcs_rate(u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate) +{ + u16 max_rate = 0; + + if (MCS_rate[3]) { + if (MCS_rate[3] & BIT(7)) + max_rate = (bw_40MHz) ? ((short_GI) ? 6000 : 5400) : ((short_GI) ? 2889 : 2600); + else if (MCS_rate[3] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI) ? 5400 : 4860) : ((short_GI) ? 2600 : 2340); + else if (MCS_rate[3] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI) ? 4800 : 4320) : ((short_GI) ? 2311 : 2080); + else if (MCS_rate[3] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560); + else if (MCS_rate[3] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040); + else if (MCS_rate[3] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780); + else if (MCS_rate[3] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520); + else if (MCS_rate[3] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260); + } else if (MCS_rate[2]) { + if (MCS_rate[2] & BIT(7)) + max_rate = (bw_40MHz) ? ((short_GI) ? 4500 : 4050) : ((short_GI) ? 2167 : 1950); + else if (MCS_rate[2] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI) ? 4050 : 3645) : ((short_GI) ? 1950 : 1750); + else if (MCS_rate[2] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560); + else if (MCS_rate[2] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170); + else if (MCS_rate[2] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780); + else if (MCS_rate[2] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585); + else if (MCS_rate[2] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390); + else if (MCS_rate[2] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195); + } else if (MCS_rate[1]) { + if (MCS_rate[1] & BIT(7)) + max_rate = (bw_40MHz) ? ((short_GI) ? 3000 : 2700) : ((short_GI) ? 1444 : 1300); + else if (MCS_rate[1] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170); + else if (MCS_rate[1] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040); + else if (MCS_rate[1] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780); + else if (MCS_rate[1] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520); + else if (MCS_rate[1] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390); + else if (MCS_rate[1] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260); + else if (MCS_rate[1] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130); + } else { + if (MCS_rate[0] & BIT(7)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1500 : 1350) : ((short_GI) ? 722 : 650); + else if (MCS_rate[0] & BIT(6)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585); + else if (MCS_rate[0] & BIT(5)) + max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520); + else if (MCS_rate[0] & BIT(4)) + max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390); + else if (MCS_rate[0] & BIT(3)) + max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260); + else if (MCS_rate[0] & BIT(2)) + max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195); + else if (MCS_rate[0] & BIT(1)) + max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130); + else if (MCS_rate[0] & BIT(0)) + max_rate = (bw_40MHz) ? ((short_GI) ? 150 : 135) : ((short_GI) ? 72 : 65); + } + + return max_rate; +} + +u8 rtw_ht_cap_get_rx_nss(u8 *ht_cap) +{ + u8 *ht_mcs_set = HT_CAP_ELE_SUP_MCS_SET(ht_cap); + + return rtw_ht_mcsset_to_nss(ht_mcs_set); +} + +u8 rtw_ht_cap_get_tx_nss(u8 *ht_cap) +{ + u8 *ht_mcs_set = HT_CAP_ELE_SUP_MCS_SET(ht_cap); + + if (GET_HT_CAP_ELE_TX_MCS_DEF(ht_cap) && GET_HT_CAP_ELE_TRX_MCS_NEQ(ht_cap)) + return GET_HT_CAP_ELE_TX_MAX_SS(ht_cap) + 1; + + return rtw_ht_cap_get_rx_nss(ht_cap); +} + +int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action) +{ + const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr); + u16 fc; + u8 c; + u8 a = ACT_PUBLIC_MAX; + + fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl); + + if ((fc & (RTW_IEEE80211_FCTL_FTYPE | RTW_IEEE80211_FCTL_STYPE)) + != (RTW_IEEE80211_FTYPE_MGMT | RTW_IEEE80211_STYPE_ACTION) + ) + return _FALSE; + + c = frame_body[0]; + + switch (c) { + case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */ + break; + default: + a = frame_body[1]; + } + + if (category) + *category = c; + if (action) + *action = a; + + return _TRUE; +} + +static const char *_action_public_str[] = { + [ACT_PUBLIC_BSSCOEXIST] = "ACT_PUB_BSSCOEXIST", + [ACT_PUBLIC_DSE_ENABLE] = "ACT_PUB_DSE_ENABLE", + [ACT_PUBLIC_DSE_DEENABLE] = "ACT_PUB_DSE_DEENABLE", + [ACT_PUBLIC_DSE_REG_LOCATION] = "ACT_PUB_DSE_REG_LOCATION", + [ACT_PUBLIC_EXT_CHL_SWITCH] = "ACT_PUB_EXT_CHL_SWITCH", + [ACT_PUBLIC_DSE_MSR_REQ] = "ACT_PUB_DSE_MSR_REQ", + [ACT_PUBLIC_DSE_MSR_RPRT] = "ACT_PUB_DSE_MSR_RPRT", + [ACT_PUBLIC_MP] = "ACT_PUB_MP", + [ACT_PUBLIC_DSE_PWR_CONSTRAINT] = "ACT_PUB_DSE_PWR_CONSTRAINT", + [ACT_PUBLIC_VENDOR] = "ACT_PUB_VENDOR", + [ACT_PUBLIC_GAS_INITIAL_REQ] = "ACT_PUB_GAS_INITIAL_REQ", + [ACT_PUBLIC_GAS_INITIAL_RSP] = "ACT_PUB_GAS_INITIAL_RSP", + [ACT_PUBLIC_GAS_COMEBACK_REQ] = "ACT_PUB_GAS_COMEBACK_REQ", + [ACT_PUBLIC_GAS_COMEBACK_RSP] = "ACT_PUB_GAS_COMEBACK_RSP", + [ACT_PUBLIC_TDLS_DISCOVERY_RSP] = "ACT_PUB_TDLS_DISCOVERY_RSP", + [ACT_PUBLIC_LOCATION_TRACK] = "ACT_PUB_LOCATION_TRACK", + [ACT_PUBLIC_QAB_REQ] = "ACT_PUB_QAB_REQ", + [ACT_PUBLIC_QAB_RSP] = "ACT_PUB_QAB_RSP", + [ACT_PUBLIC_QMF_POLICY] = "ACT_PUB_QMF_POLICY", + [ACT_PUBLIC_QMF_POLICY_CHANGE] = "ACT_PUB_QMF_POLICY_CHANGE", + [ACT_PUBLIC_QLOAD_REQ] = "ACT_PUB_QLOAD_REQ", + [ACT_PUBLIC_QLOAD_REPORT] = "ACT_PUB_QLOAD_REPORT", + [ACT_PUBLIC_HCCA_TXOP_ADV] = "ACT_PUB_HCCA_TXOP_ADV", + [ACT_PUBLIC_HCCA_TXOP_RSP] = "ACT_PUB_HCCA_TXOP_RSP", + [ACT_PUBLIC_PUBLIC_KEY] = "ACT_PUB_PUBLIC_KEY", + [ACT_PUBLIC_CH_AVAILABILITY_QUERY] = "ACT_PUB_CH_AVAILABILITY_QUERY", + [ACT_PUBLIC_CH_SCHEDULE_MGMT] = "ACT_PUB_CH_SCHEDULE_MGMT", + [ACT_PUBLIC_CONTACT_VERI_SIGNAL] = "ACT_PUB_CONTACT_VERI_SIGNAL", + [ACT_PUBLIC_GDD_ENABLE_REQ] = "ACT_PUB_GDD_ENABLE_REQ", + [ACT_PUBLIC_GDD_ENABLE_RSP] = "ACT_PUB_GDD_ENABLE_RSP", + [ACT_PUBLIC_NETWORK_CH_CONTROL] = "ACT_PUB_NETWORK_CH_CONTROL", + [ACT_PUBLIC_WHITE_SPACE_MAP_ANN] = "ACT_PUB_WHITE_SPACE_MAP_ANN", + [ACT_PUBLIC_FTM_REQ] = "ACT_PUB_FTM_REQ", + [ACT_PUBLIC_FTM] = "ACT_PUB_FTM", + [ACT_PUBLIC_MAX] = "ACT_PUB_RSVD", +}; + +const char *action_public_str(u8 action) +{ + action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action; + return _action_public_str[action]; +} + +/*tmp for sta mode, root cause have to wait supplicant's update.*/ +void rtw_set_spp_amsdu_mode(u8 mode, u8 *rsn_ie, int rsn_ie_len) +{ + struct rsne_info info; + int i, ret = _SUCCESS; + u8 spp_req_cap = 0; + + if (mode == RTW_AMSDU_MODE_NON_SPP ) { + spp_req_cap = (u8)(~SPP_CAP | ~SPP_REQ); + } else if (mode == RTW_AMSDU_MODE_SPP) { + spp_req_cap = (u8)(SPP_CAP | SPP_REQ); + } else if (mode == RTW_AMSDU_MODE_ALL_DROP) { + spp_req_cap = (u8)(~SPP_CAP | SPP_REQ); + } else { + RTW_INFO("%s unexpected mode = %d, please check the config\n", __func__, mode); + return; + } + + ret = rtw_rsne_info_parse(rsn_ie, rsn_ie_len, &info); + if (ret != _SUCCESS) + return; + + SET_RSN_CAP_SPP(info.cap, spp_req_cap); + RTW_INFO("%s set spp opt = %d\n", __func__, GET_RSN_CAP_SPP_OPT(info.cap)); +} + +u8 rtw_check_amsdu_disable(u8 mode, u8 spp_opt) +{ + u8 ret = _FALSE; + RTW_INFO("%s spp_opt=%u \n", __func__, spp_opt); + + /* pp amsdu: peer's required has to be 0, or disable */ + if ((mode == RTW_AMSDU_MODE_NON_SPP) && (spp_opt & SPP_REQ)) + ret = _TRUE; + /* spp amsdu: peer's cap has to be 1, or disable */ + else if ((mode == RTW_AMSDU_MODE_SPP) && ~(spp_opt & SPP_CAP)) + ret = _TRUE; + /* mode = all drop */ + else if (mode == RTW_AMSDU_MODE_ALL_DROP) + ret = _TRUE; + else + ret = _FALSE; + return ret; +} + diff --git a/core/rtw_io.c b/core/rtw_io.c new file mode 100644 index 0000000..734d3e5 --- /dev/null +++ b/core/rtw_io.c @@ -0,0 +1,952 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +/* + +The purpose of rtw_io.c + +a. provides the API + +b. provides the protocol engine + +c. provides the software interface between caller and the hardware interface + + +Compiler Flag Option: + +1. CONFIG_SDIO_HCI: + a. USE_SYNC_IRP: Only sync operations are provided. + b. USE_ASYNC_IRP:Both sync/async operations are provided. + +2. CONFIG_USB_HCI: + a. USE_ASYNC_IRP: Both sync/async operations are provided. + +3. CONFIG_CFIO_HCI: + b. USE_SYNC_IRP: Only sync operations are provided. + + +Only sync read/rtw_write_mem operations are provided. + +jackson@realtek.com.tw + +*/ + +#define _RTW_IO_C_ + +#include +#include + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_PLATFORM_RTL8197D) + #define rtw_le16_to_cpu(val) val + #define rtw_le32_to_cpu(val) val + #define rtw_cpu_to_le16(val) val + #define rtw_cpu_to_le32(val) val +#else + #define rtw_le16_to_cpu(val) le16_to_cpu(val) + #define rtw_le32_to_cpu(val) le32_to_cpu(val) + #define rtw_cpu_to_le16(val) cpu_to_le16(val) + #define rtw_cpu_to_le32(val) cpu_to_le32(val) +#endif + + +u8 _rtw_read8(_adapter *adapter, u32 addr) +{ + u8 r_val; + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u8(*_read8)(struct intf_hdl *pintfhdl, u32 addr); + _read8 = pintfhdl->io_ops._read8; + + r_val = _read8(pintfhdl, addr); + return r_val; +} + +u16 _rtw_read16(_adapter *adapter, u32 addr) +{ + u16 r_val; + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u16(*_read16)(struct intf_hdl *pintfhdl, u32 addr); + _read16 = pintfhdl->io_ops._read16; + + r_val = _read16(pintfhdl, addr); + return rtw_le16_to_cpu(r_val); +} + +u32 _rtw_read32(_adapter *adapter, u32 addr) +{ + u32 r_val; + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u32(*_read32)(struct intf_hdl *pintfhdl, u32 addr); + _read32 = pintfhdl->io_ops._read32; + + r_val = _read32(pintfhdl, addr); + return rtw_le32_to_cpu(r_val); + +} + +int _rtw_write8(_adapter *adapter, u32 addr, u8 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val); + int ret; + _write8 = pintfhdl->io_ops._write8; + + ret = _write8(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} +int _rtw_write16(_adapter *adapter, u32 addr, u16 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val); + int ret; + _write16 = pintfhdl->io_ops._write16; + + val = rtw_cpu_to_le16(val); + ret = _write16(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} +int _rtw_write32(_adapter *adapter, u32 addr, u32 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val); + int ret; + _write32 = pintfhdl->io_ops._write32; + + val = rtw_cpu_to_le32(val); + ret = _write32(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} + +int _rtw_writeN(_adapter *adapter, u32 addr , u32 length , u8 *pdata) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = (struct intf_hdl *)(&(pio_priv->intf)); + int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata); + int ret; + _writeN = pintfhdl->io_ops._writeN; + + ret = _writeN(pintfhdl, addr, length, pdata); + + return RTW_STATUS_CODE(ret); +} + +#ifdef CONFIG_SDIO_HCI +u8 _rtw_sd_f0_read8(_adapter *adapter, u32 addr) +{ + u8 r_val = 0x00; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u8(*_sd_f0_read8)(struct intf_hdl *pintfhdl, u32 addr); + + _sd_f0_read8 = pintfhdl->io_ops._sd_f0_read8; + + if (_sd_f0_read8) + r_val = _sd_f0_read8(pintfhdl, addr); + else + RTW_WARN(FUNC_ADPT_FMT" _sd_f0_read8 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return r_val; +} + +#ifdef CONFIG_SDIO_INDIRECT_ACCESS +u8 _rtw_sd_iread8(_adapter *adapter, u32 addr) +{ + u8 r_val = 0x00; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u8(*_sd_iread8)(struct intf_hdl *pintfhdl, u32 addr); + + _sd_iread8 = pintfhdl->io_ops._sd_iread8; + + if (_sd_iread8) + r_val = _sd_iread8(pintfhdl, addr); + else + RTW_ERR(FUNC_ADPT_FMT" _sd_iread8 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return r_val; +} + +u16 _rtw_sd_iread16(_adapter *adapter, u32 addr) +{ + u16 r_val = 0x00; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u16(*_sd_iread16)(struct intf_hdl *pintfhdl, u32 addr); + + _sd_iread16 = pintfhdl->io_ops._sd_iread16; + + if (_sd_iread16) + r_val = _sd_iread16(pintfhdl, addr); + else + RTW_ERR(FUNC_ADPT_FMT" _sd_iread16 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return r_val; +} + +u32 _rtw_sd_iread32(_adapter *adapter, u32 addr) +{ + u32 r_val = 0x00; + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u32(*_sd_iread32)(struct intf_hdl *pintfhdl, u32 addr); + + _sd_iread32 = pintfhdl->io_ops._sd_iread32; + + if (_sd_iread32) + r_val = _sd_iread32(pintfhdl, addr); + else + RTW_ERR(FUNC_ADPT_FMT" _sd_iread32 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return r_val; +} + +int _rtw_sd_iwrite8(_adapter *adapter, u32 addr, u8 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_sd_iwrite8)(struct intf_hdl *pintfhdl, u32 addr, u8 val); + int ret = -1; + + _sd_iwrite8 = pintfhdl->io_ops._sd_iwrite8; + + if (_sd_iwrite8) + ret = _sd_iwrite8(pintfhdl, addr, val); + else + RTW_ERR(FUNC_ADPT_FMT" _sd_iwrite8 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return RTW_STATUS_CODE(ret); +} + +int _rtw_sd_iwrite16(_adapter *adapter, u32 addr, u16 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_sd_iwrite16)(struct intf_hdl *pintfhdl, u32 addr, u16 val); + int ret = -1; + + _sd_iwrite16 = pintfhdl->io_ops._sd_iwrite16; + + if (_sd_iwrite16) + ret = _sd_iwrite16(pintfhdl, addr, val); + else + RTW_ERR(FUNC_ADPT_FMT" _sd_iwrite16 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return RTW_STATUS_CODE(ret); +} +int _rtw_sd_iwrite32(_adapter *adapter, u32 addr, u32 val) +{ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_sd_iwrite32)(struct intf_hdl *pintfhdl, u32 addr, u32 val); + int ret = -1; + + _sd_iwrite32 = pintfhdl->io_ops._sd_iwrite32; + + if (_sd_iwrite32) + ret = _sd_iwrite32(pintfhdl, addr, val); + else + RTW_ERR(FUNC_ADPT_FMT" _sd_iwrite32 callback is NULL\n", FUNC_ADPT_ARG(adapter)); + + return RTW_STATUS_CODE(ret); +} + +#endif /* CONFIG_SDIO_INDIRECT_ACCESS */ + +#endif /* CONFIG_SDIO_HCI */ + +int _rtw_write8_async(_adapter *adapter, u32 addr, u8 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val); + int ret; + _write8_async = pintfhdl->io_ops._write8_async; + + ret = _write8_async(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} +int _rtw_write16_async(_adapter *adapter, u32 addr, u16 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val); + int ret; + _write16_async = pintfhdl->io_ops._write16_async; + val = rtw_cpu_to_le16(val); + ret = _write16_async(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} +int _rtw_write32_async(_adapter *adapter, u32 addr, u32 val) +{ + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val); + int ret; + _write32_async = pintfhdl->io_ops._write32_async; + val = rtw_cpu_to_le32(val); + ret = _write32_async(pintfhdl, addr, val); + + return RTW_STATUS_CODE(ret); +} + +void _rtw_read_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem); + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + + + if (RTW_CANNOT_RUN(adapter)) { + return; + } + + _read_mem = pintfhdl->io_ops._read_mem; + + _read_mem(pintfhdl, addr, cnt, pmem); + + +} + +void _rtw_write_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem); + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + + + _write_mem = pintfhdl->io_ops._write_mem; + + _write_mem(pintfhdl, addr, cnt, pmem); + + +} + +void _rtw_read_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + u32(*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem); + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + + + if (RTW_CANNOT_RUN(adapter)) { + return; + } + + _read_port = pintfhdl->io_ops._read_port; + + _read_port(pintfhdl, addr, cnt, pmem); + + +} + +void _rtw_read_port_cancel(_adapter *adapter) +{ + void (*_read_port_cancel)(struct intf_hdl *pintfhdl); + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + + _read_port_cancel = pintfhdl->io_ops._read_port_cancel; + + RTW_DISABLE_FUNC(adapter, DF_RX_BIT); + + if (_read_port_cancel) + _read_port_cancel(pintfhdl); +} + +u32 _rtw_write_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem) +{ + u32(*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem); + /* struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue; */ + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + u32 ret = _SUCCESS; + + + _write_port = pintfhdl->io_ops._write_port; + + ret = _write_port(pintfhdl, addr, cnt, pmem); + + + return ret; +} + +u32 _rtw_write_port_and_wait(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem, int timeout_ms) +{ + int ret = _SUCCESS; + struct xmit_buf *pxmitbuf = (struct xmit_buf *)pmem; + struct submit_ctx sctx; + + rtw_sctx_init(&sctx, timeout_ms); + pxmitbuf->sctx = &sctx; + + ret = _rtw_write_port(adapter, addr, cnt, pmem); + + if (ret == _SUCCESS) { + ret = rtw_sctx_wait(&sctx, __func__); + + if (ret != _SUCCESS) + pxmitbuf->sctx = NULL; + } + + return ret; +} + +void _rtw_write_port_cancel(_adapter *adapter) +{ + void (*_write_port_cancel)(struct intf_hdl *pintfhdl); + struct io_priv *pio_priv = &adapter->iopriv; + struct intf_hdl *pintfhdl = &(pio_priv->intf); + + _write_port_cancel = pintfhdl->io_ops._write_port_cancel; + + RTW_DISABLE_FUNC(adapter, DF_TX_BIT); + + if (_write_port_cancel) + _write_port_cancel(pintfhdl); +} +int rtw_init_io_priv(_adapter *padapter, void (*set_intf_ops)(_adapter *padapter, struct _io_ops *pops)) +{ + struct io_priv *piopriv = &padapter->iopriv; + struct intf_hdl *pintf = &piopriv->intf; + + if (set_intf_ops == NULL) + return _FAIL; + + piopriv->padapter = padapter; + pintf->padapter = padapter; + pintf->pintf_dev = adapter_to_dvobj(padapter); + + set_intf_ops(padapter, &pintf->io_ops); + + return _SUCCESS; +} + +/* +* Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR +* @return _TRUE: +* @return _FALSE: +*/ +int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj) +{ + int ret = _FALSE; + int value; + + value = ATOMIC_INC_RETURN(&dvobj->continual_io_error); + if (value > MAX_CONTINUAL_IO_ERR) { + RTW_INFO("[dvobj:%p][ERROR] continual_io_error:%d > %d\n", dvobj, value, MAX_CONTINUAL_IO_ERR); + ret = _TRUE; + } else { + /* RTW_INFO("[dvobj:%p] continual_io_error:%d\n", dvobj, value); */ + } + return ret; +} + +/* +* Set the continual_io_error of this @param dvobjprive to 0 +*/ +void rtw_reset_continual_io_error(struct dvobj_priv *dvobj) +{ + ATOMIC_SET(&dvobj->continual_io_error, 0); +} + +#ifdef DBG_IO +#define RTW_IO_SNIFF_TYPE_RANGE 0 /* specific address range is accessed */ +#define RTW_IO_SNIFF_TYPE_VALUE 1 /* value match for sniffed range */ + +struct rtw_io_sniff_ent { + u8 chip; + u8 hci; + u32 addr; + u8 type; + union { + u32 end_addr; + struct { + u32 mask; + u32 val; + bool equal; + } vm; /* value match */ + } u; + bool trace; + char *tag; + bool (*assert_protsel)(_adapter *adapter, u32 addr, u8 len); +}; + +#define RTW_IO_SNIFF_RANGE_ENT(_chip, _hci, _addr, _end_addr, _trace, _tag) \ + {.chip = _chip, .hci = _hci, .addr = _addr, .u.end_addr = _end_addr, .trace = _trace, .tag = _tag, .type = RTW_IO_SNIFF_TYPE_RANGE,} + +#define RTW_IO_SNIFF_RANGE_PROT_ENT(_chip, _hci, _addr, _end_addr, _assert_protsel, _tag) \ + {.chip = _chip, .hci = _hci, .addr = _addr, .u.end_addr = _end_addr, .trace = 1, .assert_protsel = _assert_protsel, .tag = _tag, .type = RTW_IO_SNIFF_TYPE_RANGE,} + +#define RTW_IO_SNIFF_VALUE_ENT(_chip, _hci, _addr, _mask, _val, _equal, _trace, _tag) \ + {.chip = _chip, .hci = _hci, .addr = _addr, .u.vm.mask = _mask, .u.vm.val = _val, .u.vm.equal = _equal, .trace = _trace, .tag = _tag, .type = RTW_IO_SNIFF_TYPE_VALUE,} + +/* part or all sniffed range is enabled (not all 0) */ +#define RTW_IO_SNIFF_EN_ENT(_chip, _hci, _addr, _mask, _trace, _tag) \ + {.chip = _chip, .hci = _hci, .addr = _addr, .u.vm.mask = _mask, .u.vm.val = 0, .u.vm.equal = 0, .trace = _trace, .tag = _tag, .type = RTW_IO_SNIFF_TYPE_VALUE,} + +/* part or all sniffed range is disabled (not all 1) */ +#define RTW_IO_SNIFF_DIS_ENT(_chip, _hci, _addr, _mask, _trace, _tag) \ + {.chip = _chip, .hci = _hci, .addr = _addr, .u.vm.mask = _mask, .u.vm.val = 0xFFFFFFFF, .u.vm.equal = 0, .trace = _trace, .tag = _tag, .type = RTW_IO_SNIFF_TYPE_VALUE,} + +const struct rtw_io_sniff_ent read_sniff[] = { +#ifdef DBG_IO_HCI_EN_CHK + RTW_IO_SNIFF_EN_ENT(MAX_CHIP_TYPE, RTW_SDIO, 0x02, 0x1FC, 1, "SDIO 0x02[8:2] not all 0"), + RTW_IO_SNIFF_EN_ENT(MAX_CHIP_TYPE, RTW_USB, 0x02, 0x1E0, 1, "USB 0x02[8:5] not all 0"), + RTW_IO_SNIFF_EN_ENT(MAX_CHIP_TYPE, RTW_PCIE, 0x02, 0x01C, 1, "PCI 0x02[4:2] not all 0"), +#endif +#ifdef DBG_IO_SNIFF_EXAMPLE + RTW_IO_SNIFF_RANGE_ENT(MAX_CHIP_TYPE, 0, 0x522, 0x522, 0, "read TXPAUSE"), + RTW_IO_SNIFF_DIS_ENT(MAX_CHIP_TYPE, 0, 0x02, 0x3, 0, "0x02[1:0] not all 1"), +#endif +#ifdef DBG_IO_PROT_SEL + RTW_IO_SNIFF_RANGE_PROT_ENT(MAX_CHIP_TYPE, 0, 0x1501, 0x1513, rtw_assert_protsel_port, "protsel port"), + RTW_IO_SNIFF_RANGE_PROT_ENT(MAX_CHIP_TYPE, 0, 0x153a, 0x153b, rtw_assert_protsel_atimdtim, "protsel atimdtim"), +#endif +}; + +const int read_sniff_num = sizeof(read_sniff) / sizeof(struct rtw_io_sniff_ent); + +const struct rtw_io_sniff_ent write_sniff[] = { +#ifdef DBG_IO_HCI_EN_CHK + RTW_IO_SNIFF_EN_ENT(MAX_CHIP_TYPE, RTW_SDIO, 0x02, 0x1FC, 1, "SDIO 0x02[8:2] not all 0"), + RTW_IO_SNIFF_EN_ENT(MAX_CHIP_TYPE, RTW_USB, 0x02, 0x1E0, 1, "USB 0x02[8:5] not all 0"), + RTW_IO_SNIFF_EN_ENT(MAX_CHIP_TYPE, RTW_PCIE, 0x02, 0x01C, 1, "PCI 0x02[4:2] not all 0"), +#endif +#ifdef DBG_IO_8822C_1TX_PATH_EN + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x1a04, 0xc0000000, 0x02, 1, 0, "write tx_path_en_cck A enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x1a04, 0xc0000000, 0x01, 1, 0, "write tx_path_en_cck B enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x1a04, 0xc0000000, 0x03, 1, 1, "write tx_path_en_cck AB enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x820, 0x03, 0x01, 1, 0, "write tx_path_en_ofdm_1sts A enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x820, 0x03, 0x02, 1, 0, "write tx_path_en_ofdm_1sts B enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x820, 0x03, 0x03, 1, 1, "write tx_path_en_ofdm_1sts AB enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x820, 0x30, 0x01, 1, 0, "write tx_path_en_ofdm_2sts A enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x820, 0x30, 0x02, 1, 0, "write tx_path_en_ofdm_2sts B enabled"), + RTW_IO_SNIFF_VALUE_ENT(RTL8822C, 0, 0x820, 0x30, 0x03, 1, 1, "write tx_path_en_ofdm_2sts AB enabled"), +#endif +#ifdef DBG_IO_SNIFF_EXAMPLE + RTW_IO_SNIFF_RANGE_ENT(MAX_CHIP_TYPE, 0, 0x522, 0x522, 0, "write TXPAUSE"), + RTW_IO_SNIFF_DIS_ENT(MAX_CHIP_TYPE, 0, 0x02, 0x3, 0, "0x02[1:0] not all 1"), +#endif +}; + +const int write_sniff_num = sizeof(write_sniff) / sizeof(struct rtw_io_sniff_ent); + +static bool match_io_sniff_ranges(_adapter *adapter + , const struct rtw_io_sniff_ent *sniff, int i, u32 addr, u16 len) +{ + + /* check if IO range after sniff end address */ + if (addr > sniff->u.end_addr) + return 0; + + if (sniff->assert_protsel && + sniff->assert_protsel(adapter, addr, len)) + return 0; + + return 1; +} + +static bool match_io_sniff_value(_adapter *adapter + , const struct rtw_io_sniff_ent *sniff, int i, u32 addr, u8 len, u32 val) +{ + u8 sniff_len; + s8 mask_shift; + u32 mask; + s8 value_shift; + u32 value; + bool ret = 0; + + /* check if IO range after sniff end address */ + sniff_len = 4; + while (!(sniff->u.vm.mask & (0xFF << ((sniff_len - 1) * 8)))) { + sniff_len--; + if (sniff_len == 0) + goto exit; + } + if (sniff->addr + sniff_len <= addr) + goto exit; + + /* align to IO addr */ + mask_shift = (sniff->addr - addr) * 8; + value_shift = mask_shift + bitshift(sniff->u.vm.mask); + if (mask_shift > 0) + mask = sniff->u.vm.mask << mask_shift; + else if (mask_shift < 0) + mask = sniff->u.vm.mask >> -mask_shift; + else + mask = sniff->u.vm.mask; + + if (value_shift > 0) + value = sniff->u.vm.val << value_shift; + else if (mask_shift < 0) + value = sniff->u.vm.val >> -value_shift; + else + value = sniff->u.vm.val; + + if ((sniff->u.vm.equal && (mask & val) == (mask & value)) + || (!sniff->u.vm.equal && (mask & val) != (mask & value)) + ) { + ret = 1; + if (0) + RTW_INFO(FUNC_ADPT_FMT" addr:0x%x len:%u val:0x%x (i:%d sniff_len:%u m_shift:%d mask:0x%x v_shifd:%d value:0x%x equal:%d)\n" + , FUNC_ADPT_ARG(adapter), addr, len, val, i, sniff_len, mask_shift, mask, value_shift, value, sniff->u.vm.equal); + } + +exit: + return ret; +} + +static bool match_io_sniff(_adapter *adapter + , const struct rtw_io_sniff_ent *sniff, int i, u32 addr, u8 len, u32 val) +{ + bool ret = 0; + + if (sniff->chip != MAX_CHIP_TYPE + && sniff->chip != rtw_get_chip_type(adapter)) + goto exit; + if (sniff->hci + && !(sniff->hci & rtw_get_intf_type(adapter))) + goto exit; + if (sniff->addr >= addr + len) /* IO range below sniff start address */ + goto exit; + + switch (sniff->type) { + case RTW_IO_SNIFF_TYPE_RANGE: + ret = match_io_sniff_ranges(adapter, sniff, i, addr, len); + break; + case RTW_IO_SNIFF_TYPE_VALUE: + if (len == 1 || len == 2 || len == 4) + ret = match_io_sniff_value(adapter, sniff, i, addr, len, val); + break; + default: + rtw_warn_on(1); + break; + } + +exit: + return ret; +} + +u32 match_read_sniff(_adapter *adapter, u32 addr, u16 len, u32 val) +{ + int i; + bool trace = 0; + u32 match = 0; + + for (i = 0; i < read_sniff_num; i++) { + if (match_io_sniff(adapter, &read_sniff[i], i, addr, len, val)) { + match++; + trace |= read_sniff[i].trace; + if (read_sniff[i].tag) + RTW_INFO("DBG_IO TAG %s\n", read_sniff[i].tag); + } + } + + rtw_warn_on(trace); + + return match; +} + +u32 match_write_sniff(_adapter *adapter, u32 addr, u16 len, u32 val) +{ + int i; + bool trace = 0; + u32 match = 0; + + for (i = 0; i < write_sniff_num; i++) { + if (match_io_sniff(adapter, &write_sniff[i], i, addr, len, val)) { + match++; + trace |= write_sniff[i].trace; + if (write_sniff[i].tag) + RTW_INFO("DBG_IO TAG %s\n", write_sniff[i].tag); + } + } + + rtw_warn_on(trace); + + return match; +} + +struct rf_sniff_ent { + u8 path; + u16 reg; + u32 mask; +}; + +struct rf_sniff_ent rf_read_sniff_ranges[] = { + /* example for all path addr 0x55 with all RF Reg mask */ + /* {MAX_RF_PATH, 0x55, bRFRegOffsetMask}, */ +}; + +struct rf_sniff_ent rf_write_sniff_ranges[] = { + /* example for all path addr 0x55 with all RF Reg mask */ + /* {MAX_RF_PATH, 0x55, bRFRegOffsetMask}, */ +}; + +int rf_read_sniff_num = sizeof(rf_read_sniff_ranges) / sizeof(struct rf_sniff_ent); +int rf_write_sniff_num = sizeof(rf_write_sniff_ranges) / sizeof(struct rf_sniff_ent); + +bool match_rf_read_sniff_ranges(_adapter *adapter, u8 path, u32 addr, u32 mask) +{ + int i; + + for (i = 0; i < rf_read_sniff_num; i++) { + if (rf_read_sniff_ranges[i].path == MAX_RF_PATH || rf_read_sniff_ranges[i].path == path) + if (addr == rf_read_sniff_ranges[i].reg && (mask & rf_read_sniff_ranges[i].mask)) + return _TRUE; + } + + return _FALSE; +} + +bool match_rf_write_sniff_ranges(_adapter *adapter, u8 path, u32 addr, u32 mask) +{ + int i; + + for (i = 0; i < rf_write_sniff_num; i++) { + if (rf_write_sniff_ranges[i].path == MAX_RF_PATH || rf_write_sniff_ranges[i].path == path) + if (addr == rf_write_sniff_ranges[i].reg && (mask & rf_write_sniff_ranges[i].mask)) + return _TRUE; + } + + return _FALSE; +} + +void dbg_rtw_reg_read_monitor(_adapter *adapter, u32 addr, u32 len, u32 val, const char *caller, const int line) +{ + if (match_read_sniff(adapter, addr, len, val)) { + switch (len) { + case 1: + RTW_INFO("DBG_IO %s:%d read8(0x%04x) return 0x%02x\n" + , caller, line, addr, val); + break; + case 2: + RTW_INFO("DBG_IO %s:%d read16(0x%04x) return 0x%04x\n" + , caller, line, addr, val); + break; + case 4: + RTW_INFO("DBG_IO %s:%d read32(0x%04x) return 0x%08x\n" + , caller, line, addr, val); + break; + default: + RTW_INFO("DBG_IO %s:%d readN(0x%04x, %u)\n" + , caller, line, addr, len); + } + } +} + +void dbg_rtw_reg_write_monitor(_adapter *adapter, u32 addr, u32 len, u32 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, len, val)) { + switch (len) { + case 1: + RTW_INFO("DBG_IO %s:%d write8(0x%04x, 0x%02x)\n" + , caller, line, addr, val); + break; + case 2: + RTW_INFO("DBG_IO %s:%d write16(0x%04x, 0x%04x)\n" + , caller, line, addr, val); + break; + case 4: + RTW_INFO("DBG_IO %s:%d write32(0x%04x, 0x%08x)\n" + , caller, line, addr, val); + break; + default: + RTW_INFO("DBG_IO %s:%d rtw_writeN(0x%04x, %u)\n" + , caller, line, addr, len); + } + } +} + +u8 dbg_rtw_read8(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u8 val = _rtw_read8(adapter, addr); + + if (match_read_sniff(adapter, addr, 1, val)) { + RTW_INFO("DBG_IO %s:%d rtw_read8(0x%04x) return 0x%02x\n" + , caller, line, addr, val); + } + + return val; +} + +u16 dbg_rtw_read16(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u16 val = _rtw_read16(adapter, addr); + + if (match_read_sniff(adapter, addr, 2, val)) { + RTW_INFO("DBG_IO %s:%d rtw_read16(0x%04x) return 0x%04x\n" + , caller, line, addr, val); + } + + return val; +} + +u32 dbg_rtw_read32(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u32 val = _rtw_read32(adapter, addr); + + if (match_read_sniff(adapter, addr, 4, val)) { + RTW_INFO("DBG_IO %s:%d rtw_read32(0x%04x) return 0x%08x\n" + , caller, line, addr, val); + } + + return val; +} + +int dbg_rtw_write8(_adapter *adapter, u32 addr, u8 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, 1, val)) { + RTW_INFO("DBG_IO %s:%d rtw_write8(0x%04x, 0x%02x)\n" + , caller, line, addr, val); + } + + return _rtw_write8(adapter, addr, val); +} +int dbg_rtw_write16(_adapter *adapter, u32 addr, u16 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, 2, val)) { + RTW_INFO("DBG_IO %s:%d rtw_write16(0x%04x, 0x%04x)\n" + , caller, line, addr, val); + } + + return _rtw_write16(adapter, addr, val); +} +int dbg_rtw_write32(_adapter *adapter, u32 addr, u32 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, 4, val)) { + RTW_INFO("DBG_IO %s:%d rtw_write32(0x%04x, 0x%08x)\n" + , caller, line, addr, val); + } + + return _rtw_write32(adapter, addr, val); +} +int dbg_rtw_writeN(_adapter *adapter, u32 addr , u32 length , u8 *data, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, length, 0)) { + RTW_INFO("DBG_IO %s:%d rtw_writeN(0x%04x, %u)\n" + , caller, line, addr, length); + } + + return _rtw_writeN(adapter, addr, length, data); +} + +#ifdef CONFIG_SDIO_HCI +u8 dbg_rtw_sd_f0_read8(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u8 val = _rtw_sd_f0_read8(adapter, addr); + +#if 0 + if (match_read_sniff(adapter, addr, 1, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_f0_read8(0x%04x) return 0x%02x\n" + , caller, line, addr, val); + } +#endif + + return val; +} + +#ifdef CONFIG_SDIO_INDIRECT_ACCESS +u8 dbg_rtw_sd_iread8(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u8 val = rtw_sd_iread8(adapter, addr); + + if (match_read_sniff(adapter, addr, 1, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_iread8(0x%04x) return 0x%02x\n" + , caller, line, addr, val); + } + + return val; +} + +u16 dbg_rtw_sd_iread16(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u16 val = _rtw_sd_iread16(adapter, addr); + + if (match_read_sniff(adapter, addr, 2, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_iread16(0x%04x) return 0x%04x\n" + , caller, line, addr, val); + } + + return val; +} + +u32 dbg_rtw_sd_iread32(_adapter *adapter, u32 addr, const char *caller, const int line) +{ + u32 val = _rtw_sd_iread32(adapter, addr); + + if (match_read_sniff(adapter, addr, 4, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_iread32(0x%04x) return 0x%08x\n" + , caller, line, addr, val); + } + + return val; +} + +int dbg_rtw_sd_iwrite8(_adapter *adapter, u32 addr, u8 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, 1, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_iwrite8(0x%04x, 0x%02x)\n" + , caller, line, addr, val); + } + + return _rtw_sd_iwrite8(adapter, addr, val); +} +int dbg_rtw_sd_iwrite16(_adapter *adapter, u32 addr, u16 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, 2, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_iwrite16(0x%04x, 0x%04x)\n" + , caller, line, addr, val); + } + + return _rtw_sd_iwrite16(adapter, addr, val); +} +int dbg_rtw_sd_iwrite32(_adapter *adapter, u32 addr, u32 val, const char *caller, const int line) +{ + if (match_write_sniff(adapter, addr, 4, val)) { + RTW_INFO("DBG_IO %s:%d rtw_sd_iwrite32(0x%04x, 0x%08x)\n" + , caller, line, addr, val); + } + + return _rtw_sd_iwrite32(adapter, addr, val); +} + +#endif /* CONFIG_SDIO_INDIRECT_ACCESS */ + +#endif /* CONFIG_SDIO_HCI */ + +#endif diff --git a/core/rtw_ioctl_query.c b/core/rtw_ioctl_query.c new file mode 100644 index 0000000..9392138 --- /dev/null +++ b/core/rtw_ioctl_query.c @@ -0,0 +1,19 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_IOCTL_QUERY_C_ + +#include + + diff --git a/core/rtw_ioctl_set.c b/core/rtw_ioctl_set.c new file mode 100644 index 0000000..060546c --- /dev/null +++ b/core/rtw_ioctl_set.c @@ -0,0 +1,929 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_IOCTL_SET_C_ + +#include +#include + + +extern void indicate_wx_scan_complete_event(_adapter *padapter); + +#define IS_MAC_ADDRESS_BROADCAST(addr) \ + (\ + ((addr[0] == 0xff) && (addr[1] == 0xff) && \ + (addr[2] == 0xff) && (addr[3] == 0xff) && \ + (addr[4] == 0xff) && (addr[5] == 0xff)) ? _TRUE : _FALSE \ + ) + +u8 rtw_validate_bssid(u8 *bssid) +{ + u8 ret = _TRUE; + + if (is_zero_mac_addr(bssid) + || is_broadcast_mac_addr(bssid) + || is_multicast_mac_addr(bssid) + ) + ret = _FALSE; + + return ret; +} + +u8 rtw_validate_ssid(NDIS_802_11_SSID *ssid) +{ +#ifdef CONFIG_VALIDATE_SSID + u8 i; +#endif + u8 ret = _TRUE; + + + if (ssid->SsidLength > 32) { + ret = _FALSE; + goto exit; + } + +#ifdef CONFIG_VALIDATE_SSID + for (i = 0; i < ssid->SsidLength; i++) { + /* wifi, printable ascii code must be supported */ + if (!((ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e))) { + ret = _FALSE; + break; + } + } +#endif /* CONFIG_VALIDATE_SSID */ + +exit: + + + return ret; +} + +u8 rtw_do_join(_adapter *padapter); +u8 rtw_do_join(_adapter *padapter) +{ + _irqL irqL; + _list *plist, *phead; + u8 *pibss = NULL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sitesurvey_parm parm; + _queue *queue = &(pmlmepriv->scanned_queue); + u8 ret = _SUCCESS; + + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + phead = get_list_head(queue); + plist = get_next(phead); + + + pmlmepriv->cur_network.join_res = -2; + + set_fwstate(pmlmepriv, WIFI_UNDER_LINKING); + + pmlmepriv->pscanned = plist; + + pmlmepriv->to_join = _TRUE; + + rtw_init_sitesurvey_parm(padapter, &parm); + _rtw_memcpy(&parm.ssid[0], &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID)); + parm.ssid_num = 1; + + if (pmlmepriv->assoc_ch) { + parm.ch_num = 1; + parm.ch[0].hw_value = pmlmepriv->assoc_ch; + parm.ch[0].flags = 0; + } + + if (_rtw_queue_empty(queue) == _TRUE) { + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + + /* when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty */ + /* we try to issue sitesurvey firstly */ + + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == _FALSE + || rtw_to_roam(padapter) > 0 + ) { + u8 ssc_chk = rtw_sitesurvey_condition_check(padapter, _FALSE); + + if ((ssc_chk == SS_ALLOW) || (ssc_chk == SS_DENY_BUSY_TRAFFIC) ){ + /* submit site_survey_cmd */ + ret = rtw_sitesurvey_cmd(padapter, &parm); + if (_SUCCESS != ret) + pmlmepriv->to_join = _FALSE; + } else { + /*if (ssc_chk == SS_DENY_BUDDY_UNDER_SURVEY)*/ + pmlmepriv->to_join = _FALSE; + ret = _FAIL; + } + } else { + pmlmepriv->to_join = _FALSE; + ret = _FAIL; + } + + goto exit; + } else { + int select_ret; + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv); + if (select_ret == _SUCCESS) { + pmlmepriv->to_join = _FALSE; + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + } else { + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) { + #ifdef CONFIG_AP_MODE + /* submit createbss_cmd to change to a ADHOC_MASTER */ + + /* pmlmepriv->lock has been acquired by caller... */ + WLAN_BSSID_EX *pdev_network = &(padapter->registrypriv.dev_network); + + /*pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;*/ + init_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + + pibss = padapter->registrypriv.dev_network.MacAddress; + + _rtw_memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID)); + _rtw_memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID)); + + rtw_update_registrypriv_dev_network(padapter); + + rtw_generate_random_ibss(pibss); + + if (rtw_create_ibss_cmd(padapter, 0) != _SUCCESS) { + ret = _FALSE; + goto exit; + } + + pmlmepriv->to_join = _FALSE; + #endif /* CONFIG_AP_MODE */ + } else { + /* can't associate ; reset under-linking */ + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + + /* when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue */ + /* we try to issue sitesurvey firstly */ + if (pmlmepriv->LinkDetectInfo.bBusyTraffic == _FALSE + || rtw_to_roam(padapter) > 0 + ) { + u8 ssc_chk = rtw_sitesurvey_condition_check(padapter, _FALSE); + + if ((ssc_chk == SS_ALLOW) || (ssc_chk == SS_DENY_BUSY_TRAFFIC)){ + /* RTW_INFO(("rtw_do_join() when no desired bss in scanning queue\n"); */ + ret = rtw_sitesurvey_cmd(padapter, &parm); + if (_SUCCESS != ret) + pmlmepriv->to_join = _FALSE; + } else { + /*if (ssc_chk == SS_DENY_BUDDY_UNDER_SURVEY) { + } else {*/ + ret = _FAIL; + pmlmepriv->to_join = _FALSE; + } + } else { + ret = _FAIL; + pmlmepriv->to_join = _FALSE; + } + } + + } + + } + +exit: + + return ret; +} + +u8 rtw_set_802_11_bssid(_adapter *padapter, u8 *bssid) +{ + _irqL irqL; + u8 status = _SUCCESS; + + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + RTW_PRINT("set bssid:%pM\n", bssid); + + if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 && bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) || + (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF && bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) { + status = _FAIL; + goto exit; + } + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + + RTW_INFO("Set BSSID under fw_state=0x%08x\n", get_fwstate(pmlmepriv)); + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) + goto handle_tkip_countermeasure; + else if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) == _TRUE) + goto release_mlme_lock; + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE | WIFI_ADHOC_MASTER_STATE) == _TRUE) { + + if (_rtw_memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN) == _TRUE) { + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _FALSE) + goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */ + } else { + + rtw_disassoc_cmd(padapter, 0, 0); + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + rtw_indicate_disconnect(padapter, 0, _FALSE); + + rtw_free_assoc_resources_cmd(padapter, _TRUE, 0); + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) { + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + } + } + } + +handle_tkip_countermeasure: + if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) { + status = _FAIL; + goto release_mlme_lock; + } + + _rtw_memset(&pmlmepriv->assoc_ssid, 0, sizeof(NDIS_802_11_SSID)); + _rtw_memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN); + pmlmepriv->assoc_ch = 0; + pmlmepriv->assoc_by_bssid = _TRUE; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) + pmlmepriv->to_join = _TRUE; + else + status = rtw_do_join(padapter); + +release_mlme_lock: + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +exit: + + + return status; +} + +u8 rtw_set_802_11_ssid(_adapter *padapter, NDIS_802_11_SSID *ssid) +{ + _irqL irqL; + u8 status = _SUCCESS; + + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *pnetwork = &pmlmepriv->cur_network; + + + RTW_PRINT("set ssid [%s] fw_state=0x%08x\n", + ssid->Ssid, get_fwstate(pmlmepriv)); + + if (!rtw_is_hw_init_completed(padapter)) { + status = _FAIL; + goto exit; + } + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + RTW_INFO("Set SSID under fw_state=0x%08x\n", get_fwstate(pmlmepriv)); + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) + goto handle_tkip_countermeasure; + else if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) == _TRUE) + goto release_mlme_lock; + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE | WIFI_ADHOC_MASTER_STATE) == _TRUE) { + + if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) && + (_rtw_memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength) == _TRUE)) { + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _FALSE)) { + + if (rtw_is_same_ibss(padapter, pnetwork) == _FALSE) { + /* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */ + rtw_disassoc_cmd(padapter, 0, 0); + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + rtw_indicate_disconnect(padapter, 0, _FALSE); + + rtw_free_assoc_resources_cmd(padapter, _TRUE, 0); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) { + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + } + } else { + goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */ + } + } +#ifdef CONFIG_LPS + else + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 0); +#endif + } else { + + rtw_disassoc_cmd(padapter, 0, 0); + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + rtw_indicate_disconnect(padapter, 0, _FALSE); + + rtw_free_assoc_resources_cmd(padapter, _TRUE, 0); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) { + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + } + } + } + +handle_tkip_countermeasure: + if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) { + status = _FAIL; + goto release_mlme_lock; + } + + if (rtw_validate_ssid(ssid) == _FALSE) { + status = _FAIL; + goto release_mlme_lock; + } + + _rtw_memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(NDIS_802_11_SSID)); + pmlmepriv->assoc_ch = 0; + pmlmepriv->assoc_by_bssid = _FALSE; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) + pmlmepriv->to_join = _TRUE; + else + status = rtw_do_join(padapter); + +release_mlme_lock: + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +exit: + + + return status; + +} + +u8 rtw_set_802_11_connect(_adapter *padapter, + u8 *bssid, NDIS_802_11_SSID *ssid, u16 ch) +{ + _irqL irqL; + u8 status = _SUCCESS; + bool bssid_valid = _TRUE; + bool ssid_valid = _TRUE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + if (!ssid || rtw_validate_ssid(ssid) == _FALSE) + ssid_valid = _FALSE; + + if (!bssid || rtw_validate_bssid(bssid) == _FALSE) + bssid_valid = _FALSE; + + if (ssid_valid == _FALSE && bssid_valid == _FALSE) { + RTW_INFO(FUNC_ADPT_FMT" ssid:%p, ssid_valid:%d, bssid:%p, bssid_valid:%d\n", + FUNC_ADPT_ARG(padapter), ssid, ssid_valid, bssid, bssid_valid); + status = _FAIL; + goto exit; + } + + if (!rtw_is_hw_init_completed(padapter)) { + status = _FAIL; + goto exit; + } + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + RTW_PRINT(FUNC_ADPT_FMT" fw_state=0x%08x\n", + FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv)); + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) + goto handle_tkip_countermeasure; + else if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) == _TRUE) + goto release_mlme_lock; + +handle_tkip_countermeasure: + if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) { + status = _FAIL; + goto release_mlme_lock; + } + + if (ssid && ssid_valid) + _rtw_memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(NDIS_802_11_SSID)); + else + _rtw_memset(&pmlmepriv->assoc_ssid, 0, sizeof(NDIS_802_11_SSID)); + + if (bssid && bssid_valid) { + _rtw_memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN); + pmlmepriv->assoc_by_bssid = _TRUE; + } else + pmlmepriv->assoc_by_bssid = _FALSE; + + pmlmepriv->assoc_ch = ch; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) + pmlmepriv->to_join = _TRUE; + else + status = rtw_do_join(padapter); + +release_mlme_lock: + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +exit: + return status; +} + +u8 rtw_set_802_11_infrastructure_mode(_adapter *padapter, + NDIS_802_11_NETWORK_INFRASTRUCTURE networktype, u8 flags) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *cur_network = &pmlmepriv->cur_network; + NDIS_802_11_NETWORK_INFRASTRUCTURE *pold_state = &(cur_network->network.InfrastructureMode); + u8 ap2sta_mode = _FALSE; + u8 ret = _TRUE; + u8 is_linked = _FALSE, is_adhoc_master = _FALSE; + + if (*pold_state != networktype) { + /* RTW_INFO("change mode, old_mode=%d, new_mode=%d, fw_state=0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */ + + if (*pold_state == Ndis802_11APMode + || *pold_state == Ndis802_11_mesh + ) { + /* change to other mode from Ndis802_11APMode/Ndis802_11_mesh */ + cur_network->join_res = -1; + ap2sta_mode = _TRUE; +#ifdef CONFIG_NATIVEAP_MLME + stop_ap_mode(padapter); +#endif + } + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + is_linked = check_fwstate(pmlmepriv, WIFI_ASOC_STATE); + is_adhoc_master = check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + + /* flags = 0, means enqueue cmd and no wait */ + if (flags != 0) + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + if ((is_linked == _TRUE) || (*pold_state == Ndis802_11IBSS)) + rtw_disassoc_cmd(padapter, 0, flags); + + if ((is_linked == _TRUE) || + (is_adhoc_master == _TRUE)) + rtw_free_assoc_resources_cmd(padapter, _TRUE, flags); + + if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) { + if (is_linked == _TRUE) { + rtw_indicate_disconnect(padapter, 0, _FALSE); /*will clr Linked_state; before this function, we must have checked whether issue dis-assoc_cmd or not*/ + } + } + + /* flags = 0, means enqueue cmd and no wait */ + if (flags != 0) + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + *pold_state = networktype; + + _clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE); + + switch (networktype) { + case Ndis802_11IBSS: + set_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + break; + + case Ndis802_11Infrastructure: + set_fwstate(pmlmepriv, WIFI_STATION_STATE); + + if (ap2sta_mode) + rtw_init_bcmc_stainfo(padapter); + break; + + case Ndis802_11APMode: + set_fwstate(pmlmepriv, WIFI_AP_STATE); +#ifdef CONFIG_NATIVEAP_MLME + start_ap_mode(padapter); + /* rtw_indicate_connect(padapter); */ +#endif + + break; + +#ifdef CONFIG_RTW_MESH + case Ndis802_11_mesh: + set_fwstate(pmlmepriv, WIFI_MESH_STATE); + start_ap_mode(padapter); + break; +#endif + + case Ndis802_11AutoUnknown: + case Ndis802_11InfrastructureMax: + break; +#ifdef CONFIG_WIFI_MONITOR + case Ndis802_11Monitor: + set_fwstate(pmlmepriv, WIFI_MONITOR_STATE); + break; +#endif /* CONFIG_WIFI_MONITOR */ + default: + ret = _FALSE; + rtw_warn_on(1); + } + + /* SecClearAllKeys(adapter); */ + + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + } + + return ret; +} + + +u8 rtw_set_802_11_disassociate(_adapter *padapter) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) { + + rtw_disassoc_cmd(padapter, 0, 0); + rtw_indicate_disconnect(padapter, 0, _FALSE); + /* modify for CONFIG_IEEE80211W, none 11w can use it */ + rtw_free_assoc_resources_cmd(padapter, _TRUE, 0); + if (_FAIL == rtw_pwr_wakeup(padapter)) + RTW_INFO("%s(): rtw_pwr_wakeup fail !!!\n", __FUNCTION__); + } + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + + return _TRUE; +} + +#if 1 +u8 rtw_set_802_11_bssid_list_scan(_adapter *padapter, struct sitesurvey_parm *pparm) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 res = _TRUE; + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + res = rtw_sitesurvey_cmd(padapter, pparm); + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + return res; +} + +#else +u8 rtw_set_802_11_bssid_list_scan(_adapter *padapter, struct sitesurvey_parm *pparm) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 res = _TRUE; + + + + if (padapter == NULL) { + res = _FALSE; + goto exit; + } + if (!rtw_is_hw_init_completed(padapter)) { + res = _FALSE; + goto exit; + } + + if ((check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY | WIFI_UNDER_LINKING) == _TRUE) || + (pmlmepriv->LinkDetectInfo.bBusyTraffic == _TRUE)) { + /* Scan or linking is in progress, do nothing. */ + res = _TRUE; + + + } else { + if (rtw_is_scan_deny(padapter)) { + RTW_INFO(FUNC_ADPT_FMT": scan deny\n", FUNC_ADPT_ARG(padapter)); + indicate_wx_scan_complete_event(padapter); + return _SUCCESS; + } + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + res = rtw_sitesurvey_cmd(padapter, pparm); + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + } +exit: + + + return res; +} +#endif + +#ifdef CONFIG_RTW_ACS +u8 rtw_set_acs_sitesurvey(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + struct sitesurvey_parm parm; + u8 uch; + u8 ch_num = 0; + int i; + BAND_TYPE band; + u8 (*center_chs_num)(u8) = NULL; + u8 (*center_chs)(u8, u8) = NULL; + u8 ret = _FAIL; + + if (!rtw_mi_get_ch_setting_union(adapter, &uch, NULL, NULL)) + goto exit; + + _rtw_memset(&parm, 0, sizeof(struct sitesurvey_parm)); + parm.scan_mode = SCAN_PASSIVE; + parm.bw = CHANNEL_WIDTH_20; + parm.acs = 1; + + for (band = BAND_ON_2_4G; band < BAND_MAX; band++) { + if (band == BAND_ON_2_4G) { + center_chs_num = center_chs_2g_num; + center_chs = center_chs_2g; + } else + #ifdef CONFIG_IEEE80211_BAND_5GHZ + if (band == BAND_ON_5G) { + center_chs_num = center_chs_5g_num; + center_chs = center_chs_5g; + } else + #endif + { + center_chs_num = NULL; + center_chs = NULL; + } + + if (!center_chs_num || !center_chs) + continue; + + if (rfctl->ch_sel_within_same_band) { + if (rtw_is_2g_ch(uch) && band != BAND_ON_2_4G) + continue; + #ifdef CONFIG_IEEE80211_BAND_5GHZ + if (rtw_is_5g_ch(uch) && band != BAND_ON_5G) + continue; + #endif + } + + ch_num = center_chs_num(CHANNEL_WIDTH_20); + for (i = 0; i < ch_num && parm.ch_num < RTW_CHANNEL_SCAN_AMOUNT; i++) { + parm.ch[parm.ch_num].hw_value = center_chs(CHANNEL_WIDTH_20, i); + parm.ch[parm.ch_num].flags = RTW_IEEE80211_CHAN_PASSIVE_SCAN; + parm.ch_num++; + } + } + + ret = rtw_set_802_11_bssid_list_scan(adapter, &parm); + +exit: + return ret; +} +#endif /* CONFIG_RTW_ACS */ + +u8 rtw_set_802_11_authentication_mode(_adapter *padapter, NDIS_802_11_AUTHENTICATION_MODE authmode) +{ + struct security_priv *psecuritypriv = &padapter->securitypriv; + int res; + u8 ret; + + + + psecuritypriv->ndisauthtype = authmode; + + + if (psecuritypriv->ndisauthtype > 3) + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X; + +#ifdef CONFIG_WAPI_SUPPORT + if (psecuritypriv->ndisauthtype == 6) + psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_WAPI; +#endif + + res = rtw_set_auth(padapter, psecuritypriv); + + if (res == _SUCCESS) + ret = _TRUE; + else + ret = _FALSE; + + + return ret; +} + +u8 rtw_set_802_11_add_wep(_adapter *padapter, NDIS_802_11_WEP *wep) +{ + + u8 bdefaultkey; + u8 btransmitkey; + sint keyid, res; + struct security_priv *psecuritypriv = &(padapter->securitypriv); + u8 ret = _SUCCESS; + + + bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? _FALSE : _TRUE; /* for ??? */ + btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? _TRUE : _FALSE; /* for ??? */ + keyid = wep->KeyIndex & 0x3fffffff; + + if (keyid >= 4) { + ret = _FALSE; + goto exit; + } + + switch (wep->KeyLength) { + case 5: + psecuritypriv->dot11PrivacyAlgrthm = _WEP40_; + break; + case 13: + psecuritypriv->dot11PrivacyAlgrthm = _WEP104_; + break; + default: + psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_; + break; + } + + + _rtw_memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->KeyMaterial), wep->KeyLength); + + psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength; + + psecuritypriv->dot11PrivacyKeyIndex = keyid; + + + res = rtw_set_key(padapter, psecuritypriv, keyid, 1, _TRUE); + + if (res == _FAIL) + ret = _FALSE; +exit: + + + return ret; + +} + +/* +* rtw_get_cur_max_rate - +* @adapter: pointer to _adapter structure +* +* Return 0 or 100Kbps +*/ +u16 rtw_get_cur_max_rate(_adapter *adapter) +{ + int j; + int i = 0; + u16 rate = 0, max_rate = 0; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + WLAN_BSSID_EX *pcur_bss = &pmlmepriv->cur_network.network; + int sta_bssrate_len = 0; + unsigned char sta_bssrate[NumRates]; + struct sta_info *psta = NULL; + u8 short_GI = 0; + +#ifdef CONFIG_MP_INCLUDED + if (adapter->registrypriv.mp_mode == 1) { + if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) + return 0; + } +#endif + + if ((check_fwstate(pmlmepriv, WIFI_ASOC_STATE) != _TRUE) + && (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) != _TRUE)) + return 0; + + psta = rtw_get_stainfo(&adapter->stapriv, get_bssid(pmlmepriv)); + if (psta == NULL) + return 0; + + short_GI = query_ra_short_GI(psta, rtw_get_tx_bw_mode(adapter, psta)); + +#ifdef CONFIG_80211N_HT + if (is_supported_ht(psta->wireless_mode)) { + max_rate = rtw_ht_mcs_rate((psta->cmn.bw_mode == CHANNEL_WIDTH_40) ? 1 : 0 + , short_GI + , psta->htpriv.ht_cap.supp_mcs_set + ); + } +#ifdef CONFIG_80211AC_VHT + else if (is_supported_vht(psta->wireless_mode)) + max_rate = ((rtw_vht_mcs_to_data_rate(psta->cmn.bw_mode, short_GI, pmlmepriv->vhtpriv.vht_highest_rate) + 1) >> 1) * 10; +#endif /* CONFIG_80211AC_VHT */ + else +#endif /* CONFIG_80211N_HT */ + { + /*station mode show :station && ap support rate; softap :show ap support rate*/ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) + get_rate_set(adapter, sta_bssrate, &sta_bssrate_len);/*get sta rate and length*/ + + + while ((pcur_bss->SupportedRates[i] != 0) && (pcur_bss->SupportedRates[i] != 0xFF)) { + rate = pcur_bss->SupportedRates[i] & 0x7F;/*AP support rates*/ + /*RTW_INFO("%s rate=%02X \n", __func__, rate);*/ + + /*check STA support rate or not */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) { + for (j = 0; j < sta_bssrate_len; j++) { + /* Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */ + if ((rate | IEEE80211_BASIC_RATE_MASK) + == (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) { + if (rate > max_rate) { + max_rate = rate; + } + break; + } + } + } else { + + if (rate > max_rate) + max_rate = rate; + + } + i++; + } + + max_rate = max_rate * 10 / 2; + } + return max_rate; +} + +/* +* rtw_set_scan_mode - +* @adapter: pointer to _adapter structure +* @scan_mode: +* +* Return _SUCCESS or _FAIL +*/ +int rtw_set_scan_mode(_adapter *adapter, RT_SCAN_TYPE scan_mode) +{ + if (scan_mode != SCAN_ACTIVE && scan_mode != SCAN_PASSIVE) + return _FAIL; + + adapter->mlmepriv.scan_mode = scan_mode; + + return _SUCCESS; +} + +/* +* rtw_set_channel_plan - +* @adapter: pointer to _adapter structure +* @channel_plan: +* +* Return _SUCCESS or _FAIL +*/ +int rtw_set_channel_plan(_adapter *adapter, u8 channel_plan) +{ + struct registry_priv *regsty = adapter_to_regsty(adapter); + + if (!REGSTY_REGD_SRC_FROM_OS(regsty)) + return rtw_set_chplan_cmd(adapter, RTW_CMDF_WAIT_ACK, channel_plan, 1); + RTW_WARN("%s(): not applied\n", __func__); + return _SUCCESS; +} + +/* +* rtw_set_country - +* @adapter: pointer to _adapter structure +* @country_code: string of country code +* +* Return _SUCCESS or _FAIL +*/ +int rtw_set_country(_adapter *adapter, const char *country_code) +{ +#ifdef CONFIG_RTW_IOCTL_SET_COUNTRY + struct registry_priv *regsty = adapter_to_regsty(adapter); + + if (!REGSTY_REGD_SRC_FROM_OS(regsty)) + return rtw_set_country_cmd(adapter, RTW_CMDF_WAIT_ACK, country_code, 1); +#endif + RTW_WARN("%s(): not applied\n", __func__); + return _SUCCESS; +} + +/* +* rtw_set_band - +* @adapter: pointer to _adapter structure +* @band: band to set +* +* Return _SUCCESS or _FAIL +*/ +int rtw_set_band(_adapter *adapter, u8 band) +{ + if (rtw_band_valid(band)) { + RTW_INFO(FUNC_ADPT_FMT" band:%d\n", FUNC_ADPT_ARG(adapter), band); + adapter->setband = band; + return _SUCCESS; + } + + RTW_PRINT(FUNC_ADPT_FMT" band:%d fail\n", FUNC_ADPT_ARG(adapter), band); + return _FAIL; +} diff --git a/core/rtw_iol.c b/core/rtw_iol.c new file mode 100644 index 0000000..714a3f4 --- /dev/null +++ b/core/rtw_iol.c @@ -0,0 +1,388 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include + +#ifdef CONFIG_IOL +struct xmit_frame *rtw_IOL_accquire_xmit_frame(ADAPTER *adapter) +{ + struct xmit_frame *xmit_frame; + struct xmit_buf *xmitbuf; + struct pkt_attrib *pattrib; + struct xmit_priv *pxmitpriv = &(adapter->xmitpriv); + +#if 1 + xmit_frame = rtw_alloc_xmitframe(pxmitpriv, 0); + if (xmit_frame == NULL) { + RTW_INFO("%s rtw_alloc_xmitframe return null\n", __FUNCTION__); + goto exit; + } + + xmitbuf = rtw_alloc_xmitbuf(pxmitpriv); + if (xmitbuf == NULL) { + RTW_INFO("%s rtw_alloc_xmitbuf return null\n", __FUNCTION__); + rtw_free_xmitframe(pxmitpriv, xmit_frame); + xmit_frame = NULL; + goto exit; + } + + xmit_frame->frame_tag = MGNT_FRAMETAG; + xmit_frame->pxmitbuf = xmitbuf; + xmit_frame->buf_addr = xmitbuf->pbuf; + xmitbuf->priv_data = xmit_frame; + + pattrib = &xmit_frame->attrib; + update_mgntframe_attrib(adapter, pattrib); + pattrib->qsel = QSLT_BEACON;/* Beacon */ + pattrib->subtype = WIFI_BEACON; + pattrib->pktlen = pattrib->last_txcmdsz = 0; + +#else + xmit_frame = alloc_mgtxmitframe(pxmitpriv); + if (xmit_frame == NULL) + RTW_INFO("%s alloc_mgtxmitframe return null\n", __FUNCTION__); + else { + pattrib = &xmit_frame->attrib; + update_mgntframe_attrib(adapter, pattrib); + pattrib->qsel = QSLT_BEACON; + pattrib->pktlen = pattrib->last_txcmdsz = 0; + } +#endif + +exit: + return xmit_frame; +} + + +int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len) +{ + struct pkt_attrib *pattrib = &xmit_frame->attrib; + u16 buf_offset; + u32 ori_len; + + buf_offset = TXDESC_OFFSET; + ori_len = buf_offset + pattrib->pktlen; + + /* check if the io_buf can accommodate new cmds */ + if (ori_len + cmd_len + 8 > MAX_XMITBUF_SZ) { + RTW_INFO("%s %u is large than MAX_XMITBUF_SZ:%u, can't accommodate new cmds\n", __FUNCTION__ + , ori_len + cmd_len + 8, MAX_XMITBUF_SZ); + return _FAIL; + } + + _rtw_memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len); + pattrib->pktlen += cmd_len; + pattrib->last_txcmdsz += cmd_len; + + /* RTW_INFO("%s ori:%u + cmd_len:%u = %u\n", __FUNCTION__, ori_len, cmd_len, buf_offset+pattrib->pktlen); */ + + return _SUCCESS; +} + +bool rtw_IOL_applied(ADAPTER *adapter) +{ + if (1 == adapter->registrypriv.fw_iol) + return _TRUE; + +#ifdef CONFIG_USB_HCI + if ((2 == adapter->registrypriv.fw_iol) && (IS_FULL_SPEED_USB(adapter))) + return _TRUE; +#endif + + return _FALSE; +} + +int rtw_IOL_exec_cmds_sync(ADAPTER *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt) +{ + return rtw_hal_iol_cmd(adapter, xmit_frame, max_wating_ms, bndy_cnt); +} + +#ifdef CONFIG_IOL_NEW_GENERATION +int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary) +{ + return _SUCCESS; +} +int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask) +{ + struct ioreg_cfg cmd = {8, IOREG_CMD_WB_REG, 0x0, 0x0, 0x0}; + + /* RTW_PUT_LE16((u8*)&cmd.address, addr); */ + /* RTW_PUT_LE32((u8*)&cmd.value, (u32)value); */ + cmd.address = cpu_to_le16(addr); + cmd.data = cpu_to_le32(value); + + if (mask != 0xFF) { + cmd.length = 12; + /* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask); */ + cmd.mask = cpu_to_le32(mask); + } + + /* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FUNCTION__, addr,value,mask); */ + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length); + +} +int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, u16 mask) +{ + struct ioreg_cfg cmd = {8, IOREG_CMD_WW_REG, 0x0, 0x0, 0x0}; + + /* RTW_PUT_LE16((u8*)&cmd.address, addr); */ + /* RTW_PUT_LE32((u8*)&cmd.value, (u32)value); */ + cmd.address = cpu_to_le16(addr); + cmd.data = cpu_to_le32(value); + + if (mask != 0xFFFF) { + cmd.length = 12; + /* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask); */ + cmd.mask = cpu_to_le32(mask); + } + + /* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FUNCTION__, addr,value,mask); */ + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length); + +} +int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask) +{ + struct ioreg_cfg cmd = {8, IOREG_CMD_WD_REG, 0x0, 0x0, 0x0}; + + /* RTW_PUT_LE16((u8*)&cmd.address, addr); */ + /* RTW_PUT_LE32((u8*)&cmd.value, (u32)value); */ + cmd.address = cpu_to_le16(addr); + cmd.data = cpu_to_le32(value); + + if (mask != 0xFFFFFFFF) { + cmd.length = 12; + /* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask); */ + cmd.mask = cpu_to_le32(mask); + } + + /* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__, addr,value,mask); */ + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length); + +} + +int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask) +{ + struct ioreg_cfg cmd = {8, IOREG_CMD_W_RF, 0x0, 0x0, 0x0}; + + /* RTW_PUT_LE16((u8*)&cmd.address, addr); */ + /* RTW_PUT_LE32((u8*)&cmd.value, (u32)value); */ + cmd.address = (rf_path << 8) | ((addr) & 0xFF); + cmd.data = cpu_to_le32(value); + + if (mask != 0x000FFFFF) { + cmd.length = 12; + /* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask); */ + cmd.mask = cpu_to_le32(mask); + } + + /* RTW_INFO("%s rf_path:0x%02x addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__,rf_path, addr,value,mask); */ + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length); + +} + + + +int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us) +{ + struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0}; + /* RTW_PUT_LE16((u8*)&cmd.address, us); */ + cmd.address = cpu_to_le16(us); + + /* RTW_INFO("%s %u\n", __FUNCTION__, us); */ + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4); +} + +int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms) +{ + struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0}; + + /* RTW_PUT_LE16((u8*)&cmd.address, ms); */ + cmd.address = cpu_to_le16(ms); + + /* RTW_INFO("%s %u\n", __FUNCTION__, ms); */ + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4); +} +int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame) +{ + struct ioreg_cfg cmd = {4, IOREG_CMD_END, 0xFFFF, 0xFF, 0x0}; + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4); + +} + +u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame) +{ + u8 is_cmd_bndy = _FALSE; + if (((pxmit_frame->attrib.pktlen + 32) % 256) + 8 >= 256) { + rtw_IOL_append_END_cmd(pxmit_frame); + pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen + 32) / 256) + 1) * 256); + + /* printk("==> %s, pktlen(%d)\n",__FUNCTION__,pxmit_frame->attrib.pktlen); */ + pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen; + is_cmd_bndy = _TRUE; + } + return is_cmd_bndy; +} + +void rtw_IOL_cmd_buf_dump(ADAPTER *Adapter, int buf_len, u8 *pbuf) +{ + int i; + int j = 1; + + printk("###### %s ######\n", __FUNCTION__); + for (i = 0; i < buf_len; i++) { + printk("%02x-", *(pbuf + i)); + + if (j % 32 == 0) + printk("\n"); + j++; + } + printk("\n"); + printk("============= ioreg_cmd len = %d ===============\n", buf_len); +} + + +#else /* CONFIG_IOL_NEW_GENERATION */ +int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary) +{ + IOL_CMD cmd = {0x0, IOL_CMD_LLT, 0x0, 0x0}; + + RTW_PUT_BE32((u8 *)&cmd.value, (u32)page_boundary); + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8); +} + +int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value) +{ + IOL_CMD cmd = {0x0, IOL_CMD_WB_REG, 0x0, 0x0}; + + RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr); + RTW_PUT_BE32((u8 *)&cmd.value, (u32)value); + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8); +} + +int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value) +{ + IOL_CMD cmd = {0x0, IOL_CMD_WW_REG, 0x0, 0x0}; + + RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr); + RTW_PUT_BE32((u8 *)&cmd.value, (u32)value); + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8); +} + +int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value) +{ + IOL_CMD cmd = {0x0, IOL_CMD_WD_REG, 0x0, 0x0}; + u8 *pos = (u8 *)&cmd; + + RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr); + RTW_PUT_BE32((u8 *)&cmd.value, (u32)value); + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8); +} + +#ifdef DBG_IO +int dbg_rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, const char *caller, const int line) +{ + if (match_write_sniff(xmit_frame->padapter, addr, 1, value)) { + RTW_INFO("DBG_IO %s:%d IOL_WB(0x%04x, 0x%02x)\n" + , caller, line, addr, value); + } + + return _rtw_IOL_append_WB_cmd(xmit_frame, addr, value); +} + +int dbg_rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, const char *caller, const int line) +{ + if (match_write_sniff(xmit_frame->padapter, addr, 2, value)) { + RTW_INFO("DBG_IO %s:%d IOL_WW(0x%04x, 0x%04x)\n" + , caller, line, addr, value); + } + + return _rtw_IOL_append_WW_cmd(xmit_frame, addr, value); +} + +int dbg_rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, const char *caller, const int line) +{ + if (match_write_sniff(xmit_frame->padapter, addr, 4, value)) { + RTW_INFO("DBG_IO %s:%d IOL_WD(0x%04x, 0x%08x)\n" + , caller, line, addr, value); + } + + return _rtw_IOL_append_WD_cmd(xmit_frame, addr, value); +} +#endif + +int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us) +{ + IOL_CMD cmd = {0x0, IOL_CMD_DELAY_US, 0x0, 0x0}; + + RTW_PUT_BE32((u8 *)&cmd.value, (u32)us); + + /* RTW_INFO("%s %u\n", __FUNCTION__, us); */ + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8); +} + +int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms) +{ + IOL_CMD cmd = {0x0, IOL_CMD_DELAY_MS, 0x0, 0x0}; + + RTW_PUT_BE32((u8 *)&cmd.value, (u32)ms); + + /* RTW_INFO("%s %u\n", __FUNCTION__, ms); */ + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8); +} + +int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame) +{ + IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0}; + + + return rtw_IOL_append_cmds(xmit_frame, (u8 *)&end_cmd, 8); + +} + +int rtw_IOL_exec_cmd_array_sync(PADAPTER adapter, u8 *IOL_cmds, u32 cmd_num, u32 max_wating_ms) +{ + struct xmit_frame *xmit_frame; + + xmit_frame = rtw_IOL_accquire_xmit_frame(adapter); + if (xmit_frame == NULL) + return _FAIL; + + if (rtw_IOL_append_cmds(xmit_frame, IOL_cmds, cmd_num << 3) == _FAIL) + return _FAIL; + + return rtw_IOL_exec_cmds_sync(adapter, xmit_frame, max_wating_ms, 0); +} + +int rtw_IOL_exec_empty_cmds_sync(ADAPTER *adapter, u32 max_wating_ms) +{ + IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0}; + return rtw_IOL_exec_cmd_array_sync(adapter, (u8 *)&end_cmd, 1, max_wating_ms); +} +#endif /* CONFIG_IOL_NEW_GENERATION */ + + + + +#endif /* CONFIG_IOL */ diff --git a/core/rtw_mbo.c b/core/rtw_mbo.c new file mode 100644 index 0000000..44f93ad --- /dev/null +++ b/core/rtw_mbo.c @@ -0,0 +1,803 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include + +#ifdef CONFIG_RTW_MBO + +#ifndef RTW_MBO_DBG +#define RTW_MBO_DBG 0 +#endif +#if RTW_MBO_DBG + #define RTW_MBO_INFO(fmt, arg...) \ + RTW_INFO(fmt, arg) + #define RTW_MBO_DUMP(str, data, len) \ + RTW_INFO_DUMP(str, data, len) +#else + #define RTW_MBO_INFO(fmt, arg...) do {} while (0) + #define RTW_MBO_DUMP(str, data, len) do {} while (0) +#endif + +/* Cellular Data Connectivity field + * 1 : Cellular data connection available + * 2 : Cellular data connection not available + * 3 : Not Cellular data capable + * otherwise : Reserved +*/ +int rtw_mbo_cell_data_conn = 2; +module_param(rtw_mbo_cell_data_conn, int, 0644); + +static u8 wfa_mbo_oui[] = {0x50, 0x6F, 0x9A, 0x16}; + +#define rtw_mbo_get_oui(p) ((u8 *)(p) + 2) + +#define rtw_mbo_get_attr_id(p) ((u8 *)(p)) + +#define rtw_mbo_get_disallow_res(p) ((u8 *)(p) + 3) + +#define rtw_mbo_set_1byte_ie(p, v, l) \ + rtw_set_fixed_ie((p), 1, (v), (l)) + +#define rtw_mbo_set_4byte_ie(p, v, l) \ + rtw_set_fixed_ie((p), 4, (v), (l)) + +#define rtw_mbo_set_nbyte_ie(p, sz, v, l) \ + rtw_set_fixed_ie((p), (sz), (v), (l)) + +#define rtw_mbo_subfield_set(p, offset, val) (*(p + offset) = val) + +#define rtw_mbo_subfields_set(p, offset, buf, len) \ + do { \ + u32 _offset = 0; \ + u8 *_p = p + offset; \ + while(_offset < len) { \ + *(_p + _offset) = *(buf + _offset); \ + _offset++; \ + } \ + } while(0) + +static u8 *rtw_mbo_ie_get(u8 *pie, u32 *plen, u32 limit) +{ + const u8 *p = pie; + u32 tmp, i; + + if (limit <= 1) + return NULL; + + i = 0; + *plen = 0; + while (1) { + if ((*p == _VENDOR_SPECIFIC_IE_) && + (_rtw_memcmp(rtw_mbo_get_oui(p), wfa_mbo_oui, 4))) { + *plen = *(p + 1); + RTW_MBO_DUMP("VENDOR_SPECIFIC_IE MBO: ", p, *(p + 1)); + return (u8 *)p; + } else { + tmp = *(p + 1); + p += (tmp + 2); + i += (tmp + 2); + } + + if (i >= limit) + break; + } + + return NULL; +} + +static u8 *rtw_mbo_attrs_get(u8 *pie, u32 limit, u8 attr_id, u32 *attr_len) +{ + u8 *p = NULL; + u32 offset, plen = 0; + + if ((pie == NULL) || (limit <= 1)) + goto exit; + + if ((p = rtw_mbo_ie_get(pie, &plen, limit)) == NULL) + goto exit; + + /* shift 2 + OUI size and move to attributes content */ + p = p + 2 + sizeof(wfa_mbo_oui); + plen = plen - 4; + RTW_MBO_DUMP("Attributes contents: ", p, plen); + + if ((p = rtw_get_ie(p, attr_id, attr_len, plen)) == NULL) + goto exit; + + RTW_MBO_INFO("%s : id=%u(len=%u)\n", __func__, attr_id, *attr_len); + RTW_MBO_DUMP("contents : ", p, *attr_len); + +exit: + return p; + +} + +static u32 rtw_mbo_attr_sz_get( + _adapter *padapter, u8 id) +{ + u32 len = 0; + + switch (id) { + case RTW_MBO_ATTR_NPREF_CH_RPT_ID: + { + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + struct npref_ch* pch; + u32 i, attr_len, offset; + + for (i=0; i < prpt->nm_of_rpt; i++) { + pch = &prpt->ch_rpt[i]; + /*attr_len = ch list + op class + preference + reason */ + attr_len = pch->nm_of_ch + 3; + /* offset = id + len field + attr_len */ + offset = attr_len + 2; + len += offset; + } + } + break; + case RTW_MBO_ATTR_CELL_DATA_CAP_ID: + case RTW_MBO_ATTR_TRANS_REJ_ID: + len = 3; + break; + default: + break; + } + + return len; +} + +static void rtw_mbo_build_mbo_ie_hdr( + u8 **pframe, struct pkt_attrib *pattrib, u8 payload_len) +{ + u8 eid = RTW_MBO_EID; + u8 len = payload_len + 4; + + *pframe = rtw_mbo_set_1byte_ie(*pframe, &eid, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &len, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_4byte_ie(*pframe, wfa_mbo_oui, &(pattrib->pktlen)); +} + +void rtw_mbo_build_cell_data_cap_attr( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u8 attr_id = RTW_MBO_ATTR_CELL_DATA_CAP_ID; + u8 attr_len = 1; + u8 cell_data_con = rtw_mbo_cell_data_conn; + + /* used Cellular Data Capabilities from supplicant */ + if (!rtw_mbo_wifi_logo_test(padapter) && + pmlmepriv->pcell_data_cap_ie && pmlmepriv->cell_data_cap_len == 1) { + cell_data_con = *pmlmepriv->pcell_data_cap_ie; + RTW_MBO_INFO("%s : used Cellular Data Capabilities(%u) from supplicant!\n", + __func__, *pmlmepriv->pcell_data_cap_ie); + } + + *pframe = rtw_mbo_set_1byte_ie(*pframe, &attr_id, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &attr_len, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &cell_data_con, &(pattrib->pktlen)); +} + +static void rtw_mbo_update_cell_data_cap( + _adapter *padapter, u8 *pie, u32 ie_len) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u8 *mbo_attr; + u32 mbo_attrlen; + + if ((pie == NULL) || (ie_len == 0)) + return; + + mbo_attr = rtw_mbo_attrs_get(pie, ie_len, + RTW_MBO_ATTR_CELL_DATA_CAP_ID, &mbo_attrlen); + + if ((mbo_attr == NULL) || (mbo_attrlen == 0) ) { + RTW_INFO("MBO : Cellular Data Capabilities not found!\n"); + return; + } + + rtw_buf_update(&pmlmepriv->pcell_data_cap_ie, + &pmlmepriv->cell_data_cap_len, (mbo_attr + 2), mbo_attrlen); + RTW_MBO_DUMP("rtw_mbo_update_cell_data_cap : ", + pmlmepriv->pcell_data_cap_ie, pmlmepriv->cell_data_cap_len); +} + +void rtw_mbo_update_ie_data( + _adapter *padapter, u8 *pie, u32 ie_len) +{ + rtw_mbo_update_cell_data_cap(padapter, pie, ie_len); +} + +static u8 rtw_mbo_current_op_class_get(_adapter *padapter) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct p2p_channels *pch_list = &(prfctl->channel_list); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct p2p_reg_class *preg_class; + int class_idx, ch_idx; + u8 cur_op_class = 0; + + for(class_idx =0; class_idx < pch_list->reg_classes; class_idx++) { + preg_class = &pch_list->reg_class[class_idx]; + for (ch_idx = 0; ch_idx <= preg_class->channels; ch_idx++) { + if (pmlmeext->cur_channel == preg_class->channel[ch_idx]) { + cur_op_class = preg_class->reg_class; + RTW_MBO_INFO("%s : current ch : %d, op class : %d\n", + __func__, pmlmeext->cur_channel, cur_op_class); + break; + } + } + } + + return cur_op_class; +} + +static void rtw_mbo_supp_op_classes_get(_adapter *padapter, u8 *pclasses) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct p2p_channels *pch_list = &(prfctl->channel_list); + int class_idx; + + if (pclasses == NULL) + return; + + RTW_MBO_INFO("%s : support op class \n", __func__); + for(class_idx = 0; class_idx < pch_list->reg_classes; class_idx++) { + *(pclasses + class_idx) = pch_list->reg_class[class_idx].reg_class; + RTW_MBO_INFO("%u ,", *(pclasses + class_idx)); + } + + RTW_MBO_INFO("%s : \n", __func__); +} + +void rtw_mbo_build_supp_op_class_elem( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + u8 payload[32] = {0}; + u8 delimiter_130 = 130; /*0x82*/ + u8 reg_class_nm, len; + + if ((reg_class_nm = prfctl->channel_list.reg_classes) == 0) + return; + + payload[0] = rtw_mbo_current_op_class_get(padapter); + rtw_mbo_supp_op_classes_get(padapter, &payload[1]); + + /* IEEE 802.11 Std Current Operating Class Extension Sequence */ + payload[reg_class_nm + 1] = delimiter_130; + payload[reg_class_nm + 2] = 0x00; + + RTW_MBO_DUMP("op class :", payload, reg_class_nm); + + /* Current Operating Class field + Operating Class field + + OneHundredAndThirty Delimiter field */ + len = reg_class_nm + 3; + *pframe = rtw_set_ie(*pframe, EID_SupRegulatory, len , + payload, &(pattrib->pktlen)); +} + +static u8 rtw_mbo_construct_npref_ch_rpt_attr( + _adapter *padapter, u8 *pbuf, u32 buf_len, u32 *plen) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + struct npref_ch* pch; + u32 attr_len, offset; + int i; + u8 *p = pbuf; + + if (prpt->nm_of_rpt == 0) { + *plen = 0; + return _FALSE; + } + + for (i=0; i < prpt->nm_of_rpt; i++) { + pch = &prpt->ch_rpt[i]; + /* attr_len = ch list + op class + preference + reason */ + attr_len = pch->nm_of_ch + 3; + /* offset = id + len field + attr_len */ + offset = attr_len + 2; + rtw_mbo_subfield_set(p, 0, RTW_MBO_ATTR_NPREF_CH_RPT_ID); + rtw_mbo_subfield_set(p, 1, attr_len); + rtw_mbo_subfield_set(p, 2, pch->op_class); + rtw_mbo_subfields_set(p, 3, pch->chs, pch->nm_of_ch); + rtw_mbo_subfield_set(p, (offset - 2), pch->preference); + rtw_mbo_subfield_set(p, (offset - 1), pch->reason); + p += offset; + *plen += offset; + + if (*plen >= buf_len) { + RTW_ERR("MBO : construct non-preferred channel report fail!\n"); + return _FALSE; + } + } + + return _TRUE; +} + +void rtw_mbo_build_npref_ch_rpt_attr( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + u32 tmp_sz = 0, body_len = 0; + u8 *ptmp; + + tmp_sz = prpt->nm_of_rpt * sizeof(struct npref_ch); + ptmp = rtw_zmalloc(tmp_sz); + if (ptmp == NULL) + return; + + if (rtw_mbo_construct_npref_ch_rpt_attr(padapter, ptmp, tmp_sz, &body_len) == _FALSE) { + rtw_mfree(ptmp, tmp_sz); + return; + } + + RTW_MBO_DUMP("Non-preferred Channel Report :", ptmp, body_len); + *pframe = rtw_mbo_set_nbyte_ie(*pframe, body_len, ptmp, &(pattrib->pktlen)); + + rtw_mfree(ptmp, tmp_sz); +} + +void rtw_mbo_build_trans_reject_reason_attr( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib, u8 *pres) +{ + u8 attr_id = RTW_MBO_ATTR_TRANS_REJ_ID; + u8 attr_len = 1; + u32 len = 0; + + len = rtw_mbo_attr_sz_get(padapter, RTW_MBO_ATTR_TRANS_REJ_ID); + if ((len == 0) || (len > 3)) { + RTW_ERR("MBO : build Transition Rejection Reason attribute fail(len=%u)\n", len); + return; + } + + rtw_mbo_build_mbo_ie_hdr(pframe, pattrib, len); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &attr_id, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &attr_len, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, pres, &(pattrib->pktlen)); +} + +u8 rtw_mbo_disallowed_network(struct wlan_network *pnetwork) +{ + u8 *p, *attr_id, *res; + u32 attr_len = 0; + u8 disallow = _FALSE; + + if (pnetwork == NULL) + goto exit; + + p = rtw_mbo_attrs_get(pnetwork->network.IEs, + pnetwork->network.IELength, + RTW_MBO_ATTR_ASSOC_DISABLED_ID, + &attr_len); + + if (p == NULL) { + RTW_MBO_INFO("%s :Assoc Disallowed attribute not found!\n",__func__); + goto exit; + } + + RTW_MBO_DUMP("Association Disallowed attribute :",p , attr_len + 2); + RTW_INFO("MBO : block "MAC_FMT" assoc disallowed reason %d\n", + MAC_ARG(pnetwork->network.MacAddress), *(rtw_mbo_get_disallow_res(p))); + + disallow = _TRUE; +exit: + return disallow; +} + +void rtw_mbo_build_exented_cap( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + u8 content[8] = { 0 }; + + rtw_wnm_set_ext_cap_btm(content, 1); + rtw_mbo_set_ext_cap_internw(content, 1); + *pframe = rtw_set_ie(*pframe, + EID_EXTCapability, + 8, + content, + &(pattrib->pktlen)); +} + +static void rtw_mbo_non_pref_chans_dump(struct npref_ch* pch) +{ + int i; + u8 buf[128] = {0}; + + for (i=0; i < pch->nm_of_ch; i++) + rtw_sprintf(buf, 128, "%s,%d", buf, pch->chs[i]); + + RTW_MBO_INFO("%s : op_class=%01x, ch=%s, preference=%d, reason=%d\n", + __func__, pch->op_class, buf, pch->preference, pch->reason); +} + +static u8 rtw_mbo_non_pref_chan_exist(struct npref_ch* pch, u8 ch) +{ + u32 i; + u8 found = _FALSE; + + for (i=0; i < pch->nm_of_ch; i++) { + if (pch->chs[i] == ch) { + found = _TRUE; + break; + } + } + + return found; +} + +static struct npref_ch* rtw_mbo_non_pref_chan_get( + _adapter *padapter, u8 op_class, u8 prefe, u8 res) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + struct npref_ch* pch = NULL; + int i; + + if (prpt->nm_of_rpt == 0) + return pch; + + for (i=0; i < prpt->nm_of_rpt; i++) { + if ((prpt->ch_rpt[i].op_class == op_class) && + (prpt->ch_rpt[i].preference == prefe) && + (prpt->ch_rpt[i].reason == res)) { + pch = &prpt->ch_rpt[i]; + break; + } + } + + return pch; +} + +static void rtw_mbo_non_pref_chan_set( + struct npref_ch* pch, u8 op_class, u8 ch, u8 prefe, u8 res, u8 update) +{ + u32 offset = pch->nm_of_ch; + + if (update) { + if (rtw_mbo_non_pref_chan_exist(pch, ch) == _FALSE) { + pch->chs[offset] = ch; + pch->nm_of_ch++; + } + } else { + pch->op_class = op_class; + pch->chs[0] = ch; + pch->preference = prefe; + pch->reason = res; + pch->nm_of_ch = 1; + } +} + +static void rtw_mbo_non_pref_chans_update( + _adapter *padapter, u8 op_class, u8 ch, u8 prefe, u8 res) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *pch_rpt = &(prfctl->ch_rtp); + struct npref_ch* pch; + + if (pch_rpt->nm_of_rpt >= RTW_MBO_MAX_CH_RPT_NUM) { + RTW_ERR("MBO : %d non_pref_chan entries supported!", + RTW_MBO_MAX_CH_RPT_NUM); + return; + } + + if (pch_rpt->nm_of_rpt == 0) { + pch = &pch_rpt->ch_rpt[0]; + rtw_mbo_non_pref_chan_set(pch, op_class, ch, prefe, res, _FALSE); + pch_rpt->nm_of_rpt = 1; + return; + } + + pch = rtw_mbo_non_pref_chan_get(padapter, op_class, prefe, res); + if (pch == NULL) { + pch = &pch_rpt->ch_rpt[pch_rpt->nm_of_rpt]; + rtw_mbo_non_pref_chan_set(pch, op_class, ch, prefe, res, _FALSE); + pch_rpt->nm_of_rpt++; + } else + rtw_mbo_non_pref_chan_set(pch, op_class, ch, prefe, res, _TRUE); + + rtw_mbo_non_pref_chans_dump(pch); +} + +static void rtw_mbo_non_pref_chans_set( + _adapter *padapter, char *param, ssize_t sz) +{ + char *pnext; + u32 op_class, ch, prefe, res; + int i = 0; + + do { + pnext = strsep(¶m, " "); + if (pnext == NULL) + break; + + sscanf(pnext, "%d:%d:%d:%d", &op_class, &ch, &prefe, &res); + rtw_mbo_non_pref_chans_update(padapter, op_class, ch, prefe, res); + + if ((i++) > 10) { + RTW_ERR("MBO : overflow %d \n", i); + break; + } + + } while(param != '\0'); + +} + +static void rtw_mbo_non_pref_chans_del( + _adapter *padapter, char *param, ssize_t sz) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + + RTW_INFO("%s : delete non_pref_chan %s\n", __func__, param); + _rtw_memset(prpt, 0, sizeof(struct npref_ch_rtp)); +} + +ssize_t rtw_mbo_proc_non_pref_chans_set( + struct file *pfile, const char __user *buffer, + size_t count, loff_t *pos, void *pdata) +{ + struct net_device *dev = pdata; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 tmp[128] = {0}; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) { + rtw_warn_on(1); + return -EFAULT; + } + + if (buffer && !copy_from_user(tmp, buffer, count)) { + if (strncmp(tmp, "add", 3) == 0) + rtw_mbo_non_pref_chans_set(padapter, &tmp[4], (count - 4)); + else if (strncmp(tmp, "delete", 6) == 0) + rtw_mbo_non_pref_chans_del(padapter, &tmp[7], (count - 7)); + else { + RTW_ERR("MBO : Invalid format : echo [add|delete] :::\n"); + return -EFAULT; + } + } + +#ifdef CONFIG_RTW_WNM + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) && + check_fwstate(pmlmepriv, WIFI_STATION_STATE)) + rtw_wnm_issue_action(padapter, RTW_WLAN_ACTION_WNM_NOTIF_REQ, 0, 0); +#endif + + return count; +} + +int rtw_mbo_proc_non_pref_chans_get( + struct seq_file *m, void *v) +{ + struct net_device *dev = m->private; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + struct npref_ch* pch; + int i,j; + u8 buf[32] = {0}; + + RTW_PRINT_SEL(m, "op_class ch preference reason \n"); + RTW_PRINT_SEL(m, "=======================================================\n"); + + + if (prpt->nm_of_rpt == 0) { + RTW_PRINT_SEL(m, " empty table \n"); + return 0; + } + + for (i=0; i < prpt->nm_of_rpt; i++) { + pch = &prpt->ch_rpt[i]; + buf[0]='\0'; + for (j=0; j < pch->nm_of_ch; j++) { + if (j == 0) + rtw_sprintf(buf, 32, "%02u", pch->chs[j]); + else + rtw_sprintf(buf, 32, "%s,%02u", buf, pch->chs[j]); + } + + RTW_PRINT_SEL(m, " %04u %20s %02u %02u\n", + pch->op_class, buf, pch->preference, pch->reason); + } + + return 0; +} + +ssize_t rtw_mbo_proc_cell_data_set( + struct file *pfile, const char __user *buffer, + size_t count, loff_t *pos, void *pdata) +{ + struct net_device *dev = pdata; + _adapter *padapter = (_adapter *)rtw_netdev_priv(dev); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + int mbo_cell_data = 0; + u8 tmp[8] = {0}; + + if (count < 1) + return -EFAULT; + + if (count > sizeof(tmp)) + return -EFAULT; + + if (buffer && !copy_from_user(tmp, buffer, count)) { + int num = sscanf(tmp, "%d", &mbo_cell_data); + if (num == 1) { + rtw_mbo_cell_data_conn = mbo_cell_data; + #ifdef CONFIG_RTW_WNM + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) && + check_fwstate(pmlmepriv, WIFI_STATION_STATE)) + rtw_wnm_issue_action(padapter, RTW_WLAN_ACTION_WNM_NOTIF_REQ, 0, 0); + #endif + } + } + + + return count; +} + +int rtw_mbo_proc_cell_data_get( + struct seq_file *m, void *v) +{ +#if 0 + struct net_device *dev = m->private; + _adapter *adapter = (_adapter *)rtw_netdev_priv(dev); +#endif + + RTW_PRINT_SEL(m, "Cellular Data Connectivity : %d\n", rtw_mbo_cell_data_conn); + return 0; +} + +static void rtw_mbo_non_pref_chan_subelem_parsing( + _adapter *padapter, u8 *subelem, size_t subelem_len) +{ + u8 *pnon_pref_chans; + u32 non_pref_chan_offset, op_subelem_len; + u32 oui_offset = 3; + /* wpa_supplicant don't apped OUI Type */ + u32 oui_type_offset = 0; + + RTW_MBO_DUMP("Non-preferred Channel subelem : ", subelem , subelem_len); + + /* Subelem : + Vendor Specific | Length | WFA OUI | OUI Type | MBO Attributes */ + non_pref_chan_offset = 2 + oui_offset + oui_type_offset; + pnon_pref_chans = subelem + non_pref_chan_offset; + op_subelem_len = subelem_len - non_pref_chan_offset; + + /* wpa_supplicant don't indicate non_pref_chan length, + so we cannot get how many non_pref_chan in a wnm notification */ + RTW_MBO_DUMP("Non-preferred Channel : ", pnon_pref_chans, op_subelem_len); +} + +void rtw_mbo_wnm_notification_parsing( + _adapter *padapter, const u8 *pdata, size_t data_len) +{ + u8 *paction; + u8 category, action, dialog, type; + u32 len; + + if ((pdata == NULL) || (data_len == 0)) + return; + + RTW_MBO_DUMP("WNM notification data : ", pdata, data_len); + paction = (u8 *)pdata + sizeof(struct rtw_ieee80211_hdr_3addr); + category = paction[0]; + action = paction[1]; + dialog = paction[2]; + type = paction[3]; + + if ((action == RTW_WLAN_ACTION_WNM_NOTIF_REQ) && + (type == WLAN_EID_VENDOR_SPECIFIC)) { + rtw_mbo_non_pref_chan_subelem_parsing(padapter, &paction[4], + (data_len - sizeof(struct rtw_ieee80211_hdr_3addr))); + } + +} + +void rtw_mbo_build_wnm_notification( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + struct rf_ctl_t *prfctl = adapter_to_rfctl(padapter); + struct npref_ch_rtp *prpt = &(prfctl->ch_rtp); + struct npref_ch* pch; + u8 subelem_id = WLAN_EID_VENDOR_SPECIFIC; + u8 non_pref_ch_oui[] = {0x50, 0x6F, 0x9A, 0x2}; + u8 cell_data_cap_oui[] = {0x50, 0x6F, 0x9A, 0x3}; + u8 cell_data_con = rtw_mbo_cell_data_conn; + u8 len, cell_data_con_len = 0, *pcont = *pframe; + int i; + + if (rtw_mbo_cell_data_conn > 0) { + len = 0x5; + *pframe = rtw_mbo_set_1byte_ie(*pframe, &subelem_id, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &len, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_4byte_ie(*pframe, cell_data_cap_oui, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &cell_data_con, &(pattrib->pktlen)); + RTW_MBO_INFO("%s : Cellular Data Capabilities subelemen\n", __func__); + RTW_MBO_DUMP(":", pcont, len + 2); + pcont += len + 2 ; + } + + if (prpt->nm_of_rpt == 0) { + len = 0x4; + *pframe = rtw_mbo_set_1byte_ie(*pframe, &subelem_id, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &len, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_4byte_ie(*pframe, non_pref_ch_oui, &(pattrib->pktlen)); + RTW_MBO_INFO("%s :Non-preferred Channel Report subelement without data\n", __func__); + return; + } + + for (i=0; i < prpt->nm_of_rpt; i++) { + pch = &prpt->ch_rpt[i]; + /* OUI(3B) + OUT-type(1B) + op-class(1B) + ch list(nB) + + Preference(1B) + reason(1B) */ + len = pch->nm_of_ch + 7; + *pframe = rtw_mbo_set_1byte_ie(*pframe, &subelem_id, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &len, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_4byte_ie(*pframe, non_pref_ch_oui, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &pch->op_class, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_nbyte_ie(*pframe, pch->nm_of_ch, pch->chs, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &pch->preference, &(pattrib->pktlen)); + *pframe = rtw_mbo_set_1byte_ie(*pframe, &pch->reason, &(pattrib->pktlen)); + RTW_MBO_INFO("%s :Non-preferred Channel Report subelement\n", __func__); + RTW_MBO_DUMP(":", pcont, len); + pcont = *pframe; + } +} + +void rtw_mbo_build_probe_req_ies( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + u32 len =0; + + rtw_mbo_build_exented_cap(padapter, pframe, pattrib); + + len = rtw_mbo_attr_sz_get(padapter, RTW_MBO_ATTR_CELL_DATA_CAP_ID); + if ((len == 0) || (len > 3)) { + RTW_ERR("MBO : build Cellular Data Capabilities attribute fail(len=%u)\n", len); + return; + } + + rtw_mbo_build_mbo_ie_hdr(pframe, pattrib, len); + rtw_mbo_build_cell_data_cap_attr(padapter, pframe, pattrib); +} + +void rtw_mbo_build_assoc_req_ies( + _adapter *padapter, u8 **pframe, struct pkt_attrib *pattrib) +{ + u32 len = 0; + + rtw_mbo_build_supp_op_class_elem(padapter, pframe, pattrib); + + len += rtw_mbo_attr_sz_get(padapter, RTW_MBO_ATTR_CELL_DATA_CAP_ID); + len += rtw_mbo_attr_sz_get(padapter, RTW_MBO_ATTR_NPREF_CH_RPT_ID); + if ((len == 0)|| (len < 3)) { + RTW_ERR("MBO : build assoc MBO IE fail(len=%u)\n", len); + return; + } + + rtw_mbo_build_mbo_ie_hdr(pframe, pattrib, len); + rtw_mbo_build_cell_data_cap_attr(padapter, pframe, pattrib); + rtw_mbo_build_npref_ch_rpt_attr(padapter, pframe, pattrib); +} + +#endif /* CONFIG_RTW_MBO */ diff --git a/core/rtw_mem.c b/core/rtw_mem.c new file mode 100644 index 0000000..d42a4fb --- /dev/null +++ b/core/rtw_mem.c @@ -0,0 +1,171 @@ +/****************************************************************************** + * + * Copyright(c) 2016 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("Realtek Wireless Lan Driver"); +MODULE_AUTHOR("Realtek Semiconductor Corp."); +MODULE_VERSION("DRIVERVERSION"); + +/* for MAX_RECVBUF_SZ */ +#if defined(CONFIG_RTL8188E) +#include +#elif defined(CONFIG_RTL8188F) +#include +#elif defined(CONFIG_RTL8188GTV) +#include +#elif defined(CONFIG_RTL8710B) +#include +#elif defined(CONFIG_RTL8192E) +#include +#elif defined(CONFIG_RTL8192F) +#include +#elif defined(CONFIG_RTL8723B) +#include +#elif defined(CONFIG_RTL8703B) +#include +#elif defined(CONFIG_RTL8723D) +#include +#elif defined(CONFIG_RTL8812A) || defined(CONFIG_RTL8821A) +#include +#elif defined(CONFIG_RTL8822B) +#include +#elif defined(CONFIG_RTL8822C) +#include +#elif defined(CONFIG_RTL8814A) +#include +#elif defined(CONFIG_RTL8814B) +#include +#endif + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) +#define MAX_RTKM_RECVBUF_SZ MAX_RECVBUF_SZ +#define MAX_RTKM_NR_PREALLOC_RECV_SKB NR_RECVBUFF +#else /* !CONFIG_SDIO_HCI */ +#ifdef CONFIG_PLATFORM_MSTAR_HIGH + #define MAX_RTKM_RECVBUF_SZ (31744) /* 31k */ +#else + #define MAX_RTKM_RECVBUF_SZ (15360) /* 15k */ +#endif /* CONFIG_PLATFORM_MSTAR_HIGH */ +#define MAX_RTKM_NR_PREALLOC_RECV_SKB 16 +#endif /* !CONFIG_SDIO_HCI */ + +struct sk_buff_head rtk_skb_mem_q; +struct u8 *rtk_buf_mem[NR_RECVBUFF]; + +struct u8 *rtw_get_buf_premem(int index) +{ + printk("%s, rtk_buf_mem index : %d\n", __func__, index); + return rtk_buf_mem[index]; +} + +u16 rtw_rtkm_get_buff_size(void) +{ + return MAX_RTKM_RECVBUF_SZ; +} +EXPORT_SYMBOL(rtw_rtkm_get_buff_size); + +u8 rtw_rtkm_get_nr_recv_skb(void) +{ + return MAX_RTKM_NR_PREALLOC_RECV_SKB; +} +EXPORT_SYMBOL(rtw_rtkm_get_nr_recv_skb); + +struct sk_buff *rtw_alloc_skb_premem(u16 in_size) +{ + struct sk_buff *skb = NULL; + + if (in_size > MAX_RTKM_RECVBUF_SZ) { + pr_info("warning %s: driver buffer size(%d) > rtkm buffer size(%d)\n", __func__, in_size, MAX_RTKM_RECVBUF_SZ); + WARN_ON(1); + return skb; + } + + skb = skb_dequeue(&rtk_skb_mem_q); + + printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q)); + + return skb; +} +EXPORT_SYMBOL(rtw_alloc_skb_premem); + +int rtw_free_skb_premem(struct sk_buff *pskb) +{ + if (!pskb) + return -1; + + if (skb_queue_len(&rtk_skb_mem_q) >= MAX_RTKM_NR_PREALLOC_RECV_SKB) + return -1; + + skb_queue_tail(&rtk_skb_mem_q, pskb); + + printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q)); + + return 0; +} +EXPORT_SYMBOL(rtw_free_skb_premem); + +static int __init rtw_mem_init(void) +{ + int i; + SIZE_PTR tmpaddr = 0; + SIZE_PTR alignment = 0; + struct sk_buff *pskb = NULL; + + printk("%s\n", __func__); + pr_info("MAX_RTKM_NR_PREALLOC_RECV_SKB: %d\n", MAX_RTKM_NR_PREALLOC_RECV_SKB); + pr_info("MAX_RTKM_RECVBUF_SZ: %d\n", MAX_RTKM_RECVBUF_SZ); + +#ifdef CONFIG_USE_USB_BUFFER_ALLOC_RX + for (i = 0; i < NR_RECVBUFF; i++) + rtk_buf_mem[i] = usb_buffer_alloc(dev, size, (in_interrupt() ? GFP_ATOMIC : GFP_KERNEL), dma); +#endif /* CONFIG_USE_USB_BUFFER_ALLOC_RX */ + + skb_queue_head_init(&rtk_skb_mem_q); + + for (i = 0; i < MAX_RTKM_NR_PREALLOC_RECV_SKB; i++) { + pskb = __dev_alloc_skb(MAX_RTKM_RECVBUF_SZ + RECVBUFF_ALIGN_SZ, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL); + if (pskb) { + tmpaddr = (SIZE_PTR)pskb->data; + alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1); + skb_reserve(pskb, (RECVBUFF_ALIGN_SZ - alignment)); + + skb_queue_tail(&rtk_skb_mem_q, pskb); + } else + printk("%s, alloc skb memory fail!\n", __func__); + + pskb = NULL; + } + + printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q)); + + return 0; + +} + +static void __exit rtw_mem_exit(void) +{ + if (skb_queue_len(&rtk_skb_mem_q)) + printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q)); + + skb_queue_purge(&rtk_skb_mem_q); + + printk("%s\n", __func__); +} + +module_init(rtw_mem_init); +module_exit(rtw_mem_exit); diff --git a/core/rtw_mi.c b/core/rtw_mi.c new file mode 100644 index 0000000..099cd36 --- /dev/null +++ b/core/rtw_mi.c @@ -0,0 +1,1546 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_MI_C_ + +#include +#include + +void rtw_mi_update_union_chan_inf(_adapter *adapter, u8 ch, u8 offset , u8 bw) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + + if (!ch) { + dvobj->union_ch_bak = dvobj->union_ch; + dvobj->union_bw_bak = dvobj->union_bw; + dvobj->union_offset_bak = dvobj->union_offset; + } + dvobj->union_ch = ch; + dvobj->union_bw = bw; + dvobj->union_offset = offset; +} + +#ifdef DBG_IFACE_STATUS +#ifdef CONFIG_P2P +static u8 _rtw_mi_p2p_listen_scan_chk(_adapter *adapter) +{ + int i; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + u8 p2p_listen_scan_state = _FALSE; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_LISTEN) || + rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_SCAN)) { + p2p_listen_scan_state = _TRUE; + break; + } + } + return p2p_listen_scan_state; +} +#endif +#endif + +u8 rtw_mi_stayin_union_ch_chk(_adapter *adapter) +{ + u8 rst = _TRUE; + u8 u_ch, u_bw, u_offset; + u8 o_ch, o_bw, o_offset; + + u_ch = rtw_mi_get_union_chan(adapter); + u_bw = rtw_mi_get_union_bw(adapter); + u_offset = rtw_mi_get_union_offset(adapter); + + o_ch = rtw_get_oper_ch(adapter); + o_bw = rtw_get_oper_bw(adapter); + o_offset = rtw_get_oper_choffset(adapter); + + if ((u_ch != o_ch) || (u_bw != o_bw) || (u_offset != o_offset)) + rst = _FALSE; + + #ifdef DBG_IFACE_STATUS + if (rst == _FALSE) { + RTW_ERR("%s Not stay in union channel\n", __func__); + if (GET_HAL_DATA(adapter)->bScanInProcess == _TRUE) + RTW_ERR("ScanInProcess\n"); + #ifdef CONFIG_P2P + if (_rtw_mi_p2p_listen_scan_chk(adapter)) + RTW_ERR("P2P in listen or scan state\n"); + #endif + RTW_ERR("union ch, bw, offset: %u,%u,%u\n", u_ch, u_bw, u_offset); + RTW_ERR("oper ch, bw, offset: %u,%u,%u\n", o_ch, o_bw, o_offset); + RTW_ERR("=========================\n"); + } + #endif + return rst; +} + +u8 rtw_mi_stayin_union_band_chk(_adapter *adapter) +{ + u8 rst = _TRUE; + u8 u_ch, o_ch; + u8 u_band, o_band; + + u_ch = rtw_mi_get_union_chan(adapter); + o_ch = rtw_get_oper_ch(adapter); + u_band = (u_ch > 14) ? BAND_ON_5G : BAND_ON_2_4G; + o_band = (o_ch > 14) ? BAND_ON_5G : BAND_ON_2_4G; + + if (u_ch != o_ch) + if(u_band != o_band) + rst = _FALSE; + + #ifdef DBG_IFACE_STATUS + if (rst == _FALSE) + RTW_ERR("%s Not stay in union band\n", __func__); + #endif + + return rst; +} + +/* Find union about ch, bw, ch_offset of all linked/linking interfaces */ +int rtw_mi_get_ch_setting_union_by_ifbmp(struct dvobj_priv *dvobj, u8 ifbmp, u8 *ch, u8 *bw, u8 *offset) +{ + _adapter *iface; + struct mlme_ext_priv *mlmeext; + int i; + u8 ch_ret = 0; + u8 bw_ret = CHANNEL_WIDTH_20; + u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + int num = 0; + + if (ch) + *ch = 0; + if (bw) + *bw = CHANNEL_WIDTH_20; + if (offset) + *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface || !(ifbmp & BIT(iface->iface_id))) + continue; + + mlmeext = &iface->mlmeextpriv; + + if (!check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE | WIFI_UNDER_LINKING)) + continue; + + if (check_fwstate(&iface->mlmepriv, WIFI_OP_CH_SWITCHING)) + continue; + + if (num == 0) { + ch_ret = mlmeext->cur_channel; + bw_ret = mlmeext->cur_bwmode; + offset_ret = mlmeext->cur_ch_offset; + num++; + continue; + } + + if (ch_ret != mlmeext->cur_channel) { + num = 0; + break; + } + + if (bw_ret < mlmeext->cur_bwmode) { + bw_ret = mlmeext->cur_bwmode; + offset_ret = mlmeext->cur_ch_offset; + } else if (bw_ret == mlmeext->cur_bwmode && offset_ret != mlmeext->cur_ch_offset) { + num = 0; + break; + } + + num++; + } + + if (num) { + if (ch) + *ch = ch_ret; + if (bw) + *bw = bw_ret; + if (offset) + *offset = offset_ret; + } + + return num; +} + +inline int rtw_mi_get_ch_setting_union(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset) +{ + return rtw_mi_get_ch_setting_union_by_ifbmp(adapter_to_dvobj(adapter), 0xFF, ch, bw, offset); +} + +inline int rtw_mi_get_ch_setting_union_no_self(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset) +{ + return rtw_mi_get_ch_setting_union_by_ifbmp(adapter_to_dvobj(adapter), 0xFF & ~BIT(adapter->iface_id), ch, bw, offset); +} + +void rtw_mi_status_by_ifbmp(struct dvobj_priv *dvobj, u8 ifbmp, struct mi_state *mstate) +{ + _adapter *iface; + int i; + + _rtw_memset(mstate, 0, sizeof(struct mi_state)); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface || !(ifbmp & BIT(iface->iface_id))) + continue; + + if (check_fwstate(&iface->mlmepriv, WIFI_STATION_STATE) == _TRUE) { + MSTATE_STA_NUM(mstate)++; + if (check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE) == _TRUE) { + MSTATE_STA_LD_NUM(mstate)++; + + #ifdef CONFIG_TDLS + if (iface->tdlsinfo.link_established == _TRUE) + MSTATE_TDLS_LD_NUM(mstate)++; + #endif + #ifdef CONFIG_P2P + if (MLME_IS_GC(iface)) + MSTATE_P2P_GC_NUM(mstate)++; + #endif + } + if (check_fwstate(&iface->mlmepriv, WIFI_UNDER_LINKING) == _TRUE) + MSTATE_STA_LG_NUM(mstate)++; + +#ifdef CONFIG_AP_MODE + } else if (check_fwstate(&iface->mlmepriv, WIFI_AP_STATE) == _TRUE ) { + if (check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE) == _TRUE) { + MSTATE_AP_NUM(mstate)++; + if (iface->stapriv.asoc_sta_count > 2) + MSTATE_AP_LD_NUM(mstate)++; + #ifdef CONFIG_P2P + if (MLME_IS_GO(iface)) + MSTATE_P2P_GO_NUM(mstate)++; + #endif + } else + MSTATE_AP_STARTING_NUM(mstate)++; +#endif + + } else if (check_fwstate(&iface->mlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE) == _TRUE + && check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE) == _TRUE + ) { + MSTATE_ADHOC_NUM(mstate)++; + if (iface->stapriv.asoc_sta_count > 2) + MSTATE_ADHOC_LD_NUM(mstate)++; + +#ifdef CONFIG_RTW_MESH + } else if (check_fwstate(&iface->mlmepriv, WIFI_MESH_STATE) == _TRUE + && check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE) == _TRUE + ) { + MSTATE_MESH_NUM(mstate)++; + if (iface->stapriv.asoc_sta_count > 2) + MSTATE_MESH_LD_NUM(mstate)++; +#endif + + } + + if (check_fwstate(&iface->mlmepriv, WIFI_UNDER_WPS) == _TRUE) + MSTATE_WPS_NUM(mstate)++; + + if (check_fwstate(&iface->mlmepriv, WIFI_UNDER_SURVEY) == _TRUE) { + MSTATE_SCAN_NUM(mstate)++; + + if (mlmeext_scan_state(&iface->mlmeextpriv) != SCAN_DISABLE + && mlmeext_scan_state(&iface->mlmeextpriv) != SCAN_BACK_OP) + MSTATE_SCAN_ENTER_NUM(mstate)++; + } + +#ifdef CONFIG_IOCTL_CFG80211 + if (rtw_cfg80211_get_is_mgmt_tx(iface)) + MSTATE_MGMT_TX_NUM(mstate)++; + + if (rtw_cfg80211_get_is_roch(iface) == _TRUE) + MSTATE_ROCH_NUM(mstate)++; + +#endif /* CONFIG_IOCTL_CFG80211 */ +#ifdef CONFIG_P2P + if (MLME_IS_PD(iface)) + MSTATE_P2P_DV_NUM(mstate)++; +#endif + } +} + +inline void rtw_mi_status(_adapter *adapter, struct mi_state *mstate) +{ + return rtw_mi_status_by_ifbmp(adapter_to_dvobj(adapter), 0xFF, mstate); +} + +inline void rtw_mi_status_no_self(_adapter *adapter, struct mi_state *mstate) +{ + return rtw_mi_status_by_ifbmp(adapter_to_dvobj(adapter), 0xFF & ~BIT(adapter->iface_id), mstate); +} + +inline void rtw_mi_status_no_others(_adapter *adapter, struct mi_state *mstate) +{ + return rtw_mi_status_by_ifbmp(adapter_to_dvobj(adapter), BIT(adapter->iface_id), mstate); +} + +inline void rtw_mi_status_merge(struct mi_state *d, struct mi_state *a) +{ + d->sta_num += a->sta_num; + d->ld_sta_num += a->ld_sta_num; + d->lg_sta_num += a->lg_sta_num; +#ifdef CONFIG_TDLS + d->ld_tdls_num += a->ld_tdls_num; +#endif +#ifdef CONFIG_AP_MODE + d->ap_num += a->ap_num; + d->ld_ap_num += a->ld_ap_num; +#endif + d->adhoc_num += a->adhoc_num; + d->ld_adhoc_num += a->ld_adhoc_num; +#ifdef CONFIG_RTW_MESH + d->mesh_num += a->mesh_num; + d->ld_mesh_num += a->ld_mesh_num; +#endif + d->scan_num += a->scan_num; + d->scan_enter_num += a->scan_enter_num; + d->uwps_num += a->uwps_num; +#ifdef CONFIG_IOCTL_CFG80211 + #ifdef CONFIG_P2P + d->roch_num += a->roch_num; + #endif + d->mgmt_tx_num += a->mgmt_tx_num; +#endif +} + +void dump_mi_status(void *sel, struct dvobj_priv *dvobj) +{ + RTW_PRINT_SEL(sel, "== dvobj-iface_state ==\n"); + RTW_PRINT_SEL(sel, "sta_num:%d\n", DEV_STA_NUM(dvobj)); + RTW_PRINT_SEL(sel, "linking_sta_num:%d\n", DEV_STA_LG_NUM(dvobj)); + RTW_PRINT_SEL(sel, "linked_sta_num:%d\n", DEV_STA_LD_NUM(dvobj)); +#ifdef CONFIG_TDLS + RTW_PRINT_SEL(sel, "linked_tdls_num:%d\n", DEV_TDLS_LD_NUM(dvobj)); +#endif +#ifdef CONFIG_AP_MODE + RTW_PRINT_SEL(sel, "ap_num:%d\n", DEV_AP_NUM(dvobj)); + RTW_PRINT_SEL(sel, "starting_ap_num:%d\n", DEV_AP_STARTING_NUM(dvobj)); + RTW_PRINT_SEL(sel, "linked_ap_num:%d\n", DEV_AP_LD_NUM(dvobj)); +#endif + RTW_PRINT_SEL(sel, "adhoc_num:%d\n", DEV_ADHOC_NUM(dvobj)); + RTW_PRINT_SEL(sel, "linked_adhoc_num:%d\n", DEV_ADHOC_LD_NUM(dvobj)); +#ifdef CONFIG_RTW_MESH + RTW_PRINT_SEL(sel, "mesh_num:%d\n", DEV_MESH_NUM(dvobj)); + RTW_PRINT_SEL(sel, "linked_mesh_num:%d\n", DEV_MESH_LD_NUM(dvobj)); +#endif +#ifdef CONFIG_P2P + RTW_PRINT_SEL(sel, "p2p_device_num:%d\n", DEV_P2P_DV_NUM(dvobj)); + RTW_PRINT_SEL(sel, "p2p_gc_num:%d\n", DEV_P2P_GC_NUM(dvobj)); + RTW_PRINT_SEL(sel, "p2p_go_num:%d\n", DEV_P2P_GO_NUM(dvobj)); +#endif + RTW_PRINT_SEL(sel, "scan_num:%d\n", DEV_SCAN_NUM(dvobj)); + RTW_PRINT_SEL(sel, "under_wps_num:%d\n", DEV_WPS_NUM(dvobj)); +#if defined(CONFIG_IOCTL_CFG80211) + #if defined(CONFIG_P2P) + RTW_PRINT_SEL(sel, "roch_num:%d\n", DEV_ROCH_NUM(dvobj)); + #endif + RTW_PRINT_SEL(sel, "mgmt_tx_num:%d\n", DEV_MGMT_TX_NUM(dvobj)); +#endif + RTW_PRINT_SEL(sel, "union_ch:%d\n", DEV_U_CH(dvobj)); + RTW_PRINT_SEL(sel, "union_bw:%d\n", DEV_U_BW(dvobj)); + RTW_PRINT_SEL(sel, "union_offset:%d\n", DEV_U_OFFSET(dvobj)); + RTW_PRINT_SEL(sel, "================\n\n"); +} + +void dump_dvobj_mi_status(void *sel, const char *fun_name, _adapter *adapter) +{ + RTW_INFO("\n[ %s ] call %s\n", fun_name, __func__); + dump_mi_status(sel, adapter_to_dvobj(adapter)); +} + +inline void rtw_mi_update_iface_status(struct mlme_priv *pmlmepriv, sint state) +{ + _adapter *adapter = container_of(pmlmepriv, _adapter, mlmepriv); + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct mi_state *iface_state = &dvobj->iface_state; + struct mi_state tmp_mstate; + + if (state == WIFI_MONITOR_STATE + || state == 0xFFFFFFFF + ) + return; + + if (0) + RTW_INFO("%s => will change or clean state to 0x%08x\n", __func__, state); + + rtw_mi_status(adapter, &tmp_mstate); + _rtw_memcpy(iface_state, &tmp_mstate, sizeof(struct mi_state)); + +#ifdef DBG_IFACE_STATUS + DBG_IFACE_STATUS_DUMP(adapter); +#endif +} +u8 rtw_mi_check_status(_adapter *adapter, u8 type) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct mi_state *iface_state = &dvobj->iface_state; + u8 ret = _FALSE; + +#ifdef DBG_IFACE_STATUS + DBG_IFACE_STATUS_DUMP(adapter); + RTW_INFO("%s-"ADPT_FMT" check type:%d\n", __func__, ADPT_ARG(adapter), type); +#endif + + switch (type) { + case MI_LINKED: + if (MSTATE_STA_LD_NUM(iface_state) || MSTATE_AP_NUM(iface_state) || MSTATE_ADHOC_NUM(iface_state) || MSTATE_MESH_NUM(iface_state)) /*check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE)*/ + ret = _TRUE; + break; + case MI_ASSOC: + if (MSTATE_STA_LD_NUM(iface_state) || MSTATE_AP_LD_NUM(iface_state) || MSTATE_ADHOC_LD_NUM(iface_state) || MSTATE_MESH_LD_NUM(iface_state)) + ret = _TRUE; + break; + case MI_UNDER_WPS: + if (MSTATE_WPS_NUM(iface_state)) + ret = _TRUE; + break; + + case MI_AP_MODE: + if (MSTATE_AP_NUM(iface_state)) + ret = _TRUE; + break; + case MI_AP_ASSOC: + if (MSTATE_AP_LD_NUM(iface_state)) + ret = _TRUE; + break; + + case MI_ADHOC: + if (MSTATE_ADHOC_NUM(iface_state)) + ret = _TRUE; + break; + case MI_ADHOC_ASSOC: + if (MSTATE_ADHOC_LD_NUM(iface_state)) + ret = _TRUE; + break; + +#ifdef CONFIG_RTW_MESH + case MI_MESH: + if (MSTATE_MESH_NUM(iface_state)) + ret = _TRUE; + break; + case MI_MESH_ASSOC: + if (MSTATE_MESH_LD_NUM(iface_state)) + ret = _TRUE; + break; +#endif + + case MI_STA_NOLINK: /* this is misleading, but not used now */ + if (MSTATE_STA_NUM(iface_state) && (!(MSTATE_STA_LD_NUM(iface_state) || MSTATE_STA_LG_NUM(iface_state)))) + ret = _TRUE; + break; + case MI_STA_LINKED: + if (MSTATE_STA_LD_NUM(iface_state)) + ret = _TRUE; + break; + case MI_STA_LINKING: + if (MSTATE_STA_LG_NUM(iface_state)) + ret = _TRUE; + break; + + default: + break; + } + return ret; +} + +/* +* return value : 0 is failed or have not interface meet condition +* return value : !0 is success or interface numbers which meet condition +* return value of ops_func must be _TRUE or _FALSE +*/ +static u8 _rtw_mi_process(_adapter *padapter, bool exclude_self, + void *data, u8(*ops_func)(_adapter *padapter, void *data)) +{ + int i; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + u8 ret = 0; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + + if ((exclude_self) && (iface == padapter)) + continue; + + if (ops_func) + if (_TRUE == ops_func(iface, data)) + ret++; + } + } + return ret; +} +static u8 _rtw_mi_process_without_schk(_adapter *padapter, bool exclude_self, + void *data, u8(*ops_func)(_adapter *padapter, void *data)) +{ + int i; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + u8 ret = 0; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface) { + if ((exclude_self) && (iface == padapter)) + continue; + + if (ops_func) + if (ops_func(iface, data) == _TRUE) + ret++; + } + } + return ret; +} + +static u8 _rtw_mi_netif_caroff_qstop(_adapter *padapter, void *data) +{ + struct net_device *pnetdev = padapter->pnetdev; + + rtw_netif_carrier_off(pnetdev); + rtw_netif_stop_queue(pnetdev); + return _TRUE; +} +u8 rtw_mi_netif_caroff_qstop(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_netif_caroff_qstop); +} +u8 rtw_mi_buddy_netif_caroff_qstop(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_netif_caroff_qstop); +} + +static u8 _rtw_mi_netif_caron_qstart(_adapter *padapter, void *data) +{ + struct net_device *pnetdev = padapter->pnetdev; + + rtw_netif_carrier_on(pnetdev); + rtw_netif_start_queue(pnetdev); + return _TRUE; +} +u8 rtw_mi_netif_caron_qstart(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_netif_caron_qstart); +} +u8 rtw_mi_buddy_netif_caron_qstart(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_netif_caron_qstart); +} + +static u8 _rtw_mi_netif_stop_queue(_adapter *padapter, void *data) +{ + struct net_device *pnetdev = padapter->pnetdev; + + rtw_netif_stop_queue(pnetdev); + return _TRUE; +} +u8 rtw_mi_netif_stop_queue(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_netif_stop_queue); +} +u8 rtw_mi_buddy_netif_stop_queue(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_netif_stop_queue); +} + +static u8 _rtw_mi_netif_wake_queue(_adapter *padapter, void *data) +{ + struct net_device *pnetdev = padapter->pnetdev; + + if (pnetdev) + rtw_netif_wake_queue(pnetdev); + return _TRUE; +} +u8 rtw_mi_netif_wake_queue(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_netif_wake_queue); +} +u8 rtw_mi_buddy_netif_wake_queue(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_netif_wake_queue); +} + +static u8 _rtw_mi_netif_carrier_on(_adapter *padapter, void *data) +{ + struct net_device *pnetdev = padapter->pnetdev; + + if (pnetdev) + rtw_netif_carrier_on(pnetdev); + return _TRUE; +} +u8 rtw_mi_netif_carrier_on(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_netif_carrier_on); +} +u8 rtw_mi_buddy_netif_carrier_on(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_netif_carrier_on); +} + +static u8 _rtw_mi_netif_carrier_off(_adapter *padapter, void *data) +{ + struct net_device *pnetdev = padapter->pnetdev; + + if (pnetdev) + rtw_netif_carrier_off(pnetdev); + return _TRUE; +} +u8 rtw_mi_netif_carrier_off(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_netif_carrier_off); +} +u8 rtw_mi_buddy_netif_carrier_off(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_netif_carrier_off); +} + +static u8 _rtw_mi_scan_abort(_adapter *adapter, void *data) +{ + bool bwait = *(bool *)data; + + if (bwait) + rtw_scan_abort(adapter); + else + rtw_scan_abort_no_wait(adapter); + + return _TRUE; +} +void rtw_mi_scan_abort(_adapter *adapter, bool bwait) +{ + bool in_data = bwait; + + _rtw_mi_process(adapter, _FALSE, &in_data, _rtw_mi_scan_abort); + +} +void rtw_mi_buddy_scan_abort(_adapter *adapter, bool bwait) +{ + bool in_data = bwait; + + _rtw_mi_process(adapter, _TRUE, &in_data, _rtw_mi_scan_abort); +} + +static u32 _rtw_mi_start_drv_threads(_adapter *adapter, bool exclude_self) +{ + int i; + _adapter *iface = NULL; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + u32 _status = _SUCCESS; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface) { + if ((exclude_self) && (iface == adapter)) + continue; + if (rtw_start_drv_threads(iface) == _FAIL) { + _status = _FAIL; + break; + } + } + } + return _status; +} +u32 rtw_mi_start_drv_threads(_adapter *adapter) +{ + return _rtw_mi_start_drv_threads(adapter, _FALSE); +} +u32 rtw_mi_buddy_start_drv_threads(_adapter *adapter) +{ + return _rtw_mi_start_drv_threads(adapter, _TRUE); +} + +static void _rtw_mi_stop_drv_threads(_adapter *adapter, bool exclude_self) +{ + int i; + _adapter *iface = NULL; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface) { + if ((exclude_self) && (iface == adapter)) + continue; + rtw_stop_drv_threads(iface); + } + } +} +void rtw_mi_stop_drv_threads(_adapter *adapter) +{ + _rtw_mi_stop_drv_threads(adapter, _FALSE); +} +void rtw_mi_buddy_stop_drv_threads(_adapter *adapter) +{ + _rtw_mi_stop_drv_threads(adapter, _TRUE); +} + +static u8 _rtw_mi_cancel_all_timer(_adapter *adapter, void *data) +{ + rtw_cancel_all_timer(adapter); + return _TRUE; +} +void rtw_mi_cancel_all_timer(_adapter *adapter) +{ + _rtw_mi_process(adapter, _FALSE, NULL, _rtw_mi_cancel_all_timer); +} +void rtw_mi_buddy_cancel_all_timer(_adapter *adapter) +{ + _rtw_mi_process(adapter, _TRUE, NULL, _rtw_mi_cancel_all_timer); +} + +static u8 _rtw_mi_reset_drv_sw(_adapter *adapter, void *data) +{ + rtw_reset_drv_sw(adapter); + return _TRUE; +} +void rtw_mi_reset_drv_sw(_adapter *adapter) +{ + _rtw_mi_process_without_schk(adapter, _FALSE, NULL, _rtw_mi_reset_drv_sw); +} +void rtw_mi_buddy_reset_drv_sw(_adapter *adapter) +{ + _rtw_mi_process_without_schk(adapter, _TRUE, NULL, _rtw_mi_reset_drv_sw); +} + +static u8 _rtw_mi_intf_start(_adapter *adapter, void *data) +{ + rtw_intf_start(adapter); + return _TRUE; +} +void rtw_mi_intf_start(_adapter *adapter) +{ + _rtw_mi_process(adapter, _FALSE, NULL, _rtw_mi_intf_start); +} +void rtw_mi_buddy_intf_start(_adapter *adapter) +{ + _rtw_mi_process(adapter, _TRUE, NULL, _rtw_mi_intf_start); +} + +static u8 _rtw_mi_intf_stop(_adapter *adapter, void *data) +{ + rtw_intf_stop(adapter); + return _TRUE; +} +void rtw_mi_intf_stop(_adapter *adapter) +{ + _rtw_mi_process(adapter, _FALSE, NULL, _rtw_mi_intf_stop); +} +void rtw_mi_buddy_intf_stop(_adapter *adapter) +{ + _rtw_mi_process(adapter, _TRUE, NULL, _rtw_mi_intf_stop); +} + +#ifdef CONFIG_NEW_NETDEV_HDL +u8 rtw_mi_hal_iface_init(_adapter *padapter) +{ + int i; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + u8 ret = _TRUE; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface && iface->netif_up) + rtw_hal_iface_init(padapter); + } + return ret; +} +#endif + +static u8 _rtw_mi_suspend_free_assoc_resource(_adapter *padapter, void *data) +{ + return rtw_suspend_free_assoc_resource(padapter); +} +void rtw_mi_suspend_free_assoc_resource(_adapter *adapter) +{ + _rtw_mi_process(adapter, _FALSE, NULL, _rtw_mi_suspend_free_assoc_resource); +} +void rtw_mi_buddy_suspend_free_assoc_resource(_adapter *adapter) +{ + _rtw_mi_process(adapter, _TRUE, NULL, _rtw_mi_suspend_free_assoc_resource); +} + +static u8 _rtw_mi_is_scan_deny(_adapter *adapter, void *data) +{ + return rtw_is_scan_deny(adapter); +} + +u8 rtw_mi_is_scan_deny(_adapter *adapter) +{ + return _rtw_mi_process(adapter, _FALSE, NULL, _rtw_mi_is_scan_deny); + +} +u8 rtw_mi_buddy_is_scan_deny(_adapter *adapter) +{ + return _rtw_mi_process(adapter, _TRUE, NULL, _rtw_mi_is_scan_deny); +} + +#ifdef CONFIG_SET_SCAN_DENY_TIMER +static u8 _rtw_mi_set_scan_deny(_adapter *adapter, void *data) +{ + u32 ms = *(u32 *)data; + + rtw_set_scan_deny(adapter, ms); + return _TRUE; +} +void rtw_mi_set_scan_deny(_adapter *adapter, u32 ms) +{ + u32 in_data = ms; + + _rtw_mi_process(adapter, _FALSE, &in_data, _rtw_mi_set_scan_deny); +} +void rtw_mi_buddy_set_scan_deny(_adapter *adapter, u32 ms) +{ + u32 in_data = ms; + + _rtw_mi_process(adapter, _TRUE, &in_data, _rtw_mi_set_scan_deny); +} +#endif /*CONFIG_SET_SCAN_DENY_TIMER*/ + +#ifdef CONFIG_AP_MODE +static u8 _rtw_mi_beacon_update(_adapter *padapter, void *data) +{ + if (!MLME_IS_STA(padapter) + && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _TRUE) { + RTW_INFO(ADPT_FMT" - update_beacon\n", ADPT_ARG(padapter)); + update_beacon(padapter, 0xFF, NULL, _TRUE, 0); + } + return _TRUE; +} + +void rtw_mi_beacon_update(_adapter *padapter) +{ + _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_beacon_update); +} + +void rtw_mi_buddy_beacon_update(_adapter *padapter) +{ + _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_beacon_update); +} +#endif /* CONFIG_AP_MODE */ + +#ifndef CONFIG_MI_WITH_MBSSID_CAM +static u8 _rtw_mi_hal_dump_macaddr(_adapter *padapter, void *sel) +{ + u8 mac_addr[ETH_ALEN] = {0}; + + rtw_hal_get_hwreg(padapter, HW_VAR_MAC_ADDR, mac_addr); + RTW_PRINT_SEL(sel, ADPT_FMT"- hw port(%d) mac_addr ="MAC_FMT"\n", + ADPT_ARG(padapter), padapter->hw_port, MAC_ARG(mac_addr)); + + return _TRUE; +} +void rtw_mi_hal_dump_macaddr(void *sel, _adapter *padapter) +{ + _rtw_mi_process(padapter, _FALSE, sel, _rtw_mi_hal_dump_macaddr); +} +void rtw_mi_buddy_hal_dump_macaddr(void *sel, _adapter *padapter) +{ + _rtw_mi_process(padapter, _TRUE, sel, _rtw_mi_hal_dump_macaddr); +} +#endif + +#ifdef CONFIG_PCI_HCI +static u8 _rtw_mi_xmit_tasklet_schedule(_adapter *padapter, void *data) +{ + if (rtw_txframes_pending(padapter)) { + /* try to deal with the pending packets */ + tasklet_hi_schedule(&(padapter->xmitpriv.xmit_tasklet)); + } + return _TRUE; +} +void rtw_mi_xmit_tasklet_schedule(_adapter *padapter) +{ + _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_xmit_tasklet_schedule); +} +void rtw_mi_buddy_xmit_tasklet_schedule(_adapter *padapter) +{ + _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_xmit_tasklet_schedule); +} +#endif + +u8 _rtw_mi_busy_traffic_check(_adapter *padapter, void *data) +{ + return padapter->mlmepriv.LinkDetectInfo.bBusyTraffic; +} + +u8 rtw_mi_busy_traffic_check(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_busy_traffic_check); +} +u8 rtw_mi_buddy_busy_traffic_check(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_busy_traffic_check); +} +static u8 _rtw_mi_check_mlmeinfo_state(_adapter *padapter, void *data) +{ + u32 state = *(u32 *)data; + struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv; + + /*if (mlmeext_msr(mlmeext) == state)*/ + if (check_mlmeinfo_state(mlmeext, state)) + return _TRUE; + else + return _FALSE; +} + +u8 rtw_mi_check_mlmeinfo_state(_adapter *padapter, u32 state) +{ + u32 in_data = state; + + return _rtw_mi_process(padapter, _FALSE, &in_data, _rtw_mi_check_mlmeinfo_state); +} + +u8 rtw_mi_buddy_check_mlmeinfo_state(_adapter *padapter, u32 state) +{ + u32 in_data = state; + + return _rtw_mi_process(padapter, _TRUE, &in_data, _rtw_mi_check_mlmeinfo_state); +} + +/*#define DBG_DUMP_FW_STATE*/ +#ifdef DBG_DUMP_FW_STATE +static void rtw_dbg_dump_fwstate(_adapter *padapter, sint state) +{ + u8 buf[32] = {0}; + + if (state & WIFI_FW_NULL_STATE) { + _rtw_memset(buf, 0, 32); + sprintf(buf, "WIFI_FW_NULL_STATE"); + RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf); + } + + if (state & WIFI_ASOC_STATE) { + _rtw_memset(buf, 0, 32); + sprintf(buf, "WIFI_ASOC_STATE"); + RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf); + } + + if (state & WIFI_UNDER_LINKING) { + _rtw_memset(buf, 0, 32); + sprintf(buf, "WIFI_UNDER_LINKING"); + RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf); + } + + if (state & WIFI_UNDER_SURVEY) { + _rtw_memset(buf, 0, 32); + sprintf(buf, "WIFI_UNDER_SURVEY"); + RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf); + } +} +#endif + +static u8 _rtw_mi_check_fwstate(_adapter *padapter, void *data) +{ + u8 ret = _FALSE; + + sint state = *(sint *)data; + + if ((state == WIFI_FW_NULL_STATE) && + (padapter->mlmepriv.fw_state == WIFI_FW_NULL_STATE)) + ret = _TRUE; + else if (_TRUE == check_fwstate(&padapter->mlmepriv, state)) + ret = _TRUE; +#ifdef DBG_DUMP_FW_STATE + if (ret) + rtw_dbg_dump_fwstate(padapter, state); +#endif + return ret; +} +u8 rtw_mi_check_fwstate(_adapter *padapter, sint state) +{ + sint in_data = state; + + return _rtw_mi_process(padapter, _FALSE, &in_data, _rtw_mi_check_fwstate); +} +u8 rtw_mi_buddy_check_fwstate(_adapter *padapter, sint state) +{ + sint in_data = state; + + return _rtw_mi_process(padapter, _TRUE, &in_data, _rtw_mi_check_fwstate); +} + +static u8 _rtw_mi_traffic_statistics(_adapter *padapter , void *data) +{ + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + + /* Tx */ + pdvobjpriv->traffic_stat.tx_bytes += padapter->xmitpriv.tx_bytes; + pdvobjpriv->traffic_stat.tx_pkts += padapter->xmitpriv.tx_pkts; + pdvobjpriv->traffic_stat.tx_drop += padapter->xmitpriv.tx_drop; + + /* Rx */ + pdvobjpriv->traffic_stat.rx_bytes += padapter->recvpriv.rx_bytes; + pdvobjpriv->traffic_stat.rx_pkts += padapter->recvpriv.rx_pkts; + pdvobjpriv->traffic_stat.rx_drop += padapter->recvpriv.rx_drop; + return _TRUE; +} +u8 rtw_mi_traffic_statistics(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_traffic_statistics); +} + +static u8 _rtw_mi_check_miracast_enabled(_adapter *padapter , void *data) +{ + return is_miracast_enabled(padapter); +} +u8 rtw_mi_check_miracast_enabled(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_check_miracast_enabled); +} + +#ifdef CONFIG_XMIT_THREAD_MODE +static u8 _rtw_mi_check_pending_xmitbuf(_adapter *padapter , void *data) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + return check_pending_xmitbuf(pxmitpriv); +} +u8 rtw_mi_check_pending_xmitbuf(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_check_pending_xmitbuf); +} +u8 rtw_mi_buddy_check_pending_xmitbuf(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_check_pending_xmitbuf); +} +#endif + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) +static u8 _rtw_mi_dequeue_writeport(_adapter *padapter , bool exclude_self) +{ + int i; + u8 queue_empty = _TRUE; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + + if ((exclude_self) && (iface == padapter)) + continue; + + queue_empty &= _dequeue_writeport(iface); + } + } + return queue_empty; +} +u8 rtw_mi_dequeue_writeport(_adapter *padapter) +{ + return _rtw_mi_dequeue_writeport(padapter, _FALSE); +} +u8 rtw_mi_buddy_dequeue_writeport(_adapter *padapter) +{ + return _rtw_mi_dequeue_writeport(padapter, _TRUE); +} +#endif +static void _rtw_mi_adapter_reset(_adapter *padapter , u8 exclude_self) +{ + int i; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + if (dvobj->padapters[i]) { + if ((exclude_self) && (dvobj->padapters[i] == padapter)) + continue; + dvobj->padapters[i] = NULL; + } + } +} + +void rtw_mi_adapter_reset(_adapter *padapter) +{ + _rtw_mi_adapter_reset(padapter, _FALSE); +} + +void rtw_mi_buddy_adapter_reset(_adapter *padapter) +{ + _rtw_mi_adapter_reset(padapter, _TRUE); +} + +static u8 _rtw_mi_dynamic_check_timer_handlder(_adapter *adapter, void *data) +{ + rtw_iface_dynamic_check_timer_handlder(adapter); + return _TRUE; +} +u8 rtw_mi_dynamic_check_timer_handlder(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_dynamic_check_timer_handlder); +} +u8 rtw_mi_buddy_dynamic_check_timer_handlder(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_dynamic_check_timer_handlder); +} + +static u8 _rtw_mi_dynamic_chk_wk_hdl(_adapter *adapter, void *data) +{ + rtw_iface_dynamic_chk_wk_hdl(adapter); + return _TRUE; +} +u8 rtw_mi_dynamic_chk_wk_hdl(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_dynamic_chk_wk_hdl); +} +u8 rtw_mi_buddy_dynamic_chk_wk_hdl(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_dynamic_chk_wk_hdl); +} + +static u8 _rtw_mi_os_xmit_schedule(_adapter *adapter, void *data) +{ + rtw_os_xmit_schedule(adapter); + return _TRUE; +} +u8 rtw_mi_os_xmit_schedule(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_os_xmit_schedule); +} +u8 rtw_mi_buddy_os_xmit_schedule(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_os_xmit_schedule); +} + +static u8 _rtw_mi_report_survey_event(_adapter *adapter, void *data) +{ + union recv_frame *precv_frame = (union recv_frame *)data; + + report_survey_event(adapter, precv_frame); + return _TRUE; +} +u8 rtw_mi_report_survey_event(_adapter *padapter, union recv_frame *precv_frame) +{ + return _rtw_mi_process(padapter, _FALSE, precv_frame, _rtw_mi_report_survey_event); +} +u8 rtw_mi_buddy_report_survey_event(_adapter *padapter, union recv_frame *precv_frame) +{ + return _rtw_mi_process(padapter, _TRUE, precv_frame, _rtw_mi_report_survey_event); +} + +static u8 _rtw_mi_sreset_adapter_hdl(_adapter *adapter, void *data) +{ + u8 bstart = *(u8 *)data; + + if (bstart) + sreset_start_adapter(adapter); + else + sreset_stop_adapter(adapter); + return _TRUE; +} +u8 rtw_mi_sreset_adapter_hdl(_adapter *padapter, u8 bstart) +{ + u8 in_data = bstart; + + return _rtw_mi_process(padapter, _FALSE, &in_data, _rtw_mi_sreset_adapter_hdl); +} + +#if defined(CONFIG_AP_MODE) && defined(DBG_CONFIG_ERROR_RESET) && defined(CONFIG_CONCURRENT_MODE) +void rtw_mi_ap_info_restore(_adapter *adapter) +{ + int i; + _adapter *iface; + struct mlme_priv *pmlmepriv; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface) { + pmlmepriv = &iface->mlmepriv; + + if (MLME_IS_AP(iface) || MLME_IS_MESH(iface)) { + RTW_INFO(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(iface), MLME_IS_AP(iface) ? "AP" : "MESH"); + rtw_iface_bcmc_sec_cam_map_restore(iface); + } + } + } +} +#endif /*#if defined(DBG_CONFIG_ERROR_RESET) && defined(CONFIG_CONCURRENT_MODE)*/ + +u8 rtw_mi_buddy_sreset_adapter_hdl(_adapter *padapter, u8 bstart) +{ + u8 in_data = bstart; + + return _rtw_mi_process(padapter, _TRUE, &in_data, _rtw_mi_sreset_adapter_hdl); +} + +#ifdef CONFIG_AP_MODE +static u8 _rtw_mi_tx_beacon_hdl(_adapter *adapter, void *data) +{ + if ((MLME_IS_AP(adapter) || MLME_IS_MESH(adapter)) + && check_fwstate(&adapter->mlmepriv, WIFI_ASOC_STATE) == _TRUE + ) { + adapter->mlmepriv.update_bcn = _TRUE; +#ifndef CONFIG_INTERRUPT_BASED_TXBCN +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) || defined(CONFIG_PCI_BCN_POLLING) + tx_beacon_hdl(adapter, NULL); +#endif +#endif + } + return _TRUE; +} +u8 rtw_mi_tx_beacon_hdl(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_tx_beacon_hdl); +} +u8 rtw_mi_buddy_tx_beacon_hdl(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_sreset_adapter_hdl); +} + +static u8 _rtw_mi_set_tx_beacon_cmd(_adapter *adapter, void *data) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + if (MLME_IS_AP(adapter) || MLME_IS_MESH(adapter)) { + if (pmlmepriv->update_bcn == _TRUE) + set_tx_beacon_cmd(adapter, 0); + } + return _TRUE; +} +u8 rtw_mi_set_tx_beacon_cmd(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_set_tx_beacon_cmd); +} +u8 rtw_mi_buddy_set_tx_beacon_cmd(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_set_tx_beacon_cmd); +} +#endif /* CONFIG_AP_MODE */ + +#ifdef CONFIG_P2P +static u8 _rtw_mi_p2p_chk_state(_adapter *adapter, void *data) +{ + struct wifidirect_info *pwdinfo = &(adapter->wdinfo); + enum P2P_STATE state = *(enum P2P_STATE *)data; + + return rtw_p2p_chk_state(pwdinfo, state); +} +u8 rtw_mi_p2p_chk_state(_adapter *padapter, enum P2P_STATE p2p_state) +{ + u8 in_data = p2p_state; + + return _rtw_mi_process(padapter, _FALSE, &in_data, _rtw_mi_p2p_chk_state); +} +u8 rtw_mi_buddy_p2p_chk_state(_adapter *padapter, enum P2P_STATE p2p_state) +{ + u8 in_data = p2p_state; + + return _rtw_mi_process(padapter, _TRUE, &in_data, _rtw_mi_p2p_chk_state); +} +static u8 _rtw_mi_stay_in_p2p_mode(_adapter *adapter, void *data) +{ + struct wifidirect_info *pwdinfo = &(adapter->wdinfo); + + if (rtw_p2p_role(pwdinfo) != P2P_ROLE_DISABLE) + return _TRUE; + return _FALSE; +} +u8 rtw_mi_stay_in_p2p_mode(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _FALSE, NULL, _rtw_mi_stay_in_p2p_mode); +} +u8 rtw_mi_buddy_stay_in_p2p_mode(_adapter *padapter) +{ + return _rtw_mi_process(padapter, _TRUE, NULL, _rtw_mi_stay_in_p2p_mode); +} +#endif /*CONFIG_P2P*/ + +_adapter *rtw_get_iface_by_id(_adapter *padapter, u8 iface_id) +{ + _adapter *iface = NULL; + struct dvobj_priv *dvobj; + + if ((padapter == NULL) || (iface_id >= CONFIG_IFACE_NUMBER)) { + rtw_warn_on(1); + return iface; + } + + dvobj = adapter_to_dvobj(padapter); + return dvobj->padapters[iface_id]; +} + +_adapter *rtw_get_iface_by_macddr(_adapter *padapter, const u8 *mac_addr) +{ + int i; + _adapter *iface = NULL; + u8 bmatch = _FALSE; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && (_rtw_memcmp(mac_addr, adapter_mac_addr(iface), ETH_ALEN))) { + bmatch = _TRUE; + break; + } + } + if (bmatch) + return iface; + else + return NULL; +} + +_adapter *rtw_get_iface_by_hwport(_adapter *padapter, u8 hw_port) +{ + int i; + _adapter *iface = NULL; + u8 bmatch = _FALSE; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && (hw_port == iface->hw_port)) { + bmatch = _TRUE; + break; + } + } + if (bmatch) + return iface; + else + return NULL; +} + +/*#define CONFIG_SKB_ALLOCATED*/ +#define DBG_SKB_PROCESS +#ifdef DBG_SKB_PROCESS +void rtw_dbg_skb_process(_adapter *padapter, union recv_frame *precvframe, union recv_frame *pcloneframe) +{ + _pkt *pkt_copy, *pkt_org; + + pkt_org = precvframe->u.hdr.pkt; + pkt_copy = pcloneframe->u.hdr.pkt; + /* + RTW_INFO("%s ===== ORG SKB =====\n", __func__); + RTW_INFO(" SKB head(%p)\n", pkt_org->head); + RTW_INFO(" SKB data(%p)\n", pkt_org->data); + RTW_INFO(" SKB tail(%p)\n", pkt_org->tail); + RTW_INFO(" SKB end(%p)\n", pkt_org->end); + + RTW_INFO(" recv frame head(%p)\n", precvframe->u.hdr.rx_head); + RTW_INFO(" recv frame data(%p)\n", precvframe->u.hdr.rx_data); + RTW_INFO(" recv frame tail(%p)\n", precvframe->u.hdr.rx_tail); + RTW_INFO(" recv frame end(%p)\n", precvframe->u.hdr.rx_end); + + RTW_INFO("%s ===== COPY SKB =====\n", __func__); + RTW_INFO(" SKB head(%p)\n", pkt_copy->head); + RTW_INFO(" SKB data(%p)\n", pkt_copy->data); + RTW_INFO(" SKB tail(%p)\n", pkt_copy->tail); + RTW_INFO(" SKB end(%p)\n", pkt_copy->end); + + RTW_INFO(" recv frame head(%p)\n", pcloneframe->u.hdr.rx_head); + RTW_INFO(" recv frame data(%p)\n", pcloneframe->u.hdr.rx_data); + RTW_INFO(" recv frame tail(%p)\n", pcloneframe->u.hdr.rx_tail); + RTW_INFO(" recv frame end(%p)\n", pcloneframe->u.hdr.rx_end); + */ + /* + RTW_INFO("%s => recv_frame adapter(%p,%p)\n", __func__, precvframe->u.hdr.adapter, pcloneframe->u.hdr.adapter); + RTW_INFO("%s => recv_frame dev(%p,%p)\n", __func__, pkt_org->dev , pkt_copy->dev); + RTW_INFO("%s => recv_frame len(%d,%d)\n", __func__, precvframe->u.hdr.len, pcloneframe->u.hdr.len); + */ + if (precvframe->u.hdr.len != pcloneframe->u.hdr.len) + RTW_INFO("%s [WARN] recv_frame length(%d:%d) compare failed\n", __func__, precvframe->u.hdr.len, pcloneframe->u.hdr.len); + + if (_rtw_memcmp(&precvframe->u.hdr.attrib, &pcloneframe->u.hdr.attrib, sizeof(struct rx_pkt_attrib)) == _FALSE) + RTW_INFO("%s [WARN] recv_frame attrib compare failed\n", __func__); + + if (_rtw_memcmp(precvframe->u.hdr.rx_data, pcloneframe->u.hdr.rx_data, precvframe->u.hdr.len) == _FALSE) + RTW_INFO("%s [WARN] recv_frame rx_data compare failed\n", __func__); + +} +#endif + +static s32 _rtw_mi_buddy_clone_bcmc_packet(_adapter *adapter, union recv_frame *precvframe, u8 *pphy_status, union recv_frame *pcloneframe) +{ + s32 ret = _SUCCESS; +#ifdef CONFIG_SKB_ALLOCATED + u8 *pbuf = precvframe->u.hdr.rx_data; +#endif + struct rx_pkt_attrib *pattrib = NULL; + + if (pcloneframe) { + pcloneframe->u.hdr.adapter = adapter; + + _rtw_init_listhead(&pcloneframe->u.hdr.list); + pcloneframe->u.hdr.precvbuf = NULL; /*can't access the precvbuf for new arch.*/ + pcloneframe->u.hdr.len = 0; + + _rtw_memcpy(&pcloneframe->u.hdr.attrib, &precvframe->u.hdr.attrib, sizeof(struct rx_pkt_attrib)); + + pattrib = &pcloneframe->u.hdr.attrib; +#ifdef CONFIG_SKB_ALLOCATED + if (rtw_os_alloc_recvframe(adapter, pcloneframe, pbuf, NULL) == _SUCCESS) +#else + if (rtw_os_recvframe_duplicate_skb(adapter, pcloneframe, precvframe->u.hdr.pkt) == _SUCCESS) +#endif + { +#ifdef CONFIG_SKB_ALLOCATED + recvframe_put(pcloneframe, pattrib->pkt_len); +#endif + +#ifdef DBG_SKB_PROCESS + rtw_dbg_skb_process(adapter, precvframe, pcloneframe); +#endif + + if (pphy_status) + rx_query_phy_status(pcloneframe, pphy_status); + + ret = rtw_recv_entry(pcloneframe); + } else { + ret = -1; + RTW_INFO("%s()-%d: rtw_os_alloc_recvframe() failed!\n", __func__, __LINE__); + } + + } + return ret; +} + +void rtw_mi_buddy_clone_bcmc_packet(_adapter *padapter, union recv_frame *precvframe, u8 *pphy_status) +{ + int i; + s32 ret = _SUCCESS; + _adapter *iface = NULL; + union recv_frame *pcloneframe = NULL; + struct recv_priv *precvpriv = &padapter->recvpriv;/*primary_padapter*/ + _queue *pfree_recv_queue = &precvpriv->free_recv_queue; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + u8 *fhead = get_recvframe_data(precvframe); + u8 type = GetFrameType(fhead); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface || iface == padapter) + continue; + if (rtw_is_adapter_up(iface) == _FALSE || iface->registered == 0) + continue; + if (type == WIFI_DATA_TYPE && !adapter_allow_bmc_data_rx(iface)) + continue; + + pcloneframe = rtw_alloc_recvframe(pfree_recv_queue); + if (pcloneframe) { + ret = _rtw_mi_buddy_clone_bcmc_packet(iface, precvframe, pphy_status, pcloneframe); + if (_SUCCESS != ret) { + if (ret == -1) + rtw_free_recvframe(pcloneframe, pfree_recv_queue); + /*RTW_INFO(ADPT_FMT"-clone BC/MC frame failed\n", ADPT_ARG(iface));*/ + } + } + } + +} + +#ifdef CONFIG_PCI_HCI +/*API be created temporary for MI, caller is interrupt-handler, PCIE's interrupt handler cannot apply to multi-AP*/ +_adapter *rtw_mi_get_ap_adapter(_adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + int i; + _adapter *iface = NULL; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + + if (check_fwstate(&iface->mlmepriv, WIFI_AP_STATE) == _TRUE + && check_fwstate(&iface->mlmepriv, WIFI_ASOC_STATE) == _TRUE) + break; + + } + return iface; +} +#endif + +u8 rtw_mi_get_ld_sta_ifbmp(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + int i; + _adapter *iface = NULL; + u8 ifbmp = 0; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + + if (MLME_IS_STA(iface) && MLME_IS_ASOC(iface)) + ifbmp |= BIT(i); + } + + return ifbmp; +} + +u8 rtw_mi_get_ap_mesh_ifbmp(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + int i; + _adapter *iface = NULL; + u8 ifbmp = 0; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + + if (CHK_MLME_STATE(iface, WIFI_AP_STATE | WIFI_MESH_STATE) + && MLME_IS_ASOC(iface)) + ifbmp |= BIT(i); + } + + return ifbmp; +} + +void rtw_mi_update_ap_bmc_camid(_adapter *padapter, u8 camid_a, u8 camid_b) +{ +#ifdef CONFIG_CONCURRENT_MODE + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + + int i; + _adapter *iface = NULL; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + + if (macid_ctl->iface_bmc[iface->iface_id] != INVALID_SEC_MAC_CAM_ID) { + if (macid_ctl->iface_bmc[iface->iface_id] == camid_a) + macid_ctl->iface_bmc[iface->iface_id] = camid_b; + else if (macid_ctl->iface_bmc[iface->iface_id] == camid_b) + macid_ctl->iface_bmc[iface->iface_id] = camid_a; + iface->securitypriv.dot118021x_bmc_cam_id = macid_ctl->iface_bmc[iface->iface_id]; + } + } +#endif +} + +u8 rtw_mi_get_assoc_if_num(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + u8 n_assoc_iface = 0; +#if 1 + u8 i; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE)) + n_assoc_iface++; + } +#else + n_assoc_iface = DEV_STA_LD_NUM(dvobj) + DEV_AP_NUM(dvobj) + DEV_ADHOC_NUM(dvobj) + DEV_MESH_NUM(dvobj); +#endif + return n_assoc_iface; +} diff --git a/core/rtw_mlme.c b/core/rtw_mlme.c new file mode 100644 index 0000000..1be3ad0 --- /dev/null +++ b/core/rtw_mlme.c @@ -0,0 +1,5982 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_MLME_C_ + +#include +#ifdef CONFIG_PLATFORM_CMAP_INTFS +#include "../os_dep/linux/custom_multiap_intfs/custom_multiap_intfs.h" +#endif + +extern void indicate_wx_scan_complete_event(_adapter *padapter); +extern u8 rtw_do_join(_adapter *padapter); + + +void rtw_init_mlme_timer(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + rtw_init_timer(&(pmlmepriv->assoc_timer), padapter, rtw_join_timeout_handler, padapter); + rtw_init_timer(&(pmlmepriv->scan_to_timer), padapter, rtw_scan_timeout_handler, padapter); + +#ifdef CONFIG_SET_SCAN_DENY_TIMER + rtw_init_timer(&(pmlmepriv->set_scan_deny_timer), padapter, rtw_set_scan_deny_timer_hdl, padapter); +#endif + +#ifdef RTK_DMP_PLATFORM + _init_workitem(&(pmlmepriv->Linkup_workitem), Linkup_workitem_callback, padapter); + _init_workitem(&(pmlmepriv->Linkdown_workitem), Linkdown_workitem_callback, padapter); +#endif +} + +sint _rtw_init_mlme_priv(_adapter *padapter) +{ + sint i; + u8 *pbuf; + struct wlan_network *pnetwork; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + sint res = _SUCCESS; +#ifdef CONFIG_RTW_MULTI_AP + struct unassoc_sta_info *unassoc_sta; +#endif + + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */ + /* _rtw_memset((u8 *)pmlmepriv, 0, sizeof(struct mlme_priv)); */ + + + /*qos_priv*/ + /*pmlmepriv->qospriv.qos_option = pregistrypriv->wmm_enable;*/ + + /*ht_priv*/ +#ifdef CONFIG_80211N_HT + pmlmepriv->htpriv.ampdu_enable = _FALSE;/*set to disabled*/ +#endif + + pmlmepriv->nic_hdl = (u8 *)padapter; + + pmlmepriv->pscanned = NULL; + init_fwstate(pmlmepriv, WIFI_STATION_STATE); + pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown; + pmlmepriv->scan_mode = SCAN_ACTIVE; /* 1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff) */ + + _rtw_spinlock_init(&(pmlmepriv->lock)); + _rtw_init_queue(&(pmlmepriv->free_bss_pool)); + _rtw_init_queue(&(pmlmepriv->scanned_queue)); + + set_scanned_network_val(pmlmepriv, 0); + + _rtw_memset(&pmlmepriv->assoc_ssid, 0, sizeof(NDIS_802_11_SSID)); + + if (padapter->registrypriv.max_bss_cnt != 0) + pmlmepriv->max_bss_cnt = padapter->registrypriv.max_bss_cnt; + else if (rfctl->max_chan_nums <= MAX_CHANNEL_NUM_2G) + pmlmepriv->max_bss_cnt = MAX_BSS_CNT; + else + pmlmepriv->max_bss_cnt = MAX_BSS_CNT + MAX_BSS_CNT; + + + pbuf = rtw_zvmalloc(pmlmepriv->max_bss_cnt * (sizeof(struct wlan_network))); + + if (pbuf == NULL) { + res = _FAIL; + goto exit; + } + pmlmepriv->free_bss_buf = pbuf; + + pnetwork = (struct wlan_network *)pbuf; + + for (i = 0; i < pmlmepriv->max_bss_cnt; i++) { + _rtw_init_listhead(&(pnetwork->list)); + + rtw_list_insert_tail(&(pnetwork->list), &(pmlmepriv->free_bss_pool.queue)); + + pnetwork++; + } + +#ifdef CONFIG_RTW_MULTI_AP + if (is_primary_adapter(padapter)) { + _rtw_init_queue(&(pmlmepriv->free_unassoc_sta_queue)); + _rtw_init_queue(&(pmlmepriv->unassoc_sta_queue)); + for (i = 0; i < UNASOC_STA_SRC_NUM; i++) + pmlmepriv->unassoc_sta_mode_of_stype[i] = padapter->registrypriv.unassoc_sta_mode_of_stype[i]; + if (padapter->registrypriv.max_unassoc_sta_cnt != 0) + pmlmepriv->max_unassoc_sta_cnt = padapter->registrypriv.max_unassoc_sta_cnt; + else if (rfctl->max_chan_nums <= MAX_CHANNEL_NUM_2G) + pmlmepriv->max_unassoc_sta_cnt = MAX_UNASSOC_STA_CNT; + else + pmlmepriv->max_unassoc_sta_cnt = MAX_UNASSOC_STA_CNT * 2; + pbuf = rtw_zvmalloc(pmlmepriv->max_unassoc_sta_cnt * (sizeof(struct unassoc_sta_info))); + if (pbuf == NULL) { + res = _FAIL; + goto exit; + } + pmlmepriv->free_unassoc_sta_buf = pbuf; + unassoc_sta = (struct unassoc_sta_info *) pbuf; + for (i = 0; i < pmlmepriv->max_unassoc_sta_cnt; i++) { + _rtw_init_listhead(&(unassoc_sta->list)); + rtw_list_insert_tail(&(unassoc_sta->list), &(pmlmepriv->free_unassoc_sta_queue.queue)); + unassoc_sta++; + } + } +#ifdef CONFIG_PLATFORM_CMAP_INTFS + rtw_init_timer(&pmlmepriv->cmap_unassoc_sta_timer, padapter, cmap_unassoc_sta_report_info_timer, padapter); +#endif +#endif + /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */ + + rtw_clear_scan_deny(padapter); +#ifdef CONFIG_ARP_KEEP_ALIVE + pmlmepriv->bGetGateway = 0; + pmlmepriv->GetGatewayTryCnt = 0; +#endif + +#ifdef CONFIG_LAYER2_ROAMING +#define RTW_ROAM_SCAN_RESULT_EXP_MS (5*1000) +#define RTW_ROAM_RSSI_DIFF_TH 10 +#define RTW_ROAM_SCAN_INTERVAL (5) /* 5*(2 second)*/ +#define RTW_ROAM_RSSI_THRESHOLD 70 +#define RTW_ROAM_DICONNECT_DELAY 20 + _rtw_spinlock_init(&pmlmepriv->clnt_auth_lock); + pmlmepriv->roam_flags = CONFIG_ROAMING_FLAG; + + pmlmepriv->roam_scanr_exp_ms = RTW_ROAM_SCAN_RESULT_EXP_MS; + pmlmepriv->roam_rssi_diff_th = RTW_ROAM_RSSI_DIFF_TH; + pmlmepriv->roam_scan_int = RTW_ROAM_SCAN_INTERVAL; + pmlmepriv->roam_rssi_threshold = RTW_ROAM_RSSI_THRESHOLD; + pmlmepriv->need_to_roam = _FALSE; + pmlmepriv->last_roaming = rtw_get_current_time(); +#endif /* CONFIG_LAYER2_ROAMING */ + +#ifdef CONFIG_RTW_80211R + rtw_ft_info_init(&pmlmepriv->ft_roam); +#endif +#ifdef CONFIG_LAYER2_ROAMING +#if defined(CONFIG_RTW_WNM) || defined(CONFIG_RTW_80211K) + rtw_roam_nb_info_init(padapter); + pmlmepriv->ch_cnt = 0; +#endif +#endif + + pmlmepriv->defs_lmt_sta = 2; + pmlmepriv->defs_lmt_time = 5; + + rtw_init_mlme_timer(padapter); + +exit: + + + return res; +} + +void rtw_mfree_mlme_priv_lock(struct mlme_priv *pmlmepriv); +void rtw_mfree_mlme_priv_lock(struct mlme_priv *pmlmepriv) +{ + _rtw_spinlock_free(&pmlmepriv->lock); +#ifdef CONFIG_LAYER2_ROAMING + _rtw_spinlock_free(&pmlmepriv->clnt_auth_lock); +#endif + _rtw_spinlock_free(&(pmlmepriv->free_bss_pool.lock)); + _rtw_spinlock_free(&(pmlmepriv->scanned_queue.lock)); +#ifdef CONFIG_RTW_MULTI_AP + if (is_primary_adapter(mlme_to_adapter(pmlmepriv))) { + _rtw_spinlock_free(&(pmlmepriv->unassoc_sta_queue.lock)); + _rtw_spinlock_free(&(pmlmepriv->free_unassoc_sta_queue.lock)); + } +#endif +} + +void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv) +{ + rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len); + rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len); +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + rtw_buf_free(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len); + rtw_buf_free(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len); + rtw_buf_free(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len); + rtw_buf_free(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len); + + rtw_buf_free(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len); + rtw_buf_free(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len); + rtw_buf_free(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len); + rtw_buf_free(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len); + rtw_buf_free(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len); + rtw_buf_free(&pmlmepriv->p2p_assoc_resp_ie, &pmlmepriv->p2p_assoc_resp_ie_len); +#endif + +#if defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211) + rtw_buf_free(&pmlmepriv->wfd_beacon_ie, &pmlmepriv->wfd_beacon_ie_len); + rtw_buf_free(&pmlmepriv->wfd_probe_req_ie, &pmlmepriv->wfd_probe_req_ie_len); + rtw_buf_free(&pmlmepriv->wfd_probe_resp_ie, &pmlmepriv->wfd_probe_resp_ie_len); + rtw_buf_free(&pmlmepriv->wfd_go_probe_resp_ie, &pmlmepriv->wfd_go_probe_resp_ie_len); + rtw_buf_free(&pmlmepriv->wfd_assoc_req_ie, &pmlmepriv->wfd_assoc_req_ie_len); + rtw_buf_free(&pmlmepriv->wfd_assoc_resp_ie, &pmlmepriv->wfd_assoc_resp_ie_len); +#endif + +#ifdef CONFIG_RTW_80211R + rtw_buf_free(&pmlmepriv->auth_rsp, &pmlmepriv->auth_rsp_len); +#endif +#ifdef CONFIG_RTW_MBO + rtw_buf_free(&pmlmepriv->pcell_data_cap_ie, &pmlmepriv->cell_data_cap_len); +#endif +} + +#if defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211) +int rtw_mlme_update_wfd_ie_data(struct mlme_priv *mlme, u8 type, u8 *ie, u32 ie_len) +{ + _adapter *adapter = mlme_to_adapter(mlme); + struct wifi_display_info *wfd_info = &adapter->wfd_info; + u8 clear = 0; + u8 **t_ie = NULL; + u32 *t_ie_len = NULL; + int ret = _FAIL; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + goto success; + + if (wfd_info->wfd_enable == _TRUE) + goto success; /* WFD IE is build by self */ + + if (!ie && !ie_len) + clear = 1; + else if (!ie || !ie_len) { + RTW_PRINT(FUNC_ADPT_FMT" type:%u, ie:%p, ie_len:%u" + , FUNC_ADPT_ARG(adapter), type, ie, ie_len); + rtw_warn_on(1); + goto exit; + } + + switch (type) { + case MLME_BEACON_IE: + t_ie = &mlme->wfd_beacon_ie; + t_ie_len = &mlme->wfd_beacon_ie_len; + break; + case MLME_PROBE_REQ_IE: + t_ie = &mlme->wfd_probe_req_ie; + t_ie_len = &mlme->wfd_probe_req_ie_len; + break; + case MLME_PROBE_RESP_IE: + t_ie = &mlme->wfd_probe_resp_ie; + t_ie_len = &mlme->wfd_probe_resp_ie_len; + break; + case MLME_GO_PROBE_RESP_IE: + t_ie = &mlme->wfd_go_probe_resp_ie; + t_ie_len = &mlme->wfd_go_probe_resp_ie_len; + break; + case MLME_ASSOC_REQ_IE: + t_ie = &mlme->wfd_assoc_req_ie; + t_ie_len = &mlme->wfd_assoc_req_ie_len; + break; + case MLME_ASSOC_RESP_IE: + t_ie = &mlme->wfd_assoc_resp_ie; + t_ie_len = &mlme->wfd_assoc_resp_ie_len; + break; + default: + RTW_PRINT(FUNC_ADPT_FMT" unsupported type:%u" + , FUNC_ADPT_ARG(adapter), type); + rtw_warn_on(1); + goto exit; + } + + if (*t_ie) { + u32 free_len = *t_ie_len; + *t_ie_len = 0; + rtw_mfree(*t_ie, free_len); + *t_ie = NULL; + } + + if (!clear) { + *t_ie = rtw_malloc(ie_len); + if (*t_ie == NULL) { + RTW_ERR(FUNC_ADPT_FMT" type:%u, rtw_malloc() fail\n" + , FUNC_ADPT_ARG(adapter), type); + goto exit; + } + _rtw_memcpy(*t_ie, ie, ie_len); + *t_ie_len = ie_len; + } + + if (*t_ie && *t_ie_len) { + u8 *attr_content; + u32 attr_contentlen = 0; + + attr_content = rtw_get_wfd_attr_content(*t_ie, *t_ie_len, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen); + if (attr_content && attr_contentlen) { + if (RTW_GET_BE16(attr_content + 2) != wfd_info->rtsp_ctrlport) { + wfd_info->rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + RTW_INFO(FUNC_ADPT_FMT" type:%u, RTSP CTRL port = %u\n" + , FUNC_ADPT_ARG(adapter), type, wfd_info->rtsp_ctrlport); + } + } + } + +success: + ret = _SUCCESS; + +exit: + return ret; +} +#endif /* defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211) */ + +void _rtw_free_mlme_priv(struct mlme_priv *pmlmepriv) +{ + _adapter *adapter = mlme_to_adapter(pmlmepriv); + if (NULL == pmlmepriv) { + rtw_warn_on(1); + goto exit; + } + rtw_free_mlme_priv_ie_data(pmlmepriv); + + if (pmlmepriv) { + rtw_mfree_mlme_priv_lock(pmlmepriv); + + if (pmlmepriv->free_bss_buf) + rtw_vmfree(pmlmepriv->free_bss_buf, pmlmepriv->max_bss_cnt * sizeof(struct wlan_network)); +#ifdef CONFIG_RTW_MULTI_AP + if (is_primary_adapter(adapter)) { + if (pmlmepriv->free_unassoc_sta_buf) + rtw_vmfree(pmlmepriv->free_unassoc_sta_buf, pmlmepriv->max_unassoc_sta_cnt * sizeof(struct unassoc_sta_info)); + } +#ifdef CONFIG_PLATFORM_CMAP_INTFS + _cancel_timer_ex(&pmlmepriv->cmap_unassoc_sta_timer); +#endif +#endif + } +exit: + return; +} + +sint _rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork) +{ + _irqL irqL; + + + if (pnetwork == NULL) + goto exit; + + _enter_critical_bh(&queue->lock, &irqL); + + rtw_list_insert_tail(&pnetwork->list, &queue->queue); + + _exit_critical_bh(&queue->lock, &irqL); + +exit: + + + return _SUCCESS; +} + +/* +struct wlan_network *_rtw_dequeue_network(_queue *queue) +{ + _irqL irqL; + + struct wlan_network *pnetwork; + + + _enter_critical_bh(&queue->lock, &irqL); + + if (_rtw_queue_empty(queue) == _TRUE) + + pnetwork = NULL; + + else + { + pnetwork = LIST_CONTAINOR(get_next(&queue->queue), struct wlan_network, list); + + rtw_list_delete(&(pnetwork->list)); + } + + _exit_critical_bh(&queue->lock, &irqL); + + + return pnetwork; +} +*/ + +struct wlan_network *_rtw_alloc_network(struct mlme_priv *pmlmepriv) /* (_queue *free_queue) */ +{ + _irqL irqL; + struct wlan_network *pnetwork; + _queue *free_queue = &pmlmepriv->free_bss_pool; + _list *plist = NULL; + + + _enter_critical_bh(&free_queue->lock, &irqL); + + if (_rtw_queue_empty(free_queue) == _TRUE) { + pnetwork = NULL; + goto exit; + } + plist = get_next(&(free_queue->queue)); + + pnetwork = LIST_CONTAINOR(plist , struct wlan_network, list); + + rtw_list_delete(&pnetwork->list); + + pnetwork->network_type = 0; + pnetwork->fixed = _FALSE; + pnetwork->last_scanned = rtw_get_current_time(); + pnetwork->last_non_hidden_ssid_ap = pnetwork->last_scanned; +#if defined(CONFIG_RTW_MESH) && CONFIG_RTW_MESH_ACNODE_PREVENT + pnetwork->acnode_stime = 0; + pnetwork->acnode_notify_etime = 0; +#endif + + pnetwork->aid = 0; + pnetwork->join_res = 0; + + pmlmepriv->num_of_scanned++; + +exit: + _exit_critical_bh(&free_queue->lock, &irqL); + + + return pnetwork; +} + +void _rtw_free_network(struct mlme_priv *pmlmepriv , struct wlan_network *pnetwork, u8 isfreeall) +{ + u32 delta_time; + u32 lifetime = SCANQUEUE_LIFETIME; + _irqL irqL; + _queue *free_queue = &(pmlmepriv->free_bss_pool); + + + if (pnetwork == NULL) + goto exit; + + if (pnetwork->fixed == _TRUE) + goto exit; + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE)) + lifetime = 1; + + if (!isfreeall) { + delta_time = (u32) rtw_get_passing_time_ms(pnetwork->last_scanned); + if (delta_time < lifetime) /* unit:msec */ + goto exit; + } + + _enter_critical_bh(&free_queue->lock, &irqL); + + rtw_list_delete(&(pnetwork->list)); + + rtw_list_insert_tail(&(pnetwork->list), &(free_queue->queue)); + + pmlmepriv->num_of_scanned--; + + + /* RTW_INFO("_rtw_free_network:SSID=%s\n", pnetwork->network.Ssid.Ssid); */ + + _exit_critical_bh(&free_queue->lock, &irqL); + +exit: + return; +} + +void _rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork) +{ + + _queue *free_queue = &(pmlmepriv->free_bss_pool); + + + if (pnetwork == NULL) + goto exit; + + if (pnetwork->fixed == _TRUE) + goto exit; + + /* _enter_critical(&free_queue->lock, &irqL); */ + + rtw_list_delete(&(pnetwork->list)); + + rtw_list_insert_tail(&(pnetwork->list), get_list_head(free_queue)); + + pmlmepriv->num_of_scanned--; + + /* _exit_critical(&free_queue->lock, &irqL); */ + +exit: + return; +} + +void _rtw_free_network_queue(_adapter *padapter, u8 isfreeall) +{ + _irqL irqL; + _list *phead, *plist; + struct wlan_network *pnetwork; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + _queue *scanned_queue = &pmlmepriv->scanned_queue; + + + + _enter_critical_bh(&scanned_queue->lock, &irqL); + + phead = get_list_head(scanned_queue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + plist = get_next(plist); + + _rtw_free_network(pmlmepriv, pnetwork, isfreeall); + + } + + _exit_critical_bh(&scanned_queue->lock, &irqL); + + +} + + + + +sint rtw_if_up(_adapter *padapter) +{ + + sint res; + + if (RTW_CANNOT_RUN(padapter) || + (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _FALSE)) { + res = _FALSE; + } else + res = _TRUE; + + return res; +} + + +void rtw_generate_random_ibss(u8 *pibss) +{ + *((u32 *)(&pibss[2])) = rtw_random32(); + pibss[0] = 0x02; /* in ad-hoc mode local bit must set to 1 */ + pibss[1] = 0x11; + pibss[2] = 0x87; +} + +u8 *rtw_get_capability_from_ie(u8 *ie) +{ + return ie + 8 + 2; +} + + +u16 rtw_get_capability(WLAN_BSSID_EX *bss) +{ + u16 val; + + _rtw_memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->IEs), 2); + + return le16_to_cpu(val); +} + +u8 *rtw_get_timestampe_from_ie(u8 *ie) +{ + return ie + 0; +} + +u8 *rtw_get_beacon_interval_from_ie(u8 *ie) +{ + return ie + 8; +} + + +int rtw_init_mlme_priv(_adapter *padapter) /* (struct mlme_priv *pmlmepriv) */ +{ + int res; + res = _rtw_init_mlme_priv(padapter);/* (pmlmepriv); */ + return res; +} + +void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv) +{ + _rtw_free_mlme_priv(pmlmepriv); +} + +int rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork); +int rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork) +{ + int res; + res = _rtw_enqueue_network(queue, pnetwork); + return res; +} + +/* +static struct wlan_network *rtw_dequeue_network(_queue *queue) +{ + struct wlan_network *pnetwork; + pnetwork = _rtw_dequeue_network(queue); + return pnetwork; +} +*/ + +struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv); +struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv) /* (_queue *free_queue) */ +{ + struct wlan_network *pnetwork; + pnetwork = _rtw_alloc_network(pmlmepriv); + return pnetwork; +} + +void rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 is_freeall); +void rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 is_freeall)/* (struct wlan_network *pnetwork, _queue *free_queue) */ +{ + _rtw_free_network(pmlmepriv, pnetwork, is_freeall); +} + +void rtw_free_network_nolock(_adapter *padapter, struct wlan_network *pnetwork); +void rtw_free_network_nolock(_adapter *padapter, struct wlan_network *pnetwork) +{ + _rtw_free_network_nolock(&(padapter->mlmepriv), pnetwork); +#ifdef CONFIG_IOCTL_CFG80211 + rtw_cfg80211_unlink_bss(padapter, pnetwork); +#endif /* CONFIG_IOCTL_CFG80211 */ +} + + +void rtw_free_network_queue(_adapter *dev, u8 isfreeall) +{ + _rtw_free_network_queue(dev, isfreeall); +} + +struct wlan_network *_rtw_find_network(_queue *scanned_queue, const u8 *addr) +{ + _list *phead, *plist; + struct wlan_network *pnetwork = NULL; + u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0}; + + if (_rtw_memcmp(zero_addr, addr, ETH_ALEN)) { + pnetwork = NULL; + goto exit; + } + + phead = get_list_head(scanned_queue); + plist = get_next(phead); + + while (plist != phead) { + pnetwork = LIST_CONTAINOR(plist, struct wlan_network , list); + + if (_rtw_memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN) == _TRUE) + break; + + plist = get_next(plist); + } + + if (plist == phead) + pnetwork = NULL; + +exit: + return pnetwork; +} + +struct wlan_network *rtw_find_network(_queue *scanned_queue, const u8 *addr) +{ + struct wlan_network *pnetwork; + _irqL irqL; + + _enter_critical_bh(&scanned_queue->lock, &irqL); + pnetwork = _rtw_find_network(scanned_queue, addr); + _exit_critical_bh(&scanned_queue->lock, &irqL); + + return pnetwork; +} + +int rtw_is_same_ibss(_adapter *adapter, struct wlan_network *pnetwork) +{ + int ret = _TRUE; + struct security_priv *psecuritypriv = &adapter->securitypriv; + + if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) && + (pnetwork->network.Privacy == 0)) + ret = _FALSE; + else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) && + (pnetwork->network.Privacy == 1)) + ret = _FALSE; + else + ret = _TRUE; + + return ret; + +} + +inline int is_same_ess(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b) +{ + return (a->Ssid.SsidLength == b->Ssid.SsidLength) + && _rtw_memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength) == _TRUE; +} + +int is_same_network(WLAN_BSSID_EX *src, WLAN_BSSID_EX *dst, u8 feature) +{ + u16 s_cap, d_cap; + + + if (rtw_bug_check(dst, src, &s_cap, &d_cap) == _FALSE) + return _FALSE; + + _rtw_memcpy((u8 *)&s_cap, rtw_get_capability_from_ie(src->IEs), 2); + _rtw_memcpy((u8 *)&d_cap, rtw_get_capability_from_ie(dst->IEs), 2); + + + s_cap = le16_to_cpu(s_cap); + d_cap = le16_to_cpu(d_cap); + + +#ifdef CONFIG_P2P + if ((feature == 1) && /* 1: P2P supported */ + (_rtw_memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN) == _TRUE) + ) + return _TRUE; +#endif + + /* Wi-Fi driver doesn't consider the situation of BCN and ProbRsp sent from the same hidden AP, + * it considers these two packets are sent from different AP. + * Therefore, the scan queue may store two scan results of the same hidden AP, likes below. + * + * index bssid ch RSSI SdBm Noise age flag ssid + * 1 00:e0:4c:55:50:01 153 -73 -73 0 7044 [WPS][ESS] RTK5G + * 3 00:e0:4c:55:50:01 153 -73 -73 0 7044 [WPS][ESS] + * + * Original rules will compare Ssid, SsidLength, MacAddress, s_cap, d_cap at the same time. + * Wi-Fi driver will assume that the BCN and ProbRsp sent from the same hidden AP are the same network + * after we add an additional rule to compare SsidLength and Ssid. + * It means the scan queue will not store two scan results of the same hidden AP, it only store ProbRsp. + * For customer request. + */ + + if (((_rtw_memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN)) == _TRUE) && + ((s_cap & WLAN_CAPABILITY_IBSS) == (d_cap & WLAN_CAPABILITY_IBSS)) && + ((s_cap & WLAN_CAPABILITY_BSS) == (d_cap & WLAN_CAPABILITY_BSS))) { + if ((src->Ssid.SsidLength == dst->Ssid.SsidLength) && + (((_rtw_memcmp(src->Ssid.Ssid, dst->Ssid.Ssid, src->Ssid.SsidLength)) == _TRUE) || //Case of normal AP + (is_all_null(src->Ssid.Ssid, src->Ssid.SsidLength) == _TRUE || is_all_null(dst->Ssid.Ssid, dst->Ssid.SsidLength) == _TRUE))) //Case of hidden AP + return _TRUE; + else if ((src->Ssid.SsidLength == 0 || dst->Ssid.SsidLength == 0)) //Case of hidden AP + return _TRUE; + else + return _FALSE; + } else { + return _FALSE; + } +} + +struct wlan_network *_rtw_find_same_network(_queue *scanned_queue, struct wlan_network *network) +{ + _list *phead, *plist; + struct wlan_network *found = NULL; + + phead = get_list_head(scanned_queue); + plist = get_next(phead); + + while (plist != phead) { + found = LIST_CONTAINOR(plist, struct wlan_network , list); + + if (is_same_network(&network->network, &found->network, 0)) + break; + + plist = get_next(plist); + } + + if (plist == phead) + found = NULL; + + return found; +} + +struct wlan_network *rtw_find_same_network(_queue *scanned_queue, struct wlan_network *network) +{ + _irqL irqL; + struct wlan_network *found = NULL; + + if (scanned_queue == NULL || network == NULL) + goto exit; + + _enter_critical_bh(&scanned_queue->lock, &irqL); + found = _rtw_find_same_network(scanned_queue, network); + _exit_critical_bh(&scanned_queue->lock, &irqL); + +exit: + return found; +} + +struct wlan_network *rtw_get_oldest_wlan_network(_queue *scanned_queue) +{ + _list *plist, *phead; + + + struct wlan_network *pwlan = NULL; + struct wlan_network *oldest = NULL; + phead = get_list_head(scanned_queue); + + plist = get_next(phead); + + while (1) { + + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pwlan = LIST_CONTAINOR(plist, struct wlan_network, list); + + if (pwlan->fixed != _TRUE) { + if (oldest == NULL || rtw_time_after(oldest->last_scanned, pwlan->last_scanned)) + oldest = pwlan; + } + + plist = get_next(plist); + } + return oldest; + +} + +void update_network(WLAN_BSSID_EX *dst, WLAN_BSSID_EX *src, + _adapter *padapter, bool update_ie) +{ +#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1 + u8 ss_ori = dst->PhyInfo.SignalStrength; + u8 sq_ori = dst->PhyInfo.SignalQuality; + u8 ss_smp = src->PhyInfo.SignalStrength; + long rssi_smp = src->Rssi; +#endif + long rssi_ori = dst->Rssi; + + u8 sq_smp = src->PhyInfo.SignalQuality; + u8 ss_final; + u8 sq_final; + long rssi_final; + + +#ifdef CONFIG_ANTENNA_DIVERSITY + rtw_hal_antdiv_rssi_compared(padapter, dst, src); /* this will update src.Rssi, need consider again */ +#endif + +#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1 + if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) { + RTW_INFO(FUNC_ADPT_FMT" %s("MAC_FMT", ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n" + , FUNC_ADPT_ARG(padapter) + , src->Ssid.Ssid, MAC_ARG(src->MacAddress), src->Configuration.DSConfig + , ss_ori, sq_ori, rssi_ori + , ss_smp, sq_smp, rssi_smp + ); + } +#endif + + /* The rule below is 1/5 for sample value, 4/5 for history value */ + if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) && is_same_network(&(padapter->mlmepriv.cur_network.network), src, 0)) { + /* Take the recvpriv's value for the connected AP*/ + ss_final = padapter->recvpriv.signal_strength; + sq_final = padapter->recvpriv.signal_qual; + /* the rssi value here is undecorated, and will be used for antenna diversity */ + if (sq_smp != 101) /* from the right channel */ + rssi_final = (src->Rssi + dst->Rssi * 4) / 5; + else + rssi_final = rssi_ori; + } else { + if (sq_smp != 101) { /* from the right channel */ + ss_final = ((u32)(src->PhyInfo.SignalStrength) + (u32)(dst->PhyInfo.SignalStrength) * 4) / 5; + sq_final = ((u32)(src->PhyInfo.SignalQuality) + (u32)(dst->PhyInfo.SignalQuality) * 4) / 5; + rssi_final = (src->Rssi + dst->Rssi * 4) / 5; + } else { + /* bss info not receving from the right channel, use the original RX signal infos */ + ss_final = dst->PhyInfo.SignalStrength; + sq_final = dst->PhyInfo.SignalQuality; + rssi_final = dst->Rssi; + } + + } + + if (update_ie) { + dst->Reserved[0] = src->Reserved[0]; + dst->Reserved[1] = src->Reserved[1]; + _rtw_memcpy((u8 *)dst, (u8 *)src, get_WLAN_BSSID_EX_sz(src)); + } +#ifdef CONFIG_LAYER2_ROAMING + dst->tsf = src->tsf; +#endif + dst->PhyInfo.SignalStrength = ss_final; + dst->PhyInfo.SignalQuality = sq_final; + dst->Rssi = rssi_final; + +#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1 + if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) { + RTW_INFO(FUNC_ADPT_FMT" %s("MAC_FMT"), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n" + , FUNC_ADPT_ARG(padapter) + , dst->Ssid.Ssid, MAC_ARG(dst->MacAddress), dst->PhyInfo.SignalStrength, dst->PhyInfo.SignalQuality, dst->Rssi); + } +#endif + +#if 0 /* old codes, may be useful one day... + * RTW_INFO("update_network: rssi=0x%lx dst->Rssi=%d ,dst->Rssi=0x%lx , src->Rssi=0x%lx",(dst->Rssi+src->Rssi)/2,dst->Rssi,dst->Rssi,src->Rssi); */ + if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) && is_same_network(&(padapter->mlmepriv.cur_network.network), src)) { + + /* RTW_INFO("b:ssid=%s update_network: src->rssi=0x%d padapter->recvpriv.ui_rssi=%d\n",src->Ssid.Ssid,src->Rssi,padapter->recvpriv.signal); */ + if (padapter->recvpriv.signal_qual_data.total_num++ >= PHY_LINKQUALITY_SLID_WIN_MAX) { + padapter->recvpriv.signal_qual_data.total_num = PHY_LINKQUALITY_SLID_WIN_MAX; + last_evm = padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index]; + padapter->recvpriv.signal_qual_data.total_val -= last_evm; + } + padapter->recvpriv.signal_qual_data.total_val += query_rx_pwr_percentage(src->Rssi); + + padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index++] = query_rx_pwr_percentage(src->Rssi); + if (padapter->recvpriv.signal_qual_data.index >= PHY_LINKQUALITY_SLID_WIN_MAX) + padapter->recvpriv.signal_qual_data.index = 0; + + /* RTW_INFO("Total SQ=%d pattrib->signal_qual= %d\n", padapter->recvpriv.signal_qual_data.total_val, src->Rssi); */ + + /* <1> Showed on UI for user,in percentage. */ + tmpVal = padapter->recvpriv.signal_qual_data.total_val / padapter->recvpriv.signal_qual_data.total_num; + padapter->recvpriv.signal = (u8)tmpVal; /* Link quality */ + + src->Rssi = translate_percentage_to_dbm(padapter->recvpriv.signal) ; + } else { + /* RTW_INFO("ELSE:ssid=%s update_network: src->rssi=0x%d dst->rssi=%d\n",src->Ssid.Ssid,src->Rssi,dst->Rssi); */ + src->Rssi = (src->Rssi + dst->Rssi) / 2; /* dBM */ + } + + /* RTW_INFO("a:update_network: src->rssi=0x%d padapter->recvpriv.ui_rssi=%d\n",src->Rssi,padapter->recvpriv.signal); */ + +#endif + +} + +static void update_current_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork) +{ + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + + rtw_bug_check(&(pmlmepriv->cur_network.network), + &(pmlmepriv->cur_network.network), + &(pmlmepriv->cur_network.network), + &(pmlmepriv->cur_network.network)); + + if ((check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) && (is_same_network(&(pmlmepriv->cur_network.network), pnetwork, 0))) { + + /* if(pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */ + { + update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, _TRUE); + rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(NDIS_802_11_FIXED_IEs), + pmlmepriv->cur_network.network.IELength); + } + } + + +} + + +/* + +Caller must hold pmlmepriv->lock first. + + +*/ +bool rtw_update_scanned_network(_adapter *adapter, WLAN_BSSID_EX *target) +{ + _irqL irqL; + _list *plist, *phead; + u32 bssid_ex_sz; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(adapter->wdinfo); +#endif /* CONFIG_P2P */ + _queue *queue = &(pmlmepriv->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct wlan_network *choice = NULL; + int target_find = 0; + u8 feature = 0; + bool update_ie = _FALSE; + + _enter_critical_bh(&queue->lock, &irqL); + phead = get_list_head(queue); + plist = get_next(phead); + +#if 0 + RTW_INFO("%s => ssid:%s , rssi:%ld , ss:%d\n", + __func__, target->Ssid.Ssid, target->Rssi, target->PhyInfo.SignalStrength); +#endif + +#ifdef CONFIG_P2P + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + feature = 1; /* p2p enable */ +#endif + + while (1) { + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + rtw_bug_check(pnetwork, pnetwork, pnetwork, pnetwork); + +#ifdef CONFIG_P2P + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && + (_rtw_memcmp(pnetwork->network.MacAddress, target->MacAddress, ETH_ALEN) == _TRUE)) { + target_find = 1; + break; + } +#endif + + if (is_same_network(&(pnetwork->network), target, feature)) { + target_find = 1; + break; + } + + if (rtw_roam_flags(adapter)) { + /* TODO: don't select netowrk in the same ess as choice if it's new enough*/ + } + if (pnetwork->fixed) { + plist = get_next(plist); + continue; + } + +#ifdef CONFIG_RSSI_PRIORITY + if ((choice == NULL) || (pnetwork->network.PhyInfo.SignalStrength < choice->network.PhyInfo.SignalStrength)) + #ifdef CONFIG_RTW_MESH + if (!MLME_IS_MESH(adapter) || !MLME_IS_ASOC(adapter) + || !rtw_bss_is_same_mbss(&pmlmepriv->cur_network.network, &pnetwork->network)) + #endif + choice = pnetwork; +#else + if (choice == NULL || rtw_time_after(choice->last_scanned, pnetwork->last_scanned)) + #ifdef CONFIG_RTW_MESH + if (!MLME_IS_MESH(adapter) || !MLME_IS_ASOC(adapter) + || !rtw_bss_is_same_mbss(&pmlmepriv->cur_network.network, &pnetwork->network)) + #endif + choice = pnetwork; +#endif + plist = get_next(plist); + + } + + + /* If we didn't find a match, then get a new network slot to initialize + * with this beacon's information */ + /* if (rtw_end_of_queue_search(phead,plist)== _TRUE) { */ + if (!target_find) { + if (_rtw_queue_empty(&(pmlmepriv->free_bss_pool)) == _TRUE) { + /* If there are no more slots, expire the choice */ + /* list_del_init(&choice->list); */ + pnetwork = choice; + if (pnetwork == NULL) + goto unlock_scan_queue; + +#ifdef CONFIG_RSSI_PRIORITY + RTW_DBG("%s => ssid:%s ,bssid:"MAC_FMT" will be deleted from scanned_queue (rssi:%ld , ss:%d)\n", + __func__, pnetwork->network.Ssid.Ssid, MAC_ARG(pnetwork->network.MacAddress), pnetwork->network.Rssi, pnetwork->network.PhyInfo.SignalStrength); +#else + RTW_DBG("%s => ssid:%s ,bssid:"MAC_FMT" will be deleted from scanned_queue\n", + __func__, pnetwork->network.Ssid.Ssid, MAC_ARG(pnetwork->network.MacAddress)); +#endif + +#ifdef CONFIG_ANTENNA_DIVERSITY + rtw_hal_get_odm_var(adapter, HAL_ODM_ANTDIV_SELECT, &(target->PhyInfo.Optimum_antenna), NULL); +#endif + _rtw_memcpy(&(pnetwork->network), target, get_WLAN_BSSID_EX_sz(target)); + pnetwork->bcn_keys_valid = 0; + if (target->Reserved[0] == BSS_TYPE_BCN || target->Reserved[0] == BSS_TYPE_PROB_RSP) + rtw_update_bcn_keys_of_network(pnetwork); + /* variable initialize */ + pnetwork->fixed = _FALSE; + pnetwork->last_scanned = rtw_get_current_time(); + pnetwork->last_non_hidden_ssid_ap = pnetwork->last_scanned; + #if defined(CONFIG_RTW_MESH) && CONFIG_RTW_MESH_ACNODE_PREVENT + pnetwork->acnode_stime = 0; + pnetwork->acnode_notify_etime = 0; + #endif +#ifdef CONFIG_LAYER2_ROAMING + pnetwork->network.tsf = target->tsf; +#endif + pnetwork->network_type = 0; + pnetwork->aid = 0; + pnetwork->join_res = 0; + + /* bss info not receving from the right channel */ + if (pnetwork->network.PhyInfo.SignalQuality == 101) + pnetwork->network.PhyInfo.SignalQuality = 0; + } else { + /* Otherwise just pull from the free list */ + + pnetwork = rtw_alloc_network(pmlmepriv); /* will update scan_time */ + if (pnetwork == NULL) + goto unlock_scan_queue; + + bssid_ex_sz = get_WLAN_BSSID_EX_sz(target); + target->Length = bssid_ex_sz; +#ifdef CONFIG_ANTENNA_DIVERSITY + rtw_hal_get_odm_var(adapter, HAL_ODM_ANTDIV_SELECT, &(target->PhyInfo.Optimum_antenna), NULL); +#endif + _rtw_memcpy(&(pnetwork->network), target, bssid_ex_sz); + pnetwork->bcn_keys_valid = 0; + if (target->Reserved[0] == BSS_TYPE_BCN || target->Reserved[0] == BSS_TYPE_PROB_RSP) + rtw_update_bcn_keys_of_network(pnetwork); + + /* bss info not receving from the right channel */ + if (pnetwork->network.PhyInfo.SignalQuality == 101) + pnetwork->network.PhyInfo.SignalQuality = 0; + + rtw_list_insert_tail(&(pnetwork->list), &(queue->queue)); + + } + } else { + /* we have an entry and we are going to update it. But this entry may + * be already expired. In this case we do the same as we found a new + * net and call the new_net handler + */ + #if defined(CONFIG_RTW_MESH) && CONFIG_RTW_MESH_ACNODE_PREVENT + systime last_scanned = pnetwork->last_scanned; + #endif + struct beacon_keys bcn_keys; + bool bcn_keys_valid = 0; + bool is_hidden_ssid_ap = 0; + + pnetwork->last_scanned = rtw_get_current_time(); + + if (target->Reserved[0] == BSS_TYPE_BCN || target->Reserved[0] == BSS_TYPE_PROB_RSP) { + if (target->InfrastructureMode == Ndis802_11Infrastructure) { + is_hidden_ssid_ap = hidden_ssid_ap(target); + if (!is_hidden_ssid_ap) /* update last time it's non hidden ssid AP */ + pnetwork->last_non_hidden_ssid_ap = rtw_get_current_time(); + } + bcn_keys_valid = rtw_get_bcn_keys_from_bss(target, &bcn_keys); + } + + if (target->InfrastructureMode == Ndis802_11_mesh + || target->Reserved[0] >= pnetwork->network.Reserved[0]) + update_ie = _TRUE; + else if (target->InfrastructureMode == Ndis802_11Infrastructure && !pnetwork->fixed + && rtw_get_passing_time_ms(pnetwork->last_non_hidden_ssid_ap) > SCANQUEUE_LIFETIME) + update_ie = _TRUE; + else if (bcn_keys_valid) { + if (is_hidden_ssid(bcn_keys.ssid, bcn_keys.ssid_len)) { + /* hidden ssid, replace with current beacon ssid directly */ + _rtw_memcpy(bcn_keys.ssid, pnetwork->bcn_keys.ssid, pnetwork->bcn_keys.ssid_len); + bcn_keys.ssid_len = pnetwork->bcn_keys.ssid_len; + } + if (rtw_bcn_key_compare(&pnetwork->bcn_keys, &bcn_keys) == _FALSE) + update_ie = _TRUE; + } + + #if defined(CONFIG_RTW_MESH) && CONFIG_RTW_MESH_ACNODE_PREVENT + if (!MLME_IS_MESH(adapter) || !MLME_IS_ASOC(adapter) + || pnetwork->network.Configuration.DSConfig != target->Configuration.DSConfig + || rtw_get_passing_time_ms(last_scanned) > adapter->mesh_cfg.peer_sel_policy.scanr_exp_ms + || !rtw_bss_is_same_mbss(&pnetwork->network, target) + ) { + pnetwork->acnode_stime = 0; + pnetwork->acnode_notify_etime = 0; + } + #endif + + if (bcn_keys_valid) { + _rtw_memcpy(&pnetwork->bcn_keys, &bcn_keys, sizeof(bcn_keys)); + pnetwork->bcn_keys_valid = 1; + } else if (update_ie) + pnetwork->bcn_keys_valid = 0; + + update_network(&(pnetwork->network), target, adapter, update_ie); + } + + #if defined(CONFIG_RTW_MESH) && CONFIG_RTW_MESH_ACNODE_PREVENT + if (MLME_IS_MESH(adapter) && MLME_IS_ASOC(adapter)) + rtw_mesh_update_scanned_acnode_status(adapter, pnetwork); + #endif + +unlock_scan_queue: + _exit_critical_bh(&queue->lock, &irqL); + +#ifdef CONFIG_RTW_MESH + if (pnetwork && MLME_IS_MESH(adapter) + && check_fwstate(pmlmepriv, WIFI_ASOC_STATE) + && !check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) + ) + rtw_chk_candidate_peer_notify(adapter, pnetwork); +#endif + + return update_ie; +} + +void rtw_add_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork); +void rtw_add_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork) +{ + bool update_ie; + /* _queue *queue = &(pmlmepriv->scanned_queue); */ + + /* _enter_critical_bh(&queue->lock, &irqL); */ + +#if defined(CONFIG_P2P) && defined(CONFIG_P2P_REMOVE_GROUP_INFO) + if (adapter->registrypriv.wifi_spec == 0) + rtw_bss_ex_del_p2p_attr(pnetwork, P2P_ATTR_GROUP_INFO); +#endif + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + rtw_bss_ex_del_wfd_ie(pnetwork); + /* Wi-Fi driver will update the current network if the scan result of the connected AP be updated by scan. */ + update_ie = rtw_update_scanned_network(adapter, pnetwork); + + if (update_ie) + update_current_network(adapter, pnetwork); + + /* _exit_critical_bh(&queue->lock, &irqL); */ + +} + +#ifdef CONFIG_RTW_MULTI_AP +void rtw_unassoc_sta_set_mode(_adapter *adapter, u8 stype, u8 mode) +{ + if (stype >= UNASOC_STA_SRC_NUM + || mode >= UNASOC_STA_MODE_NUM) + return; + + adapter = GET_PRIMARY_ADAPTER(adapter); + + if (adapter->mlmepriv.unassoc_sta_mode_of_stype[stype] == mode) + return; + + adapter->mlmepriv.unassoc_sta_mode_of_stype[stype] = mode; + + rtw_run_in_thread_cmd_wait(adapter, ((void *)(rtw_hal_rcr_set_chk_bssid_act_non)), adapter, 2000); +} + +bool rtw_unassoc_sta_src_chk(_adapter *adapter, u8 stype) +{ + if (stype >= UNASOC_STA_SRC_NUM) + return 0; + + adapter = GET_PRIMARY_ADAPTER(adapter); + + return adapter->mlmepriv.unassoc_sta_mode_of_stype[stype] == UNASOC_STA_MODE_ALL + || (adapter->mlmepriv.unassoc_sta_mode_of_stype[stype] == UNASOC_STA_MODE_INTERESTED + && adapter->mlmepriv.interested_unassoc_sta_cnt) + ; +} + +const char *unasoc_sta_src_str[] = { + "BMC", + "NMY_UC", +}; + +const char *unasoc_sta_mode_str[] = { + "DISABLED", + "INTERESTED", + "ALL", +}; + +void dump_unassoc_sta(void *sel, _adapter *adapter) +{ + struct mlme_priv *mlmepriv; + _queue *queue; + _list *list, *head; + struct unassoc_sta_info **unassoc_sta_arr; + struct unassoc_sta_info *unassoc_sta; + u16 i, unassoc_sta_cnt = 0; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + for (i = 0; i < UNASOC_STA_SRC_NUM; i++) { + RTW_PRINT_SEL(sel, "[%u]%-6s:%u(%s)\n", i, unasoc_sta_src_str[i] + , mlmepriv->unassoc_sta_mode_of_stype[i], unasoc_sta_mode_str[mlmepriv->unassoc_sta_mode_of_stype[i]]); + } + RTW_PRINT_SEL(sel, "interested_unassoc_sta_cnt:%u\n", mlmepriv->interested_unassoc_sta_cnt); + + unassoc_sta_arr = rtw_zvmalloc(mlmepriv->max_unassoc_sta_cnt * sizeof(struct unassoc_sta_info *)); + if (!unassoc_sta_arr) + return; + + enter_critical_bh(&queue->lock); + head = get_list_head(queue); + list = get_next(head); + + while (rtw_end_of_queue_search(head, list) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list, struct unassoc_sta_info, list); + list = get_next(list); + + unassoc_sta_arr[unassoc_sta_cnt++] = unassoc_sta; + } + + exit_critical_bh(&queue->lock); + + RTW_PRINT_SEL(sel, " %17s %18s %6s\n", "mac_addr", "measure_delta_time", "rssi"); + + for (i = 0; i < unassoc_sta_cnt; i++) { + u8 rcpi; + s8 rx_power; + u32 measure_delta_time; + + unassoc_sta = unassoc_sta_arr[i]; + + measure_delta_time = rtw_systime_to_ms(rtw_get_current_time() - unassoc_sta->time); + + RTW_PRINT_SEL(sel, "%c "MAC_FMT" %18u %6d\n" + , unassoc_sta->interested ? '*' : ' ' + , MAC_ARG(unassoc_sta->addr), measure_delta_time, unassoc_sta->recv_signal_power); + } + + rtw_vmfree(unassoc_sta_arr, mlmepriv->max_unassoc_sta_cnt * sizeof(struct unassoc_sta_info *)); +} + +static void del_unassoc_sta(struct mlme_priv *mlmepriv, struct unassoc_sta_info *unassoc_sta) +{ + _irqL irqL; + _queue *free_queue = &(mlmepriv->free_unassoc_sta_queue); + + if (unassoc_sta->interested) + mlmepriv->interested_unassoc_sta_cnt--; + if (mlmepriv->interested_unassoc_sta_cnt == 0) { + rtw_run_in_thread_cmd(mlme_to_adapter(mlmepriv) + , ((void *)(rtw_hal_rcr_set_chk_bssid_act_non)), mlme_to_adapter(mlmepriv)); + } + + _enter_critical_bh(&free_queue->lock, &irqL); + rtw_list_delete(&(unassoc_sta->list)); + rtw_list_insert_tail(&(unassoc_sta->list), &(free_queue->queue)); + _exit_critical_bh(&free_queue->lock, &irqL); +} + +static u8 del_unassoc_sta_chk(struct mlme_priv *mlmepriv, struct unassoc_sta_info *unassoc_sta) +{ + systime cur, lifetime; + + if (unassoc_sta == NULL) + return UNASOC_STA_DEL_CHK_SKIP; + + if (unassoc_sta->interested) + return UNASOC_STA_DEL_CHK_SKIP; + + cur = rtw_get_current_time(); + lifetime = unassoc_sta->time + rtw_ms_to_systime(UNASSOC_STA_LIFETIME_MS); + if (rtw_time_before(cur, lifetime)) + return UNASOC_STA_DEL_CHK_ALIVE; + + del_unassoc_sta(mlmepriv, unassoc_sta); + + return UNASOC_STA_DEL_CHK_DELETED; +} + +static struct unassoc_sta_info *alloc_unassoc_sta(struct mlme_priv *mlmepriv) +{ + _irqL irqL; + struct unassoc_sta_info *unassoc_sta; + _queue *free_queue = &mlmepriv->free_unassoc_sta_queue; + _list *list = NULL; + + + _enter_critical_bh(&free_queue->lock, &irqL); + + if (_rtw_queue_empty(free_queue) == _TRUE) { + unassoc_sta = NULL; + goto exit; + } + list = get_next(&(free_queue->queue)); + + unassoc_sta = LIST_CONTAINOR(list, struct unassoc_sta_info, list); + + rtw_list_delete(&unassoc_sta->list); + + _rtw_memset(unassoc_sta->addr, 0, ETH_ALEN); + unassoc_sta->recv_signal_power = 0; + unassoc_sta->time = 0; + unassoc_sta->interested = 0; +exit: + _exit_critical_bh(&free_queue->lock, &irqL); + + return unassoc_sta; + +} + +void rtw_del_unassoc_sta_queue(_adapter *adapter) +{ + struct unassoc_sta_info *unassoc_sta; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + del_unassoc_sta(mlmepriv, unassoc_sta); + } + + _exit_critical_bh(&queue->lock, &irqL); + +} + +void rtw_del_unassoc_sta(_adapter *adapter, u8 *addr) +{ + struct unassoc_sta_info *unassoc_sta; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + if (_rtw_memcmp(addr, unassoc_sta->addr, ETH_ALEN) == _TRUE) { + del_unassoc_sta(mlmepriv, unassoc_sta); + goto unlock_unassoc_sta_queue; + } + } + +unlock_unassoc_sta_queue: + _exit_critical_bh(&queue->lock, &irqL); +} + +void rtw_rx_add_unassoc_sta(_adapter *adapter, u8 stype, u8 *addr, s8 recv_signal_power) +{ + struct unassoc_sta_info *unassoc_sta; + struct unassoc_sta_info *oldest_unassoc_sta = NULL; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + if (_rtw_memcmp(addr, unassoc_sta->addr, ETH_ALEN) == _TRUE) { + if (unassoc_sta->interested + || mlmepriv->unassoc_sta_mode_of_stype[stype] >= UNASOC_STA_MODE_ALL + ) { + unassoc_sta->recv_signal_power = recv_signal_power; + unassoc_sta->time = rtw_get_current_time(); + goto unlock_unassoc_sta_queue; + } + } + + if (del_unassoc_sta_chk(mlmepriv, unassoc_sta) == UNASOC_STA_DEL_CHK_ALIVE) { + if (oldest_unassoc_sta == NULL) + oldest_unassoc_sta = unassoc_sta; + else if (rtw_time_before(unassoc_sta->time, oldest_unassoc_sta->time)) + oldest_unassoc_sta = unassoc_sta; + } + } + + if (mlmepriv->unassoc_sta_mode_of_stype[stype] <= UNASOC_STA_MODE_INTERESTED) + goto unlock_unassoc_sta_queue; + + unassoc_sta = alloc_unassoc_sta(mlmepriv); + if (unassoc_sta == NULL) { + if (oldest_unassoc_sta) { + del_unassoc_sta(mlmepriv, oldest_unassoc_sta); + unassoc_sta = alloc_unassoc_sta(mlmepriv); + } else + goto unlock_unassoc_sta_queue; + } + _rtw_memcpy(unassoc_sta->addr, addr, ETH_ALEN); + unassoc_sta->recv_signal_power = recv_signal_power; + unassoc_sta->time = rtw_get_current_time(); + rtw_list_insert_tail(&(unassoc_sta->list), &(queue->queue)); + +unlock_unassoc_sta_queue: + _exit_critical_bh(&queue->lock, &irqL); +} + +void rtw_add_interested_unassoc_sta(_adapter *adapter, u8 *addr) +{ + struct unassoc_sta_info *unassoc_sta; + struct unassoc_sta_info *oldest_unassoc_sta = NULL; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + if (_rtw_memcmp(addr, unassoc_sta->addr, ETH_ALEN) == _TRUE) { + if (!unassoc_sta->interested) { + unassoc_sta->interested = 1; + mlmepriv->interested_unassoc_sta_cnt++; + if (mlmepriv->interested_unassoc_sta_cnt == 1) { + rtw_run_in_thread_cmd(mlme_to_adapter(mlmepriv) + , ((void *)(rtw_hal_rcr_set_chk_bssid_act_non)), mlme_to_adapter(mlmepriv)); + } + } + goto unlock_unassoc_sta_queue; + } + + if (del_unassoc_sta_chk(mlmepriv, unassoc_sta) == UNASOC_STA_DEL_CHK_ALIVE) { + if (oldest_unassoc_sta == NULL) + oldest_unassoc_sta = unassoc_sta; + else if (rtw_time_after(unassoc_sta->time, oldest_unassoc_sta->time)) + oldest_unassoc_sta = unassoc_sta; + } + } + unassoc_sta = alloc_unassoc_sta(mlmepriv); + if (unassoc_sta == NULL) { + RTW_INFO(FUNC_ADPT_FMT": Allocate fail\n", FUNC_ADPT_ARG(adapter)); + if (oldest_unassoc_sta) { + RTW_INFO(FUNC_ADPT_FMT": Delete oldest entry and try again.\n", FUNC_ADPT_ARG(adapter)); + del_unassoc_sta(mlmepriv, oldest_unassoc_sta); + unassoc_sta = alloc_unassoc_sta(mlmepriv); + } else + goto unlock_unassoc_sta_queue; + } + _rtw_memcpy(unassoc_sta->addr, addr, ETH_ALEN); + unassoc_sta->interested = 1; + unassoc_sta->recv_signal_power = 0; + unassoc_sta->time = rtw_get_current_time() - rtw_ms_to_systime(UNASSOC_STA_LIFETIME_MS); + rtw_list_insert_tail(&(unassoc_sta->list), &(queue->queue)); + mlmepriv->interested_unassoc_sta_cnt++; + if (mlmepriv->interested_unassoc_sta_cnt == 1) { + rtw_run_in_thread_cmd(mlme_to_adapter(mlmepriv) + , ((void *)(rtw_hal_rcr_set_chk_bssid_act_non)), mlme_to_adapter(mlmepriv)); + } + +unlock_unassoc_sta_queue: + _exit_critical_bh(&queue->lock, &irqL); +} + +void rtw_undo_interested_unassoc_sta(_adapter *adapter, u8 *addr) +{ + struct unassoc_sta_info *unassoc_sta; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + if (_rtw_memcmp(addr, unassoc_sta->addr, ETH_ALEN) == _TRUE) { + if (unassoc_sta->interested) { + unassoc_sta->interested = 0; + mlmepriv->interested_unassoc_sta_cnt--; + if (mlmepriv->interested_unassoc_sta_cnt == 0) { + rtw_run_in_thread_cmd(mlme_to_adapter(mlmepriv) + , ((void *)(rtw_hal_rcr_set_chk_bssid_act_non)), mlme_to_adapter(mlmepriv)); + } + } + goto unlock_unassoc_sta_queue; + } + } +unlock_unassoc_sta_queue: + _exit_critical_bh(&queue->lock, &irqL); +} + +void rtw_undo_all_interested_unassoc_sta(_adapter *adapter) +{ + struct unassoc_sta_info *unassoc_sta; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + if (unassoc_sta->interested) { + unassoc_sta->interested = 0; + mlmepriv->interested_unassoc_sta_cnt--; + if (mlmepriv->interested_unassoc_sta_cnt == 0) { + rtw_run_in_thread_cmd(mlme_to_adapter(mlmepriv) + , ((void *)(rtw_hal_rcr_set_chk_bssid_act_non)), mlme_to_adapter(mlmepriv)); + goto unlock_unassoc_sta_queue; + } + } + } +unlock_unassoc_sta_queue: + _exit_critical_bh(&queue->lock, &irqL); +} + +u8 rtw_search_unassoc_sta(_adapter *adapter, u8 *addr, struct unassoc_sta_info *ret_sta) +{ + struct unassoc_sta_info *unassoc_sta = NULL; + struct mlme_priv *mlmepriv; + _queue *queue; + _irqL irqL; + _list *head, *list; + u8 searched = 0; + + adapter = GET_PRIMARY_ADAPTER(adapter); + mlmepriv = &(adapter->mlmepriv); + queue = &(mlmepriv->unassoc_sta_queue); + + _enter_critical_bh(&queue->lock, &irqL); + head = get_list_head(queue); + list = get_next(head); + + while ((rtw_end_of_queue_search(head, list)) == _FALSE) { + unassoc_sta = LIST_CONTAINOR(list , struct unassoc_sta_info, list); + list = get_next(list); + + if (_rtw_memcmp(addr, unassoc_sta->addr, ETH_ALEN) == _TRUE) { + memcpy(ret_sta, unassoc_sta, sizeof(struct unassoc_sta_info)); + searched = 1; + break; + } + } + _exit_critical_bh(&queue->lock, &irqL); + + return searched; +} +#endif /* CONFIG_RTW_MULTI_AP */ + +/* select the desired network based on the capability of the (i)bss. + * check items: (1) security + * (2) network_type + * (3) WMM + * (4) HT + * (5) others */ +int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork); +int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork) +{ + struct security_priv *psecuritypriv = &adapter->securitypriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u32 desired_encmode; + u32 privacy; + + /* u8 wps_ie[512]; */ + uint wps_ielen; + + int bselected = _TRUE; + + desired_encmode = psecuritypriv->ndisencryptstatus; + privacy = pnetwork->network.Privacy; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + if (rtw_get_wps_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, pnetwork->network.IELength - _FIXED_IE_LENGTH_, NULL, &wps_ielen) != NULL) + return _TRUE; + else + return _FALSE; + } + if (adapter->registrypriv.wifi_spec == 1) { /* for correct flow of 8021X to do.... */ + u8 *p = NULL; + uint ie_len = 0; + + if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0)) + bselected = _FALSE; + + if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPA2PSK) { + p = rtw_get_ie(pnetwork->network.IEs + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pnetwork->network.IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) + bselected = _TRUE; + else + bselected = _FALSE; + } + } + + + if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) { + RTW_INFO("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy); + bselected = _FALSE; + } + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) { + if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode) + bselected = _FALSE; + } + +#ifdef CONFIG_RTW_MBO + if (rtw_mbo_disallowed_network(pnetwork) == _TRUE) + bselected = _FALSE; +#endif + + return bselected; +} + +#ifdef CONFIG_80211D +static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct registry_priv *pregistrypriv; + struct mlme_ext_priv *pmlmeext; + RT_CHANNEL_INFO *chplan_new; + u8 channel; + u8 i; + + + pregistrypriv = &padapter->registrypriv; + pmlmeext = &padapter->mlmeextpriv; + + /* Adjust channel plan by AP Country IE */ + if (pregistrypriv->enable80211d + && (!pmlmeext->update_channel_plan_by_ap_done)) { + u8 *ie, *p; + u32 len; + RT_CHANNEL_PLAN chplan_ap; + RT_CHANNEL_INFO *chplan_sta = NULL; + u8 country[4]; + u8 fcn; /* first channel number */ + u8 noc; /* number of channel */ + u8 j, k; + + ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_); + if (!ie) + return; + if (len < 6) + return; + + ie += 2; + p = ie; + ie += len; + + _rtw_memset(country, 0, 4); + _rtw_memcpy(country, p, 3); + p += 3; + RTW_INFO("%s: 802.11d country=%s\n", __FUNCTION__, country); + + i = 0; + while ((ie - p) >= 3) { + fcn = *(p++); + noc = *(p++); + p++; + + for (j = 0; j < noc; j++) { + if (fcn <= 14) + channel = fcn + j; /* 2.4 GHz */ + else + channel = fcn + j * 4; /* 5 GHz */ + + chplan_ap.Channel[i++] = channel; + } + } + chplan_ap.Len = i; + +#ifdef CONFIG_RTW_DEBUG + i = 0; + RTW_INFO("%s: AP[%s] channel plan {", __FUNCTION__, bssid->Ssid.Ssid); + while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0)) { + _RTW_INFO("%02d,", chplan_ap.Channel[i]); + i++; + } + _RTW_INFO("}\n"); +#endif + + chplan_sta = rtw_malloc(sizeof(RT_CHANNEL_INFO) * MAX_CHANNEL_NUM); + if (!chplan_sta) + goto done_update_chplan_from_ap; + + _rtw_memcpy(chplan_sta, rfctl->channel_set, sizeof(RT_CHANNEL_INFO) * MAX_CHANNEL_NUM); +#ifdef CONFIG_RTW_DEBUG + i = 0; + RTW_INFO("%s: STA channel plan {", __FUNCTION__); + while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) { + _RTW_INFO("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].flags & RTW_CHF_NO_IR ? 'p' : 'a'); + i++; + } + _RTW_INFO("}\n"); +#endif + + _rtw_memset(rfctl->channel_set, 0, sizeof(rfctl->channel_set)); + chplan_new = rfctl->channel_set; + + i = j = k = 0; + if (pregistrypriv->wireless_mode & WIRELESS_11G) { + do { + if ((i == MAX_CHANNEL_NUM) + || (chplan_sta[i].ChannelNum == 0) + || (chplan_sta[i].ChannelNum > 14)) + break; + + if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14)) + break; + + if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + i++; + j++; + k++; + } else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) { + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; +#if 0 + if (chplan_sta[i].flags & RTW_CHF_NO_IR) + chplan_new[k].flags |= RTW_CHF_NO_IR; +#else + chplan_new[k].flags |= RTW_CHF_NO_IR; +#endif + i++; + k++; + } else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + j++; + k++; + } + } while (1); + + /* change AP not support channel to Passive scan */ + while ((i < MAX_CHANNEL_NUM) + && (chplan_sta[i].ChannelNum != 0) + && (chplan_sta[i].ChannelNum <= 14)) { + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; +#if 0 + if (chplan_sta[i].flags & RTW_CHF_NO_IR) + chplan_new[k].flags |= RTW_CHF_NO_IR; +#else + chplan_new[k].flags |= RTW_CHF_NO_IR; +#endif + i++; + k++; + } + + /* add channel AP supported */ + while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + j++; + k++; + } + } else { + /* keep original STA 2.4G channel plan */ + while ((i < MAX_CHANNEL_NUM) + && (chplan_sta[i].ChannelNum != 0) + && (chplan_sta[i].ChannelNum <= 14)) { + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; + if (chplan_sta[i].flags & RTW_CHF_NO_IR) + chplan_new[k].flags |= RTW_CHF_NO_IR; + i++; + k++; + } + + /* skip AP 2.4G channel plan */ + while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) + j++; + } + + if (pregistrypriv->wireless_mode & WIRELESS_11A) { + do { + if ((i >= MAX_CHANNEL_NUM) + || (chplan_sta[i].ChannelNum == 0)) + break; + + if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0)) + break; + + if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + i++; + j++; + k++; + } else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) { + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; +#if 0 + if (chplan_sta[i].flags & RTW_CHF_NO_IR) + chplan_new[k].flags |= RTW_CHF_NO_IR; +#else + chplan_new[k].flags |= RTW_CHF_NO_IR; +#endif + i++; + k++; + } else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + j++; + k++; + } + } while (1); + + /* change AP not support channel to Passive scan */ + while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) { + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; +#if 0 + if (chplan_sta[i].flags & RTW_CHF_NO_IR) + chplan_new[k].flags |= RTW_CHF_NO_IR; +#else + chplan_new[k].flags |= RTW_CHF_NO_IR; +#endif + i++; + k++; + } + + /* add channel AP supported */ + while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0)) { + chplan_new[k].ChannelNum = chplan_ap.Channel[j]; + j++; + k++; + } + } else { + /* keep original STA 5G channel plan */ + while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) { + chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum; + if (chplan_sta[i].flags & RTW_CHF_NO_IR) + chplan_new[k].flags |= RTW_CHF_NO_IR; + i++; + k++; + } + } + + pmlmeext->update_channel_plan_by_ap_done = 1; + rtw_nlrtw_reg_change_event(padapter); + +#ifdef CONFIG_RTW_DEBUG + k = 0; + RTW_INFO("%s: new STA channel plan {", __FUNCTION__); + while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) { + _RTW_INFO("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].flags & RTW_CHF_NO_IR ? 'p' : 'c'); + k++; + } + _RTW_INFO("}\n"); +#endif + +#if 0 + /* recover the right channel index */ + channel = chplan_sta[pmlmeext->sitesurvey_res.channel_idx].ChannelNum; + k = 0; + while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) { + if (chplan_new[k].ChannelNum == channel) { + RTW_INFO("%s: change mlme_ext sitesurvey channel index from %d to %d\n", + __FUNCTION__, pmlmeext->sitesurvey_res.channel_idx, k); + pmlmeext->sitesurvey_res.channel_idx = k; + break; + } + k++; + } +#endif + +done_update_chplan_from_ap: + if (chplan_sta) + rtw_mfree(chplan_sta, sizeof(RT_CHANNEL_INFO) * MAX_CHANNEL_NUM); + } +} +#endif + +void rtw_survey_event_callback(_adapter *adapter, u8 *pbuf) +{ + _irqL irqL; + u32 len; + u8 val8; + WLAN_BSSID_EX *pnetwork; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + pnetwork = (WLAN_BSSID_EX *)pbuf; + + len = get_WLAN_BSSID_EX_sz(pnetwork); + if (len > (sizeof(WLAN_BSSID_EX))) { + return; + } + +#ifdef CONFIG_RTW_80211K + val8 = 0; + rtw_hal_get_hwreg(adapter, HW_VAR_FREECNT, &val8); + + /* use TSF if no free run counter */ + if (val8==0) + pnetwork->PhyInfo.free_cnt = (u32)rtw_hal_get_tsftr_by_port( + adapter, rtw_hal_get_port(adapter)); +#endif + + if (pnetwork->InfrastructureMode == Ndis802_11Infrastructure) { + #ifdef CONFIG_80211D + process_80211d(adapter, pnetwork); + #endif + if (MLME_IS_SCAN(adapter)) { + adapter->mlmeextpriv.sitesurvey_res.activate_ch_cnt + += rtw_process_beacon_hint(adapter, pnetwork); + } + } + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + /* update IBSS_network 's timestamp */ + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == _TRUE) { + if (_rtw_memcmp(&(pmlmepriv->cur_network.network.MacAddress), pnetwork->MacAddress, ETH_ALEN)) { + struct wlan_network *ibss_wlan = NULL; + _irqL irqL; + + _rtw_memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8); + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + ibss_wlan = _rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->MacAddress); + if (ibss_wlan) { + _rtw_memcpy(ibss_wlan->network.IEs , pnetwork->IEs, 8); + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + goto exit; + } + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + } + } + + /* lock pmlmepriv->lock when you accessing network_q */ + if ((check_fwstate(pmlmepriv, WIFI_UNDER_LINKING)) == _FALSE) { + if (pnetwork->Ssid.Ssid[0] == 0) + pnetwork->Ssid.SsidLength = 0; + rtw_add_network(adapter, pnetwork); + } + +exit: + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + + return; +} + +void rtw_surveydone_event_callback(_adapter *adapter, u8 *pbuf) +{ + _irqL irqL; + struct surveydone_event *parm = (struct surveydone_event *)pbuf; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + +#ifdef CONFIG_MLME_EXT + mlmeext_surveydone_event_callback(adapter); +#endif + + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + if (pmlmepriv->wps_probe_req_ie) { + u32 free_len = pmlmepriv->wps_probe_req_ie_len; + pmlmepriv->wps_probe_req_ie_len = 0; + rtw_mfree(pmlmepriv->wps_probe_req_ie, free_len); + pmlmepriv->wps_probe_req_ie = NULL; + } + + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _FALSE) { + RTW_INFO(FUNC_ADPT_FMT" fw_state:0x%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv)); + /* rtw_warn_on(1); */ + } + + if (pmlmeext->scan_abort == _TRUE) + pmlmeext->scan_abort = _FALSE; + + _clr_fwstate_(pmlmepriv, WIFI_UNDER_SURVEY); + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + _cancel_timer_ex(&pmlmepriv->scan_to_timer); + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + rtw_set_signal_stat_timer(&adapter->recvpriv); +#endif + + if (pmlmepriv->to_join == _TRUE) { + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE)) { + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _FALSE) { + set_fwstate(pmlmepriv, WIFI_UNDER_LINKING); + + if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS) + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + #ifdef CONFIG_AP_MODE + else { + WLAN_BSSID_EX *pdev_network = &(adapter->registrypriv.dev_network); + u8 *pibss = adapter->registrypriv.dev_network.MacAddress; + + /* pmlmepriv->fw_state ^= WIFI_UNDER_SURVEY; */ /* because don't set assoc_timer */ + _clr_fwstate_(pmlmepriv, WIFI_UNDER_SURVEY); + + + _rtw_memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID)); + _rtw_memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID)); + + rtw_update_registrypriv_dev_network(adapter); + rtw_generate_random_ibss(pibss); + + /*pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;*/ + init_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + + if (rtw_create_ibss_cmd(adapter, 0) != _SUCCESS) + RTW_ERR("rtw_create_ibss_cmd FAIL\n"); + + pmlmepriv->to_join = _FALSE; + } + #endif /* CONFIG_AP_MODE */ + } + } else { + int s_ret; + set_fwstate(pmlmepriv, WIFI_UNDER_LINKING); + pmlmepriv->to_join = _FALSE; + s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv); + if (_SUCCESS == s_ret) + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + else if (s_ret == 2) { /* there is no need to wait for join */ + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + rtw_indicate_connect(adapter); + } else { + RTW_INFO("try_to_join, but select scanning queue fail, to_roam:%d\n", rtw_to_roam(adapter)); + + if (rtw_to_roam(adapter) != 0) { + struct sitesurvey_parm scan_parm; + u8 ssc_chk = rtw_sitesurvey_condition_check(adapter, _FALSE); + + rtw_init_sitesurvey_parm(adapter, &scan_parm); + _rtw_memcpy(&scan_parm.ssid[0], &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID)); + scan_parm.ssid_num = 1; + + if (rtw_dec_to_roam(adapter) == 0 + || (ssc_chk != SS_ALLOW && ssc_chk != SS_DENY_BUSY_TRAFFIC) + || _SUCCESS != rtw_sitesurvey_cmd(adapter, &scan_parm) + ) { + rtw_set_to_roam(adapter, 0); + rtw_free_assoc_resources(adapter, _TRUE); + rtw_indicate_disconnect(adapter, 0, _FALSE); + } else + pmlmepriv->to_join = _TRUE; + } else + rtw_indicate_disconnect(adapter, 0, _FALSE); + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + } + } + } else { + if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE) + #if (defined(CONFIG_RTW_WNM) && defined(CONFIG_RTW_80211R)) + || rtw_wnm_btm_roam_triggered(adapter) + #endif + ) { + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) + && check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) { + if (rtw_select_roaming_candidate(pmlmepriv) == _SUCCESS) { + #ifdef CONFIG_RTW_80211R + rtw_ft_start_roam(adapter, + (u8 *)pmlmepriv->roam_network->network.MacAddress); + #else + receive_disconnect(adapter, pmlmepriv->cur_network.network.MacAddress + , WLAN_REASON_ACTIVE_ROAM, _FALSE); + #endif + } + } + } + } + + /* RTW_INFO("scan complete in %dms\n",rtw_get_passing_time_ms(pmlmepriv->scan_start_time)); */ + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +#ifdef CONFIG_P2P_PS + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + p2p_ps_wk_cmd(adapter, P2P_PS_SCAN_DONE, 0); +#endif /* CONFIG_P2P_PS */ + + rtw_mi_os_xmit_schedule(adapter); + +#ifdef CONFIG_DRVEXT_MODULE_WSC + drvext_surveydone_callback(&adapter->drvextpriv); +#endif + +#ifdef DBG_CONFIG_ERROR_DETECT + { + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + if (pmlmeext->sitesurvey_res.bss_cnt == 0) { + /* rtw_hal_sreset_reset(adapter); */ + } + } +#endif + +#ifdef CONFIG_IOCTL_CFG80211 + rtw_cfg80211_surveydone_event_callback(adapter); +#endif /* CONFIG_IOCTL_CFG80211 */ + + rtw_indicate_scan_done(adapter, _FALSE); + +#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_IOCTL_CFG80211) + rtw_cfg80211_indicate_scan_done_for_buddy(adapter, _FALSE); +#endif + + if (parm->activate_ch_cnt) { + op_class_pref_apply_regulatory(adapter, REG_BEACON_HINT); + rtw_nlrtw_reg_beacon_hint_event(adapter); + } + +#ifdef CONFIG_RTW_MESH + #if CONFIG_RTW_MESH_OFFCH_CAND + if (rtw_mesh_offch_candidate_accepted(adapter)) { + u8 ch; + + ch = rtw_mesh_select_operating_ch(adapter); + if (ch && pmlmepriv->cur_network.network.Configuration.DSConfig != ch) { + u8 ifbmp = rtw_mi_get_ap_mesh_ifbmp(adapter); + + if (ifbmp) { + /* switch to selected channel */ + rtw_change_bss_chbw_cmd(adapter, RTW_CMDF_DIRECTLY, ifbmp, 0, ch, REQ_BW_ORI, REQ_OFFSET_NONE); + issue_probereq_ex(adapter, &pmlmepriv->cur_network.network.mesh_id, NULL, 0, 0, 0, 0); + } else + rtw_warn_on(1); + } + } + #endif +#endif /* CONFIG_RTW_MESH */ + +#ifdef CONFIG_RTW_ACS + if (parm->acs) { + u8 ifbmp = rtw_mi_get_ap_mesh_ifbmp(adapter); + + if (ifbmp) + rtw_change_bss_chbw_cmd(adapter, RTW_CMDF_DIRECTLY, ifbmp, 0, REQ_CH_INT_INFO, REQ_BW_ORI, REQ_OFFSET_NONE); + } +#endif +} + +u8 _rtw_sitesurvey_condition_check(const char *caller, _adapter *adapter, bool check_sc_interval) +{ + u8 ss_condition = SS_ALLOW; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct registry_priv *registry_par = &adapter->registrypriv; + + +#ifdef CONFIG_MP_INCLUDED + if (rtw_mp_mode_check(adapter)) { + RTW_INFO("%s ("ADPT_FMT") MP mode block Scan request\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_MP_MODE; + goto _exit; + } +#endif + +#ifdef DBG_LA_MODE + if(registry_par->la_mode_en == 1 && MLME_IS_ASOC(adapter)) { + RTW_INFO("%s ("ADPT_FMT") LA debug mode block Scan request\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_LA_MODE; + goto _exit; + } +#endif + +#ifdef CONFIG_RTW_REPEATER_SON + if (adapter->rtw_rson_scanstage == RSON_SCAN_PROCESS) { + RTW_INFO("%s ("ADPT_FMT") blocking scan for under rson scanning process\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_RSON_SCANING; + goto _exit; + } +#endif +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(adapter)->block_scan == _TRUE) { + RTW_INFO("%s ("ADPT_FMT") wdev_priv.block_scan is set\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_BLOCK_SCAN; + goto _exit; + } +#endif + + if (adapter_to_dvobj(adapter)->scan_deny == _TRUE) { + RTW_INFO("%s ("ADPT_FMT") tpt mode, scan deny!\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_BLOCK_SCAN; + goto _exit; + } + + if (rtw_is_scan_deny(adapter)) { + RTW_INFO("%s ("ADPT_FMT") : scan deny\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_BY_DRV; + goto _exit; + } + +#ifdef CONFIG_ADAPTIVITY_DENY_SCAN + if (registry_par->adaptivity_en + && rtw_phydm_get_edcca_flag(adapter) + && rtw_is_2g_ch(GET_HAL_DATA(adapter)->current_channel)) { + RTW_WARN(FUNC_ADPT_FMT": Adaptivity block scan! (ch=%u)\n", + FUNC_ADPT_ARG(adapter), + GET_HAL_DATA(adapter)->current_channel); + ss_condition = SS_DENY_ADAPTIVITY; + goto _exit; + } +#endif /* CONFIG_ADAPTIVITY_DENY_SCAN */ + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)){ + if(check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!! AP mode process WPS\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_SELF_AP_UNDER_WPS; + goto _exit; + } else if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) == _TRUE) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!!AP mode under linking (fwstate=0x%x)\n", + caller, ADPT_ARG(adapter), pmlmepriv->fw_state); + ss_condition = SS_DENY_SELF_AP_UNDER_LINKING; + goto _exit; + } else if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!!AP mode under survey (fwstate=0x%x)\n", + caller, ADPT_ARG(adapter), pmlmepriv->fw_state); + ss_condition = SS_DENY_SELF_AP_UNDER_SURVEY; + goto _exit; + } + } else { + if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) == _TRUE) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!!STA mode under linking (fwstate=0x%x)\n", + caller, ADPT_ARG(adapter), pmlmepriv->fw_state); + ss_condition = SS_DENY_SELF_STA_UNDER_LINKING; + goto _exit; + } else if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) == _TRUE) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!!STA mode under survey (fwstate=0x%x)\n", + caller, ADPT_ARG(adapter), pmlmepriv->fw_state); + ss_condition = SS_DENY_SELF_STA_UNDER_SURVEY; + goto _exit; + } + } + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(adapter, WIFI_UNDER_LINKING | WIFI_UNDER_WPS)) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!! buddy_intf under linking or wps\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_BUDDY_UNDER_LINK_WPS; + goto _exit; + + } else if (rtw_mi_buddy_check_fwstate(adapter, WIFI_UNDER_SURVEY)) { + RTW_INFO("%s ("ADPT_FMT") : scan abort!! buddy_intf under survey\n", caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_BUDDY_UNDER_SURVEY; + goto _exit; + } +#endif /* CONFIG_CONCURRENT_MODE */ + +#ifdef RTW_BUSY_DENY_SCAN + /* + * busy traffic check + * Rules: + * 1. If (scan interval <= BUSY_TRAFFIC_SCAN_DENY_PERIOD) always allow + * scan, otherwise goto rule 2. + * 2. Deny scan if any interface is busy, otherwise allow scan. + */ + if (pmlmepriv->lastscantime + && (rtw_get_passing_time_ms(pmlmepriv->lastscantime) > + registry_par->scan_interval_thr) + && rtw_mi_busy_traffic_check(adapter)) { + RTW_WARN("%s ("ADPT_FMT") : scan abort!! BusyTraffic\n", + caller, ADPT_ARG(adapter)); + ss_condition = SS_DENY_BUSY_TRAFFIC; + goto _exit; + } +#endif /* RTW_BUSY_DENY_SCAN */ + +_exit: + return ss_condition; +} + +static void free_scanqueue(struct mlme_priv *pmlmepriv) +{ + _irqL irqL, irqL0; + _queue *free_queue = &pmlmepriv->free_bss_pool; + _queue *scan_queue = &pmlmepriv->scanned_queue; + _list *plist, *phead, *ptemp; + + + _enter_critical_bh(&scan_queue->lock, &irqL0); + _enter_critical_bh(&free_queue->lock, &irqL); + + phead = get_list_head(scan_queue); + plist = get_next(phead); + + while (plist != phead) { + ptemp = get_next(plist); + rtw_list_delete(plist); + rtw_list_insert_tail(plist, &free_queue->queue); + plist = ptemp; + pmlmepriv->num_of_scanned--; + } + + _exit_critical_bh(&free_queue->lock, &irqL); + _exit_critical_bh(&scan_queue->lock, &irqL0); + +} + +void rtw_reset_rx_info(_adapter *adapter) +{ + struct recv_priv *precvpriv = &adapter->recvpriv; + + precvpriv->dbg_rx_ampdu_drop_count = 0; + precvpriv->dbg_rx_ampdu_forced_indicate_count = 0; + precvpriv->dbg_rx_ampdu_loss_count = 0; + precvpriv->dbg_rx_dup_mgt_frame_drop_count = 0; + precvpriv->dbg_rx_ampdu_window_shift_cnt = 0; + precvpriv->dbg_rx_drop_count = 0; + precvpriv->dbg_rx_conflic_mac_addr_cnt = 0; +} + +/* +*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock +*/ +void rtw_free_assoc_resources(_adapter *adapter, u8 lock_scanned_queue) +{ + _irqL irqL; + struct wlan_network *pwlan = NULL; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_network *tgt_network = &pmlmepriv->cur_network; + + +#ifdef CONFIG_TDLS + struct tdls_info *ptdlsinfo = &adapter->tdlsinfo; +#endif /* CONFIG_TDLS */ + + + RTW_INFO("%s-"ADPT_FMT" tgt_network MacAddress=" MAC_FMT" ssid=%s\n", + __func__, ADPT_ARG(adapter), MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.Ssid); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + struct sta_info *psta; + + psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress); + +#ifdef CONFIG_TDLS + rtw_free_all_tdls_sta(adapter, _TRUE); + rtw_reset_tdls_info(adapter); + + if (ptdlsinfo->link_established == _TRUE) + rtw_tdls_cmd(adapter, NULL, TDLS_RS_RCR); +#endif /* CONFIG_TDLS */ + + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + rtw_free_stainfo(adapter, psta); + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + + } + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) { + struct sta_info *psta; + + rtw_free_all_stainfo(adapter); + + psta = rtw_get_bcmc_stainfo(adapter); + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + rtw_free_stainfo(adapter, psta); + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + + rtw_init_bcmc_stainfo(adapter); + } + + if (lock_scanned_queue) + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS) || (pmlmepriv->wpa_phase == _TRUE)){ + RTW_INFO("Dont free disconnecting network of scanned_queue due to uner %s %s phase\n\n", + check_fwstate(pmlmepriv, WIFI_UNDER_WPS) ? "WPS" : "", + (pmlmepriv->wpa_phase == _TRUE) ? "WPA" : ""); + } else { + pwlan = _rtw_find_same_network(&pmlmepriv->scanned_queue, tgt_network); + if (pwlan) { + pwlan->fixed = _FALSE; + + RTW_INFO("Free disconnecting network of scanned_queue\n"); + rtw_free_network_nolock(adapter, pwlan); +#ifdef CONFIG_P2P + if (!rtw_p2p_chk_state(&adapter->wdinfo, P2P_STATE_NONE)) { + rtw_set_scan_deny(adapter, 2000); + /* rtw_clear_scan_deny(adapter); */ + } +#endif /* CONFIG_P2P */ + } else + RTW_ERR("Free disconnecting network of scanned_queue failed due to pwlan == NULL\n\n"); + } + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count == 1)) + /*||check_fwstate(pmlmepriv, WIFI_STATION_STATE)*/) { + if (pwlan) + rtw_free_network_nolock(adapter, pwlan); + } + + if (lock_scanned_queue) + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + adapter->securitypriv.key_mask = 0; + + rtw_reset_rx_info(adapter); + + +} + +/* +*rtw_indicate_connect: the caller has to lock pmlmepriv->lock +*/ +void rtw_indicate_connect(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + pmlmepriv->to_join = _FALSE; + + if (!check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE)) { + + set_fwstate(pmlmepriv, WIFI_ASOC_STATE); + + rtw_led_control(padapter, LED_CTL_LINK); + + rtw_os_indicate_connect(padapter); + + #ifdef CONFIG_RTW_WDS + if (MLME_IS_STA(padapter)) + rtw_wds_gptr_tbl_init(padapter); + #endif + } + + rtw_set_to_roam(padapter, 0); + if (!MLME_IS_AP(padapter) && !MLME_IS_MESH(padapter)) + rtw_mi_set_scan_deny(padapter, 3000); + + +} + + +/* +*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock +*/ +void rtw_indicate_disconnect(_adapter *padapter, u16 reason, u8 locally_generated) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); +#ifdef CONFIG_WAPI_SUPPORT + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; +#endif + u8 *wps_ie = NULL; + uint wpsie_len = 0; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) + pmlmepriv->wpa_phase = _TRUE; + + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS | WIFI_OP_CH_SWITCHING | WIFI_UNDER_KEY_HANDSHAKE); + + /* force to clear cur_network_scanned's SELECTED REGISTRAR */ + if (pmlmepriv->cur_network_scanned) { + WLAN_BSSID_EX *current_joined_bss = &(pmlmepriv->cur_network_scanned->network); + if (current_joined_bss) { + wps_ie = rtw_get_wps_ie(current_joined_bss->IEs + _FIXED_IE_LENGTH_, + current_joined_bss->IELength - _FIXED_IE_LENGTH_, NULL, &wpsie_len); + if (wps_ie && wpsie_len > 0) { + u8 *attr = NULL; + u32 attr_len; + attr = rtw_get_wps_attr(wps_ie, wpsie_len, WPS_ATTR_SELECTED_REGISTRAR, + NULL, &attr_len); + if (attr) + *(attr + 4) = 0; + } + } + } + /* RTW_INFO("clear wps when %s\n", __func__); */ + + if (rtw_to_roam(padapter) > 0) + _clr_fwstate_(pmlmepriv, WIFI_ASOC_STATE); + +#ifdef CONFIG_WAPI_SUPPORT + psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress); + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) + rtw_wapi_return_one_sta_info(padapter, psta->cmn.mac_addr); + else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) || + check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) + rtw_wapi_return_all_sta_info(padapter); +#endif + + if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) + || (rtw_to_roam(padapter) <= 0) + ) { + #ifdef CONFIG_RTW_WDS + adapter_set_use_wds(padapter, 0); + rtw_wds_gptr_tbl_unregister(padapter); + #endif + #ifdef CONFIG_RTW_MULTI_AP + padapter->multi_ap = 0; + #endif + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (ATOMIC_READ(&padapter->tbtx_tx_pause) == _TRUE) { + ATOMIC_SET(&padapter->tbtx_tx_pause, _FALSE); + rtw_tx_control_cmd(padapter); + } +#endif + + rtw_os_indicate_disconnect(padapter, reason, locally_generated); + + /* set ips_deny_time to avoid enter IPS before LPS leave */ + rtw_set_ips_deny(padapter, 3000); + + _clr_fwstate_(pmlmepriv, WIFI_ASOC_STATE); + + rtw_led_control(padapter, LED_CTL_NO_LINK); + + rtw_clear_scan_deny(padapter); + } + +#ifdef CONFIG_P2P_PS + p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1); +#endif /* CONFIG_P2P_PS */ + +#ifdef CONFIG_LPS + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 0); +#endif + +#ifdef CONFIG_BEAMFORMING + beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_LEAVE, cur_network->MacAddress, ETH_ALEN, 1); +#endif /*CONFIG_BEAMFORMING*/ + +} + +inline void rtw_indicate_scan_done(_adapter *padapter, bool aborted) +{ + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + rtw_os_indicate_scan_done(padapter, aborted); + +#ifdef CONFIG_IPS + if (is_primary_adapter(padapter) + && (_FALSE == adapter_to_pwrctl(padapter)->bInSuspend) + && (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE | WIFI_UNDER_LINKING) == _FALSE)) { + struct pwrctrl_priv *pwrpriv; + + pwrpriv = adapter_to_pwrctl(padapter); + rtw_set_ips_deny(padapter, 0); +#ifdef CONFIG_IPS_CHECK_IN_WD + _set_timer(&adapter_to_dvobj(padapter)->dynamic_chk_timer, 1); +#else /* !CONFIG_IPS_CHECK_IN_WD */ + _rtw_set_pwr_state_check_timer(pwrpriv, 1); +#endif /* !CONFIG_IPS_CHECK_IN_WD */ + } +#endif /* CONFIG_IPS */ +} + +static u32 _rtw_wait_scan_done(_adapter *adapter, u8 abort, u32 timeout_ms) +{ + systime start; + u32 pass_ms; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + start = rtw_get_current_time(); + + pmlmeext->scan_abort = abort; + + while (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) + && rtw_get_passing_time_ms(start) <= timeout_ms) { + + if (RTW_CANNOT_RUN(adapter)) + break; + + RTW_INFO(FUNC_NDEV_FMT"fw_state=WIFI_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev)); + rtw_msleep_os(20); + } + + if (_TRUE == abort) { + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY)) { + if (!RTW_CANNOT_RUN(adapter)) + RTW_INFO(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev)); +#ifdef CONFIG_PLATFORM_MSTAR + /*_clr_fwstate_(pmlmepriv, WIFI_UNDER_SURVEY);*/ + set_survey_timer(pmlmeext, 0); + mlme_set_scan_to_timer(pmlmepriv, 50); +#endif + rtw_indicate_scan_done(adapter, _TRUE); + } + } + + pmlmeext->scan_abort = _FALSE; + pass_ms = rtw_get_passing_time_ms(start); + + return pass_ms; + +} + +void rtw_scan_wait_completed(_adapter *adapter) +{ + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct ss_res *ss = &pmlmeext->sitesurvey_res; + + _rtw_wait_scan_done(adapter, _FALSE, ss->scan_timeout_ms); +} + +u32 rtw_scan_abort_timeout(_adapter *adapter, u32 timeout_ms) +{ + return _rtw_wait_scan_done(adapter, _TRUE, timeout_ms); +} + +void rtw_scan_abort_no_wait(_adapter *adapter) +{ + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY)) + pmlmeext->scan_abort = _TRUE; +} + +void rtw_scan_abort(_adapter *adapter) +{ + rtw_scan_abort_timeout(adapter, 200); +} + +static u32 _rtw_wait_join_done(_adapter *adapter, u8 abort, u32 timeout_ms) +{ + systime start; + u32 pass_ms; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + + start = rtw_get_current_time(); + + pmlmeext->join_abort = abort; + if (abort) + set_link_timer(pmlmeext, 1); + + while (rtw_get_passing_time_ms(start) <= timeout_ms + && (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) + #ifdef CONFIG_IOCTL_CFG80211 + || rtw_cfg80211_is_connect_requested(adapter) + #endif + ) + ) { + if (RTW_CANNOT_RUN(adapter)) + break; + + RTW_INFO(FUNC_ADPT_FMT" linking...\n", FUNC_ADPT_ARG(adapter)); + rtw_msleep_os(20); + } + + if (abort) { + if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING) + #ifdef CONFIG_IOCTL_CFG80211 + || rtw_cfg80211_is_connect_requested(adapter) + #endif + ) { + if (!RTW_CANNOT_RUN(adapter)) + RTW_INFO(FUNC_ADPT_FMT" waiting for join_abort time out!\n", FUNC_ADPT_ARG(adapter)); + } + } + + pmlmeext->join_abort = 0; + pass_ms = rtw_get_passing_time_ms(start); + + return pass_ms; +} + +u32 rtw_join_abort_timeout(_adapter *adapter, u32 timeout_ms) +{ + return _rtw_wait_join_done(adapter, _TRUE, timeout_ms); +} + +static struct sta_info *rtw_joinbss_update_stainfo(_adapter *padapter, struct wlan_network *pnetwork) +{ + int i; + struct sta_info *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#ifdef CONFIG_RTS_FULL_BW + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct wlan_network *cur_network = &(pmlmepriv->cur_network); +#endif/*CONFIG_RTS_FULL_BW*/ + + psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress); + if (psta == NULL) + psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress); + + if (psta) { /* update ptarget_sta */ + RTW_INFO("%s\n", __FUNCTION__); + + psta->cmn.aid = pnetwork->join_res; + + update_sta_info(padapter, psta); + + /* update station supportRate */ + psta->bssratelen = rtw_get_rateset_len(pnetwork->network.SupportedRates); + _rtw_memcpy(psta->bssrateset, pnetwork->network.SupportedRates, psta->bssratelen); + rtw_hal_update_sta_ra_info(padapter, psta); + + psta->wireless_mode = pmlmeext->cur_wireless_mode; + rtw_hal_update_sta_wset(padapter, psta); + + /* sta mode */ + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE); + + /* security related */ +#ifdef CONFIG_RTW_80211R + if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + && (psta->ft_pairwise_key_installed == _FALSE)) { +#else + if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { +#endif + u8 *ie; + sint ie_len; + u8 mfp_opt = MFP_NO; + u8 spp_opt = 0; + + padapter->securitypriv.binstallGrpkey = _FALSE; + padapter->securitypriv.busetkipkey = _FALSE; + padapter->securitypriv.bgrpkey_handshake = _FALSE; + + ie = rtw_get_ie(pnetwork->network.IEs + _BEACON_IE_OFFSET_, WLAN_EID_RSN + , &ie_len, (pnetwork->network.IELength - _BEACON_IE_OFFSET_)); + if (ie && ie_len > 0 + && rtw_parse_wpa2_ie(ie, ie_len + 2, NULL, NULL, NULL, NULL, &mfp_opt, &spp_opt) == _SUCCESS + ) { + if (padapter->securitypriv.mfp_opt >= MFP_OPTIONAL && mfp_opt >= MFP_OPTIONAL) + psta->flags |= WLAN_STA_MFP; + } + + if (padapter->securitypriv.dot11PrivacyAlgrthm != _NO_PRIVACY_ ) { + /*check if amsdu is allowed */ + if (rtw_check_amsdu_disable(padapter->registrypriv.amsdu_mode, spp_opt) == _TRUE) + psta->flags |= WLAN_STA_AMSDU_DISABLE; + } + psta->ieee8021x_blocked = _TRUE; + psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm; + + _rtw_memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype)); + _rtw_memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype)); + _rtw_memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype)); + } + + /* Commented by Albert 2012/07/21 */ + /* When doing the WPS, the wps_ie_len won't equal to 0 */ + /* And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */ + if (padapter->securitypriv.wps_ie_len != 0) { + psta->ieee8021x_blocked = _TRUE; + padapter->securitypriv.wps_ie_len = 0; + } + + + /* for A-MPDU Rx reordering buffer control for sta_info */ + /* if A-MPDU Rx is enabled, reseting rx_ordering_ctrl wstart_b(indicate_seq) to default value=0xffff */ + /* todo: check if AP can send A-MPDU packets */ + for (i = 0; i < 16 ; i++) { + /* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */ + preorder_ctrl = &psta->recvreorder_ctrl[i]; + preorder_ctrl->enable = _FALSE; + preorder_ctrl->indicate_seq = 0xffff; + rtw_clear_bit(RTW_RECV_ACK_OR_TIMEOUT, &preorder_ctrl->rec_abba_rsp_ack); + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_CLEAR indicate_seq:%u preorder_ctrl->rec_abba_rsp_ack:%lu\n" + , FUNC_ADPT_ARG(padapter) + , i + , preorder_ctrl->indicate_seq + ,preorder_ctrl->rec_abba_rsp_ack + ); + #endif + preorder_ctrl->wend_b = 0xffff; + preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; */ /* ex. 32(kbytes) -> wsize_b=32 */ + preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID; + } + } + +#ifdef CONFIG_RTW_80211K + _rtw_memcpy(&psta->rm_en_cap, pnetwork->network.PhyInfo.rm_en_cap, 5); +#endif +#ifdef CONFIG_RTW_MULTI_AP + if (padapter->multi_ap & MULTI_AP_BACKHAUL_STA) { + u8 multi_ap = rtw_get_multi_ap_ie_ext(pmlmepriv->assoc_rsp + sizeof(struct rtw_ieee80211_hdr_3addr) + 6 + , pmlmepriv->assoc_rsp_len - sizeof(struct rtw_ieee80211_hdr_3addr) - 6); + + if (multi_ap & MULTI_AP_BACKHAUL_BSS) /* backhaul bss, enable WDS */ + psta->flags |= WLAN_STA_MULTI_AP | WLAN_STA_WDS; + else if (multi_ap & MULTI_AP_FRONTHAUL_BSS) /* fronthaul bss only */ + psta->flags |= WLAN_STA_MULTI_AP; + } +#endif +#ifdef CONFIG_RTS_FULL_BW + rtw_parse_sta_vendor_ie_8812(padapter, psta, BSS_EX_TLV_IES(&cur_network->network), BSS_EX_TLV_IES_LEN(&cur_network->network)); +#endif + return psta; + +} + +/* pnetwork : returns from rtw_joinbss_event_callback + * ptarget_wlan: found from scanned_queue */ +static void rtw_joinbss_update_network(_adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network *pnetwork) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + sint tmp_fw_state = 0x0; + + RTW_INFO("%s\n", __FUNCTION__); + + /* why not use ptarget_wlan?? */ + _rtw_memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length); + /* some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs */ + cur_network->network.IELength = ptarget_wlan->network.IELength; + _rtw_memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ); + + cur_network->aid = pnetwork->join_res; + + +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + rtw_set_signal_stat_timer(&padapter->recvpriv); +#endif + padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength; + padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality; + /* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */ + padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength); +#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1 + RTW_INFO(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u" + "\n" + , FUNC_ADPT_ARG(padapter) + , padapter->recvpriv.signal_strength + , padapter->recvpriv.rssi + , padapter->recvpriv.signal_qual + ); +#endif +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + rtw_set_signal_stat_timer(&padapter->recvpriv); +#endif + + /* update fw_state */ /* will clr WIFI_UNDER_LINKING here indirectly */ + + switch (pnetwork->network.InfrastructureMode) { + case Ndis802_11Infrastructure: + /* Check encryption */ + if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + tmp_fw_state = tmp_fw_state | WIFI_UNDER_KEY_HANDSHAKE; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) + tmp_fw_state = tmp_fw_state | WIFI_UNDER_WPS; + + init_fwstate(pmlmepriv, WIFI_STATION_STATE | tmp_fw_state); + + break; + case Ndis802_11IBSS: + /*pmlmepriv->fw_state = WIFI_ADHOC_STATE;*/ + init_fwstate(pmlmepriv, WIFI_ADHOC_STATE); + break; + default: + /*pmlmepriv->fw_state = WIFI_NULL_STATE;*/ + init_fwstate(pmlmepriv, WIFI_NULL_STATE); + break; + } + + rtw_update_protection(padapter, (cur_network->network.IEs) + sizeof(NDIS_802_11_FIXED_IEs), + (cur_network->network.IELength)); + +#ifdef CONFIG_80211N_HT + rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength, (u8) cur_network->network.Configuration.DSConfig); +#endif +} + +/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet) + * pnetwork : returns from rtw_joinbss_event_callback + * ptarget_wlan: found from scanned_queue + * if join_res > 0, for (fw_state==WIFI_STATION_STATE), we check if "ptarget_sta" & "ptarget_wlan" exist. + * if join_res > 0, for (fw_state==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. + * if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan !=NULL). + */ +/* #define REJOIN */ +void rtw_joinbss_event_prehandle(_adapter *adapter, u8 *pbuf, u16 status) +{ + _irqL irqL; + static u8 retry = 0; + struct sta_info *ptarget_sta = NULL, *pcur_sta = NULL; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct wlan_network *pnetwork = (struct wlan_network *)pbuf; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct wlan_network *pcur_wlan = NULL, *ptarget_wlan = NULL; + unsigned int the_same_macaddr = _FALSE; + + rtw_get_encrypt_decrypt_from_registrypriv(adapter); + + the_same_macaddr = _rtw_memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN); + + pnetwork->network.Length = get_WLAN_BSSID_EX_sz(&pnetwork->network); + if (pnetwork->network.Length > sizeof(WLAN_BSSID_EX)) + goto exit; + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0; + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0; + + + if (pnetwork->join_res > 0) { + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + retry = 0; + if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING)) { + /* s1. find ptarget_wlan */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) { + if (the_same_macaddr == _TRUE) + ptarget_wlan = _rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + else { + pcur_wlan = _rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + if (pcur_wlan) + pcur_wlan->fixed = _FALSE; + + pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress); + if (pcur_sta) { + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */ + rtw_free_stainfo(adapter, pcur_sta); + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */ + } + + ptarget_wlan = _rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress); + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) { + if (ptarget_wlan) + ptarget_wlan->fixed = _TRUE; + } + } + + } else { + ptarget_wlan = _rtw_find_same_network(&pmlmepriv->scanned_queue, pnetwork); + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) { + if (ptarget_wlan) + ptarget_wlan->fixed = _TRUE; + } + } + + /* s2. update cur_network */ + if (ptarget_wlan) + rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork); + else { + RTW_PRINT("Can't find ptarget_wlan when joinbss_event callback\n"); + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + goto ignore_joinbss_callback; + } + + + /* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) { + ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork); + if (ptarget_sta == NULL) { + RTW_ERR("Can't update stainfo when joinbss_event callback\n"); + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + goto ignore_joinbss_callback; + } + + /* Queue TX packets before FW/HW ready */ + /* clear in mlmeext_joinbss_event_callback() */ + rtw_xmit_queue_set(ptarget_sta); + } + + /* s4. indicate connect */ + if (MLME_IS_STA(adapter) || MLME_IS_ADHOC(adapter)) { + pmlmepriv->cur_network_scanned = ptarget_wlan; + rtw_indicate_connect(adapter); + } + + /* s5. Cancle assoc_timer */ + _cancel_timer_ex(&pmlmepriv->assoc_timer); + + + } else { + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + goto ignore_joinbss_callback; + } + + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + } else if (pnetwork->join_res == -4) { + rtw_reset_securitypriv(adapter); + pmlmepriv->join_status = status; + _set_timer(&pmlmepriv->assoc_timer, 1); + + /* rtw_free_assoc_resources(adapter, _TRUE); */ + + if ((check_fwstate(pmlmepriv, WIFI_UNDER_LINKING)) == _TRUE) { + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + } + + } else { /* if join_res < 0 (join fails), then try again */ + +#ifdef REJOIN + res = _FAIL; + if (retry < 2) { + res = rtw_select_and_join_from_scanned_queue(pmlmepriv); + } + + if (res == _SUCCESS) { + /* extend time of assoc_timer */ + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + retry++; + } else if (res == 2) { /* there is no need to wait for join */ + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + rtw_indicate_connect(adapter); + } else { +#endif + pmlmepriv->join_status = status; + _set_timer(&pmlmepriv->assoc_timer, 1); + /* rtw_free_assoc_resources(adapter, _TRUE); */ + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + +#ifdef REJOIN + retry = 0; + } +#endif + } + +ignore_joinbss_callback: + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +exit: + return; +} + +void rtw_joinbss_event_callback(_adapter *adapter, u8 *pbuf) +{ + struct wlan_network *pnetwork = (struct wlan_network *)pbuf; + + + mlmeext_joinbss_event_callback(adapter, pnetwork->join_res); + + rtw_mi_os_xmit_schedule(adapter); + +} + +void rtw_sta_media_status_rpt(_adapter *adapter, struct sta_info *sta, bool connected) +{ + struct macid_ctl_t *macid_ctl = &adapter->dvobj->macid_ctl; + bool miracast_enabled = 0; + bool miracast_sink = 0; + u8 role = H2C_MSR_ROLE_RSVD; + + if (sta == NULL) { + RTW_PRINT(FUNC_ADPT_FMT" sta is NULL\n" + , FUNC_ADPT_ARG(adapter)); + rtw_warn_on(1); + return; + } + + if (sta->cmn.mac_id >= macid_ctl->num) { + RTW_PRINT(FUNC_ADPT_FMT" invalid macid:%u\n" + , FUNC_ADPT_ARG(adapter), sta->cmn.mac_id); + rtw_warn_on(1); + return; + } + + if (!rtw_macid_is_used(macid_ctl, sta->cmn.mac_id)) { + RTW_PRINT(FUNC_ADPT_FMT" macid:%u not is used, set connected to 0\n" + , FUNC_ADPT_ARG(adapter), sta->cmn.mac_id); + connected = 0; + rtw_warn_on(1); + } + + if (connected && !rtw_macid_is_bmc(macid_ctl, sta->cmn.mac_id)) { + miracast_enabled = STA_OP_WFD_MODE(sta) != 0 && is_miracast_enabled(adapter); + miracast_sink = miracast_enabled && (STA_OP_WFD_MODE(sta) & MIRACAST_SINK); + +#ifdef CONFIG_TDLS + if (sta->tdls_sta_state & TDLS_LINKED_STATE) + role = H2C_MSR_ROLE_TDLS; + else +#endif + if (MLME_IS_STA(adapter)) { + if (MLME_IS_GC(adapter)) + role = H2C_MSR_ROLE_GO; + else + role = H2C_MSR_ROLE_AP; + } else if (MLME_IS_AP(adapter)) { + if (MLME_IS_GO(adapter)) + role = H2C_MSR_ROLE_GC; + else + role = H2C_MSR_ROLE_STA; + } else if (MLME_IS_ADHOC(adapter) || MLME_IS_ADHOC_MASTER(adapter)) + role = H2C_MSR_ROLE_ADHOC; + else if (MLME_IS_MESH(adapter)) + role = H2C_MSR_ROLE_MESH; + +#ifdef CONFIG_WFD + if (role == H2C_MSR_ROLE_GC + || role == H2C_MSR_ROLE_GO + || role == H2C_MSR_ROLE_TDLS + ) { + if (adapter->wfd_info.rtsp_ctrlport + || adapter->wfd_info.tdls_rtsp_ctrlport + || adapter->wfd_info.peer_rtsp_ctrlport) + rtw_wfd_st_switch(sta, 1); + } +#endif + } + + rtw_hal_set_FwMediaStatusRpt_single_cmd(adapter + , connected + , miracast_enabled + , miracast_sink + , role + , sta->cmn.mac_id + ); +} + +u8 rtw_sta_media_status_rpt_cmd(_adapter *adapter, struct sta_info *sta, bool connected) +{ + struct cmd_priv *cmdpriv = &adapter->cmdpriv; + struct cmd_obj *cmdobj; + struct drvextra_cmd_parm *cmd_parm; + struct sta_media_status_rpt_cmd_parm *rpt_parm; + u8 res = _SUCCESS; + + cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmdobj == NULL) { + res = _FAIL; + goto exit; + } + + cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (cmd_parm == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + rpt_parm = (struct sta_media_status_rpt_cmd_parm *)rtw_zmalloc(sizeof(struct sta_media_status_rpt_cmd_parm)); + if (rpt_parm == NULL) { + rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)cmd_parm, sizeof(struct drvextra_cmd_parm)); + res = _FAIL; + goto exit; + } + + rpt_parm->sta = sta; + rpt_parm->connected = connected; + + cmd_parm->ec_id = STA_MSTATUS_RPT_WK_CID; + cmd_parm->type = 0; + cmd_parm->size = sizeof(struct sta_media_status_rpt_cmd_parm); + cmd_parm->pbuf = (u8 *)rpt_parm; + init_h2fwcmd_w_parm_no_rsp(cmdobj, cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(cmdpriv, cmdobj); + +exit: + return res; +} + +inline void rtw_sta_media_status_rpt_cmd_hdl(_adapter *adapter, struct sta_media_status_rpt_cmd_parm *parm) +{ + rtw_sta_media_status_rpt(adapter, parm->sta, parm->connected); +} + +void rtw_stassoc_event_callback(_adapter *adapter, u8 *pbuf) +{ + _irqL irqL; + struct sta_info *psta; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct stassoc_event *pstassoc = (struct stassoc_event *)pbuf; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); + struct wlan_network *ptarget_wlan = NULL; + + +#if CONFIG_RTW_MACADDR_ACL + if (rtw_access_ctrl(adapter, pstassoc->macaddr) == _FALSE) + return; +#endif + +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + if (MLME_IS_AP(adapter) || MLME_IS_MESH(adapter)) { + psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr); + if (psta) { + u8 *passoc_req = NULL; + u32 assoc_req_len = 0; + + rtw_sta_media_status_rpt(adapter, psta, 1); + +#ifdef CONFIG_MCC_MODE + rtw_hal_mcc_update_macid_bitmap(adapter, psta->cmn.mac_id, _TRUE); +#endif /* CONFIG_MCC_MODE */ + +#ifndef CONFIG_AUTO_AP_MODE + ap_sta_info_defer_update(adapter, psta); + + if (!MLME_IS_MESH(adapter)) { + /* report to upper layer */ + RTW_INFO("indicate_sta_assoc_event to upper layer - hostapd\n"); + #ifdef CONFIG_IOCTL_CFG80211 + _enter_critical_bh(&psta->lock, &irqL); + if (psta->passoc_req && psta->assoc_req_len > 0) { + passoc_req = rtw_zmalloc(psta->assoc_req_len); + if (passoc_req) { + assoc_req_len = psta->assoc_req_len; + _rtw_memcpy(passoc_req, psta->passoc_req, assoc_req_len); + } + } + _exit_critical_bh(&psta->lock, &irqL); + + if (passoc_req && assoc_req_len > 0) { + rtw_cfg80211_indicate_sta_assoc(adapter, passoc_req, assoc_req_len); + rtw_mfree(passoc_req, assoc_req_len); + } + #else /* !CONFIG_IOCTL_CFG80211 */ + rtw_indicate_sta_assoc_event(adapter, psta); + #endif /* !CONFIG_IOCTL_CFG80211 */ + } +#endif /* !CONFIG_AUTO_AP_MODE */ + +#ifdef CONFIG_BEAMFORMING + beamforming_wk_cmd(adapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0); +#endif/*CONFIG_BEAMFORMING*/ + if (is_wep_enc(adapter->securitypriv.dot11PrivacyAlgrthm)) + rtw_ap_wep_pk_setting(adapter, psta); + + #ifdef CONFIG_PLATFORM_CMAP_INTFS + if (MLME_IS_AP(adapter)) { + cmap_intfs_nl_sta_event(psta->cmn.mac_addr, adapter_mac_addr(adapter), 1 + , psta->passoc_req + IEEE80211_3ADDR_LEN, psta->assoc_req_len - IEEE80211_3ADDR_LEN); + } + #endif + } + goto exit; + } +#endif /* defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */ + + /* for AD-HOC mode */ + psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr); + if (psta == NULL) { + RTW_ERR(FUNC_ADPT_FMT" get no sta_info with "MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(pstassoc->macaddr)); + rtw_warn_on(1); + goto exit; + } + + rtw_sta_media_status_rpt(adapter, psta, 1); + + if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) + psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm; + + + psta->ieee8021x_blocked = _FALSE; + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE)) { + if (adapter->stapriv.asoc_sta_count == 2) { + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + ptarget_wlan = _rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress); + pmlmepriv->cur_network_scanned = ptarget_wlan; + if (ptarget_wlan) + ptarget_wlan->fixed = _TRUE; + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + /* a sta + bc/mc_stainfo (not Ibss_stainfo) */ + rtw_indicate_connect(adapter); + } + } + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + + mlmeext_sta_add_event_callback(adapter, psta); + +#ifdef CONFIG_RTL8711 + /* submit SetStaKey_cmd to tell fw, fw will allocate an CAM entry for this sta */ + rtw_setstakey_cmd(adapter, psta, GROUP_KEY, _TRUE); +#endif + +exit: +#ifdef CONFIG_RTS_FULL_BW + rtw_set_rts_bw(adapter); +#endif/*CONFIG_RTS_FULL_BW*/ + return; +} + +#ifdef CONFIG_IEEE80211W +void rtw_sta_timeout_event_callback(_adapter *adapter, u8 *pbuf) +{ +#ifdef CONFIG_AP_MODE + _irqL irqL; + struct sta_info *psta; + struct stadel_event *pstadel = (struct stadel_event *)pbuf; + struct sta_priv *pstapriv = &adapter->stapriv; + + psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr); + + if (psta) { + u8 updated = _FALSE; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&psta->asoc_list) == _FALSE) { + rtw_list_delete(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + updated = ap_free_sta(adapter, psta, _TRUE, WLAN_REASON_PREV_AUTH_NOT_VALID, _TRUE); + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + associated_clients_update(adapter, updated, STA_INFO_UPDATE_ALL); + } +#endif /* CONFIG_AP_MODE */ +} +#endif /* CONFIG_IEEE80211W */ + +void rtw_sta_mstatus_disc_rpt(_adapter *adapter, u8 mac_id) +{ + struct macid_ctl_t *macid_ctl = &adapter->dvobj->macid_ctl; + + if (mac_id < macid_ctl->num) { + u8 id_is_shared = mac_id == RTW_DEFAULT_MGMT_MACID; /* TODO: real shared macid judgment */ + + RTW_INFO(FUNC_ADPT_FMT" - mac_id=%d%s\n", FUNC_ADPT_ARG(adapter) + , mac_id, id_is_shared ? " shared" : ""); + + if (!id_is_shared) { + rtw_hal_macid_drop(adapter, mac_id); + rtw_hal_set_FwMediaStatusRpt_single_cmd(adapter, 0, 0, 0, 0, mac_id); + /* + * For safety, prevent from keeping macid sleep. + * If we can sure all power mode enter/leave are paired, + * this check can be removed. + * Lucas@20131113 + */ + /* wakeup macid after disconnect. */ + /*if (MLME_IS_STA(adapter))*/ + rtw_hal_macid_wakeup(adapter, mac_id); + } + } else { + RTW_PRINT(FUNC_ADPT_FMT" invalid macid:%u\n" + , FUNC_ADPT_ARG(adapter), mac_id); + rtw_warn_on(1); + } +} +void rtw_sta_mstatus_report(_adapter *adapter) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_network *tgt_network = &pmlmepriv->cur_network; + struct sta_info *psta = NULL; + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) && check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) { + psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress); + if (psta) + rtw_sta_mstatus_disc_rpt(adapter, psta->cmn.mac_id); + else { + RTW_INFO("%s "ADPT_FMT" - mac_addr: "MAC_FMT" psta == NULL\n", __func__, ADPT_ARG(adapter), MAC_ARG(tgt_network->network.MacAddress)); + rtw_warn_on(1); + } + } +} + +void rtw_stadel_event_callback(_adapter *adapter, u8 *pbuf) +{ + _irqL irqL, irqL2; + + struct sta_info *psta; + struct wlan_network *pwlan = NULL; + WLAN_BSSID_EX *pdev_network = NULL; + u8 *pibss = NULL; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct stadel_event *pstadel = (struct stadel_event *)pbuf; + struct wlan_network *tgt_network = &(pmlmepriv->cur_network); + + RTW_INFO("%s(mac_id=%d)=" MAC_FMT "\n", __func__, pstadel->mac_id, MAC_ARG(pstadel->macaddr)); +#ifdef CONFIG_LAYER2_ROAMING + if (pmlmepriv->roam_network) + rtw_msleep_os(RTW_ROAM_DICONNECT_DELAY); +#endif + rtw_sta_mstatus_disc_rpt(adapter, pstadel->mac_id); + +#ifdef CONFIG_MCC_MODE + rtw_hal_mcc_update_macid_bitmap(adapter, pstadel->mac_id, _FALSE); +#endif /* CONFIG_MCC_MODE */ + + psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr); + + if (psta == NULL) { + RTW_INFO("%s(mac_id=%d)=" MAC_FMT " psta == NULL\n", __func__, pstadel->mac_id, MAC_ARG(pstadel->macaddr)); + /*rtw_warn_on(1);*/ + } + + if (psta) + rtw_wfd_st_switch(psta, 0); + + if (MLME_IS_MESH(adapter)) { + rtw_free_stainfo(adapter, psta); + goto exit; + } + +#ifdef CONFIG_AP_MODE + if (MLME_IS_AP(adapter)) { +#ifdef CONFIG_IOCTL_CFG80211 +#ifdef COMPAT_KERNEL_RELEASE + +#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) + rtw_cfg80211_indicate_sta_disassoc(adapter, pstadel->macaddr, *(u16 *)pstadel->rsvd); +#endif /* (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */ +#endif /* CONFIG_IOCTL_CFG80211 */ + + #ifdef CONFIG_PLATFORM_CMAP_INTFS + cmap_intfs_nl_sta_event(pstadel->macaddr, adapter_mac_addr(adapter), 0, NULL, 0); + #endif + + rtw_free_stainfo(adapter, psta); + + goto exit; + } +#endif /* CONFIG_AP_MODE */ + + mlmeext_sta_del_event_callback(adapter); + + _enter_critical_bh(&pmlmepriv->lock, &irqL2); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + u16 reason = *((unsigned short *)(pstadel->rsvd)); + bool roam = _FALSE; + struct wlan_network *roam_target = NULL; + +#ifdef CONFIG_LAYER2_ROAMING +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam_expired(adapter, reason)) + pmlmepriv->ft_roam.ft_roam_on_expired = _TRUE; + else + pmlmepriv->ft_roam.ft_roam_on_expired = _FALSE; +#endif + if (adapter->registrypriv.wifi_spec == 1) + roam = _FALSE; + else if (reason == WLAN_REASON_EXPIRATION_CHK && rtw_chk_roam_flags(adapter, RTW_ROAM_ON_EXPIRED)) + roam = _TRUE; + else if (reason == WLAN_REASON_ACTIVE_ROAM && rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) { + roam = _TRUE; + roam_target = pmlmepriv->roam_network; + } + +#ifdef CONFIG_RTW_80211R + if (reason == WLAN_REASON_ACTIVE_ROAM && rtw_ft_chk_flags(adapter, RTW_FT_BTM_ROAM)) { + roam = _TRUE; + roam_target = pmlmepriv->roam_network; + } +#endif + + if (roam == _TRUE) { + if (rtw_to_roam(adapter) > 0) + rtw_dec_to_roam(adapter); /* this stadel_event is caused by roaming, decrease to_roam */ + else if (rtw_to_roam(adapter) == 0) + rtw_set_to_roam(adapter, adapter->registrypriv.max_roaming_times); + } else + rtw_set_to_roam(adapter, 0); +#endif /* CONFIG_LAYER2_ROAMING */ + + rtw_free_uc_swdec_pending_queue(adapter); + + rtw_free_assoc_resources(adapter, _TRUE); + rtw_free_mlme_priv_ie_data(pmlmepriv); + + rtw_indicate_disconnect(adapter, *(u16 *)pstadel->rsvd, pstadel->locally_generated); + +#ifdef CONFIG_LAYER2_ROAMING + _rtw_roaming(adapter, roam_target); +#endif + } + +#ifdef CONFIG_AP_MODE + if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) || + check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) { + + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + rtw_free_stainfo(adapter, psta); + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + + if (adapter->stapriv.asoc_sta_count == 1) { /* a sta + bc/mc_stainfo (not Ibss_stainfo) */ + /* rtw_indicate_disconnect(adapter); */ /* removed@20091105 */ + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + /* free old ibss network */ + /* pwlan = _rtw_find_network(&pmlmepriv->scanned_queue, pstadel->macaddr); */ + pwlan = _rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress); + if (pwlan) { + pwlan->fixed = _FALSE; + rtw_free_network_nolock(adapter, pwlan); + } + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + /* re-create ibss */ + pdev_network = &(adapter->registrypriv.dev_network); + pibss = adapter->registrypriv.dev_network.MacAddress; + + _rtw_memcpy(pdev_network, &tgt_network->network, get_WLAN_BSSID_EX_sz(&tgt_network->network)); + + _rtw_memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID)); + _rtw_memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID)); + + rtw_update_registrypriv_dev_network(adapter); + + rtw_generate_random_ibss(pibss); + + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) { + set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + _clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE); + } + + if (rtw_create_ibss_cmd(adapter, 0) != _SUCCESS) + RTW_ERR("rtw_create_ibss_cmd FAIL\n"); + + } + + } +#endif /* CONFIG_AP_MODE */ + _exit_critical_bh(&pmlmepriv->lock, &irqL2); +exit: + #ifdef CONFIG_RTS_FULL_BW + rtw_set_rts_bw(adapter); + #endif/*CONFIG_RTS_FULL_BW*/ + return; +} + +void rtw_wmm_event_callback(PADAPTER padapter, u8 *pbuf) +{ + + WMMOnAssocRsp(padapter); + + +} + +/* +* rtw_join_timeout_handler - Timeout/failure handler for CMD JoinBss +*/ +void rtw_join_timeout_handler(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + _irqL irqL; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + +#if 0 + if (rtw_is_drv_stopped(adapter)) { + _rtw_up_sema(&pmlmepriv->assoc_terminate); + return; + } +#endif + + + + RTW_INFO("%s, fw_state=%x\n", __FUNCTION__, get_fwstate(pmlmepriv)); + + if (RTW_CANNOT_RUN(adapter)) + return; + + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + +#ifdef CONFIG_LAYER2_ROAMING + if (rtw_to_roam(adapter) > 0) { /* join timeout caused by roaming */ + while (1) { + rtw_dec_to_roam(adapter); + if (rtw_to_roam(adapter) != 0) { /* try another */ + int do_join_r; + RTW_INFO("%s try another roaming\n", __FUNCTION__); + do_join_r = rtw_do_join(adapter); + if (_SUCCESS != do_join_r) { + RTW_INFO("%s roaming do_join return %d\n", __FUNCTION__ , do_join_r); + continue; + } + break; + } else { + RTW_INFO("%s We've try roaming but fail\n", __FUNCTION__); +#ifdef CONFIG_RTW_80211R + rtw_ft_clr_flags(adapter, RTW_FT_PEER_EN|RTW_FT_PEER_OTD_EN); + rtw_ft_reset_status(adapter); +#endif + rtw_indicate_disconnect(adapter, pmlmepriv->join_status, _FALSE); + break; + } + } + + } else +#endif + { + rtw_indicate_disconnect(adapter, pmlmepriv->join_status, _FALSE); + free_scanqueue(pmlmepriv);/* ??? */ + +#ifdef CONFIG_IOCTL_CFG80211 + /* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */ + rtw_cfg80211_indicate_disconnect(adapter, pmlmepriv->join_status, _FALSE); +#endif /* CONFIG_IOCTL_CFG80211 */ + + } + + pmlmepriv->join_status = 0; /* reset */ + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + +#ifdef CONFIG_DRVEXT_MODULE_WSC + drvext_assoc_fail_indicate(&adapter->drvextpriv); +#endif + + + +} + +/* +* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey +* @adapter: pointer to _adapter structure +*/ +void rtw_scan_timeout_handler(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + _irqL irqL; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + RTW_INFO(FUNC_ADPT_FMT" fw_state=%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv)); + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + _clr_fwstate_(pmlmepriv, WIFI_UNDER_SURVEY); + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +#ifdef CONFIG_IOCTL_CFG80211 + rtw_cfg80211_surveydone_event_callback(adapter); +#endif /* CONFIG_IOCTL_CFG80211 */ + + rtw_indicate_scan_done(adapter, _TRUE); + +#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_IOCTL_CFG80211) + rtw_cfg80211_indicate_scan_done_for_buddy(adapter, _TRUE); +#endif +} + +void rtw_mlme_reset_auto_scan_int(_adapter *adapter, u8 *reason) +{ +#if defined(CONFIG_RTW_MESH) && defined(CONFIG_DFS_MASTER) +#if CONFIG_RTW_MESH_OFFCH_CAND + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); +#endif +#endif + u8 u_ch; + u32 interval_ms = 0xffffffff; /* 0xffffffff: special value to make min() works well, also means no auto scan */ + + *reason = RTW_AUTO_SCAN_REASON_UNSPECIFIED; + rtw_mi_get_ch_setting_union(adapter, &u_ch, NULL, NULL); + + if (hal_chk_bw_cap(adapter, BW_CAP_40M) + && is_client_associated_to_ap(adapter) == _TRUE + && u_ch >= 1 && u_ch <= 14 + && adapter->registrypriv.wifi_spec + /* TODO: AP Connected is 40MHz capability? */ + ) { + interval_ms = rtw_min(interval_ms, 60 * 1000); + *reason |= RTW_AUTO_SCAN_REASON_2040_BSS; + } + +#ifdef CONFIG_RTW_MESH + #if CONFIG_RTW_MESH_OFFCH_CAND + if (adapter->mesh_cfg.peer_sel_policy.offch_find_int_ms + && rtw_mesh_offch_candidate_accepted(adapter) + #ifdef CONFIG_DFS_MASTER + && (!rfctl->radar_detect_ch || (IS_CH_WAITING(rfctl) && !IS_UNDER_CAC(rfctl))) + #endif + ) { + interval_ms = rtw_min(interval_ms, adapter->mesh_cfg.peer_sel_policy.offch_find_int_ms); + *reason |= RTW_AUTO_SCAN_REASON_MESH_OFFCH_CAND; + } + #endif +#endif /* CONFIG_RTW_MESH */ + + if (interval_ms == 0xffffffff) + interval_ms = 0; + + rtw_mlme_set_auto_scan_int(adapter, interval_ms); + return; +} + +void rtw_drv_scan_by_self(_adapter *padapter, u8 reason) +{ + struct sitesurvey_parm parm; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + int i; +#if 1 + u8 ssc_chk; + + ssc_chk = rtw_sitesurvey_condition_check(padapter, _FALSE); + if( ssc_chk == SS_DENY_BUSY_TRAFFIC) { + #ifdef CONFIG_LAYER2_ROAMING + if (rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE) && pmlmepriv->need_to_roam == _TRUE) + RTW_INFO(FUNC_ADPT_FMT" need to roam, don't care BusyTraffic\n", FUNC_ADPT_ARG(padapter)); + else + #endif + { + RTW_INFO(FUNC_ADPT_FMT" exit BusyTraffic\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + } + else if (ssc_chk != SS_ALLOW) + goto exit; + + if (!rtw_is_adapter_up(padapter)) + goto exit; +#else + if (rtw_is_scan_deny(padapter)) + goto exit; + + if (!rtw_is_adapter_up(padapter)) + goto exit; + + if (rtw_mi_busy_traffic_check(padapter)) { +#ifdef CONFIG_LAYER2_ROAMING + if (rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE) && pmlmepriv->need_to_roam == _TRUE) { + RTW_INFO("need to roam, don't care BusyTraffic\n"); + } else +#endif + { + RTW_INFO(FUNC_ADPT_FMT" exit BusyTraffic\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + } + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) && check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + RTW_INFO(FUNC_ADPT_FMT" WIFI_AP_STATE && WIFI_UNDER_WPS\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + if (check_fwstate(pmlmepriv, (WIFI_UNDER_SURVEY | WIFI_UNDER_LINKING)) == _TRUE) { + RTW_INFO(FUNC_ADPT_FMT" WIFI_UNDER_SURVEY|WIFI_UNDER_LINKING\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(padapter, (WIFI_UNDER_SURVEY | WIFI_UNDER_LINKING | WIFI_UNDER_WPS))) { + RTW_INFO(FUNC_ADPT_FMT", but buddy_intf is under scanning or linking or wps_phase\n", FUNC_ADPT_ARG(padapter)); + goto exit; + } +#endif +#endif + + RTW_INFO(FUNC_ADPT_FMT" reason:0x%02x\n", FUNC_ADPT_ARG(padapter), reason); + + /* only for 20/40 BSS */ + if (reason == RTW_AUTO_SCAN_REASON_2040_BSS) { + rtw_init_sitesurvey_parm(padapter, &parm); + for (i=0;i<14;i++) { + parm.ch[i].hw_value = i + 1; + parm.ch[i].flags = RTW_IEEE80211_CHAN_PASSIVE_SCAN; + } + parm.ch_num = 14; + rtw_set_802_11_bssid_list_scan(padapter, &parm); + goto exit; + } + +#if defined(CONFIG_RTW_WNM) || defined(CONFIG_RTW_80211K) + if ((reason == RTW_AUTO_SCAN_REASON_ROAM) + && (rtw_roam_nb_scan_list_set(padapter, &parm))) + goto exit; +#endif + + rtw_init_sitesurvey_parm(padapter, &parm); + parm.reason = reason; + rtw_set_802_11_bssid_list_scan(padapter, &parm); +exit: + return; +} + +static void rtw_auto_scan_handler(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 reason = RTW_AUTO_SCAN_REASON_UNSPECIFIED; + + rtw_mlme_reset_auto_scan_int(padapter, &reason); + +#ifdef CONFIG_P2P + if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE)) + goto exit; +#endif + +#ifdef CONFIG_TDLS + if (padapter->tdlsinfo.link_established == _TRUE) + goto exit; +#endif + + if (pmlmepriv->auto_scan_int_ms == 0 + || rtw_get_passing_time_ms(pmlmepriv->scan_start_time) < pmlmepriv->auto_scan_int_ms) + goto exit; + + rtw_drv_scan_by_self(padapter, reason); + +exit: + return; +} +static u8 is_drv_in_lps(_adapter *adapter) +{ + u8 is_in_lps = _FALSE; + + #ifdef CONFIG_LPS_LCLK_WD_TIMER /* to avoid leaving lps 32k frequently*/ + if ((adapter_to_pwrctl(adapter)->bFwCurrentInPSMode == _TRUE) + #ifdef CONFIG_BT_COEXIST + && (rtw_btcoex_IsBtControlLps(adapter) == _FALSE) + #endif + ) + is_in_lps = _TRUE; + #endif /* CONFIG_LPS_LCLK_WD_TIMER*/ + return is_in_lps; +} +void rtw_iface_dynamic_check_timer_handlder(_adapter *adapter) +{ + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + if (adapter->net_closed == _TRUE) + return; + #ifdef CONFIG_LPS_LCLK_WD_TIMER /* to avoid leaving lps 32k frequently*/ + if (is_drv_in_lps(adapter)) { + u8 bEnterPS; + + linked_status_chk(adapter, 1); + + bEnterPS = traffic_status_watchdog(adapter, 1); + if (bEnterPS) { + /* rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 0); */ + rtw_hal_dm_watchdog_in_lps(adapter); + } else { + /* call rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 0) in traffic_status_watchdog() */ + } + } + #endif /* CONFIG_LPS_LCLK_WD_TIMER */ + + /* auto site survey */ + rtw_auto_scan_handler(adapter); + +#ifdef CONFIG_AP_MODE + if (MLME_IS_AP(adapter)|| MLME_IS_MESH(adapter)) { + #ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + expire_timeout_chk(adapter); + #endif /* !CONFIG_ACTIVE_KEEP_ALIVE_CHECK */ + + #ifdef CONFIG_BMC_TX_RATE_SELECT + rtw_update_bmc_sta_tx_rate(adapter); + #endif /*CONFIG_BMC_TX_RATE_SELECT*/ + } +#endif /*CONFIG_AP_MODE*/ + + +#ifdef CONFIG_BR_EXT +if (!adapter_use_wds(adapter)) { +#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35)) + rcu_read_lock(); +#endif /* (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35)) */ + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) + if (adapter->pnetdev->br_port +#else /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */ + if (rcu_dereference(adapter->pnetdev->rx_handler_data) +#endif /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */ + && (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE) == _TRUE)) { + /* expire NAT2.5 entry */ + void nat25_db_expire(_adapter *priv); + nat25_db_expire(adapter); + + if (adapter->pppoe_connection_in_progress > 0) + adapter->pppoe_connection_in_progress--; + /* due to rtw_dynamic_check_timer_handlder() is called every 2 seconds */ + if (adapter->pppoe_connection_in_progress > 0) + adapter->pppoe_connection_in_progress--; + } + +#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35)) + rcu_read_unlock(); +#endif /* (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35)) */ +} +#endif /* CONFIG_BR_EXT */ + +} + +/*TP_avg(t) = (1/10) * TP_avg(t-1) + (9/10) * TP(t) MBps*/ +static void collect_sta_traffic_statistics(_adapter *adapter) +{ + struct macid_ctl_t *macid_ctl = &adapter->dvobj->macid_ctl; + struct sta_info *sta; + u64 curr_tx_bytes = 0, curr_rx_bytes = 0; + u32 curr_tx_mbytes = 0, curr_rx_mbytes = 0; + int i; + + for (i = 0; i < MACID_NUM_SW_LIMIT; i++) { + sta = macid_ctl->sta[i]; + if (sta && !is_broadcast_mac_addr(sta->cmn.mac_addr)) { + if (sta->sta_stats.last_tx_bytes > sta->sta_stats.tx_bytes) + sta->sta_stats.last_tx_bytes = sta->sta_stats.tx_bytes; + if (sta->sta_stats.last_rx_bytes > sta->sta_stats.rx_bytes) + sta->sta_stats.last_rx_bytes = sta->sta_stats.rx_bytes; + if (sta->sta_stats.last_rx_bc_bytes > sta->sta_stats.rx_bc_bytes) + sta->sta_stats.last_rx_bc_bytes = sta->sta_stats.rx_bc_bytes; + if (sta->sta_stats.last_rx_mc_bytes > sta->sta_stats.rx_mc_bytes) + sta->sta_stats.last_rx_mc_bytes = sta->sta_stats.rx_mc_bytes; + + curr_tx_bytes = sta->sta_stats.tx_bytes - sta->sta_stats.last_tx_bytes; + curr_rx_bytes = sta->sta_stats.rx_bytes - sta->sta_stats.last_rx_bytes; + sta->sta_stats.tx_tp_kbits = (curr_tx_bytes * 8 / 2) >> 10;/*Kbps*/ + sta->sta_stats.rx_tp_kbits = (curr_rx_bytes * 8 / 2) >> 10;/*Kbps*/ + + sta->sta_stats.smooth_tx_tp_kbits = (sta->sta_stats.smooth_tx_tp_kbits * 6 / 10) + (sta->sta_stats.tx_tp_kbits * 4 / 10);/*Kbps*/ + sta->sta_stats.smooth_rx_tp_kbits = (sta->sta_stats.smooth_rx_tp_kbits * 6 / 10) + (sta->sta_stats.rx_tp_kbits * 4 / 10);/*Kbps*/ + + curr_tx_mbytes = (curr_tx_bytes / 2) >> 20;/*MBps*/ + curr_rx_mbytes = (curr_rx_bytes / 2) >> 20;/*MBps*/ + + sta->cmn.tx_moving_average_tp = + (sta->cmn.tx_moving_average_tp / 10) + (curr_tx_mbytes * 9 / 10); /*MBps*/ + + sta->cmn.rx_moving_average_tp = + (sta->cmn.rx_moving_average_tp / 10) + (curr_rx_mbytes * 9 /10); /*MBps*/ + + rtw_collect_bcn_info(sta->padapter); + + if (adapter->bsta_tp_dump) + dump_sta_traffic(RTW_DBGDUMP, adapter, sta); + + sta->sta_stats.last_tx_bytes = sta->sta_stats.tx_bytes; + sta->sta_stats.last_rx_bytes = sta->sta_stats.rx_bytes; + sta->sta_stats.last_rx_bc_bytes = sta->sta_stats.rx_bc_bytes; + sta->sta_stats.last_rx_mc_bytes = sta->sta_stats.rx_mc_bytes; + } + } +} + +void rtw_sta_traffic_info(void *sel, _adapter *adapter) +{ + struct macid_ctl_t *macid_ctl = &adapter->dvobj->macid_ctl; + struct sta_info *sta; + int i; + + for (i = 0; i < MACID_NUM_SW_LIMIT; i++) { + sta = macid_ctl->sta[i]; + if (sta && !is_broadcast_mac_addr(sta->cmn.mac_addr)) + dump_sta_traffic(sel, adapter, sta); + } +} + +/*#define DBG_TRAFFIC_STATISTIC*/ +static void collect_traffic_statistics(_adapter *padapter) +{ + struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter); + + /*_rtw_memset(&pdvobjpriv->traffic_stat, 0, sizeof(struct rtw_traffic_statistics));*/ + + /* Tx bytes reset*/ + pdvobjpriv->traffic_stat.tx_bytes = 0; + pdvobjpriv->traffic_stat.tx_pkts = 0; + pdvobjpriv->traffic_stat.tx_drop = 0; + + /* Rx bytes reset*/ + pdvobjpriv->traffic_stat.rx_bytes = 0; + pdvobjpriv->traffic_stat.rx_pkts = 0; + pdvobjpriv->traffic_stat.rx_drop = 0; + + rtw_mi_traffic_statistics(padapter); + + /* Calculate throughput in last interval */ + pdvobjpriv->traffic_stat.cur_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes - pdvobjpriv->traffic_stat.last_tx_bytes; + pdvobjpriv->traffic_stat.cur_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes - pdvobjpriv->traffic_stat.last_rx_bytes; + pdvobjpriv->traffic_stat.last_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes; + pdvobjpriv->traffic_stat.last_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes; + + pdvobjpriv->traffic_stat.cur_tx_tp = (u32)(pdvobjpriv->traffic_stat.cur_tx_bytes * 8 / 2 / 1024 / 1024);/*Mbps*/ + pdvobjpriv->traffic_stat.cur_rx_tp = (u32)(pdvobjpriv->traffic_stat.cur_rx_bytes * 8 / 2 / 1024 / 1024);/*Mbps*/ + + #ifdef DBG_TRAFFIC_STATISTIC + RTW_INFO("\n========================\n"); + RTW_INFO("cur_tx_bytes:%lld\n", pdvobjpriv->traffic_stat.cur_tx_bytes); + RTW_INFO("cur_rx_bytes:%lld\n", pdvobjpriv->traffic_stat.cur_rx_bytes); + + RTW_INFO("last_tx_bytes:%lld\n", pdvobjpriv->traffic_stat.last_tx_bytes); + RTW_INFO("last_rx_bytes:%lld\n", pdvobjpriv->traffic_stat.last_rx_bytes); + + RTW_INFO("cur_tx_tp:%d (Mbps)\n", pdvobjpriv->traffic_stat.cur_tx_tp); + RTW_INFO("cur_rx_tp:%d (Mbps)\n", pdvobjpriv->traffic_stat.cur_rx_tp); + #endif + +#ifdef CONFIG_RTW_NAPI +#ifdef CONFIG_RTW_NAPI_DYNAMIC + dynamic_napi_th_chk (padapter); +#endif /* CONFIG_RTW_NAPI_DYNAMIC */ +#endif + +} + +void rtw_dynamic_check_timer_handlder(void *ctx) +{ + struct dvobj_priv *pdvobj = (struct dvobj_priv *)ctx; + _adapter *adapter = dvobj_get_primary_adapter(pdvobj); + + if (!adapter) + goto exit; + +#if (MP_DRIVER == 1) + if (adapter->registrypriv.mp_mode == 1 && adapter->mppriv.mp_dm == 0) { /* for MP ODM dynamic Tx power tracking */ + /* RTW_INFO("%s mp_dm =0 return\n", __func__); */ + goto exit; + } +#endif + + if (!rtw_is_hw_init_completed(adapter)) + goto exit; + + if (RTW_CANNOT_RUN(adapter)) + goto exit; + + collect_traffic_statistics(adapter); + collect_sta_traffic_statistics(adapter); + rtw_mi_dynamic_check_timer_handlder(adapter); + + if (!is_drv_in_lps(adapter)) + rtw_dynamic_chk_wk_cmd(adapter); + +exit: + _set_timer(&pdvobj->dynamic_chk_timer, 2000); +} + + +#ifdef CONFIG_SET_SCAN_DENY_TIMER +inline bool rtw_is_scan_deny(_adapter *adapter) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + return (ATOMIC_READ(&mlmepriv->set_scan_deny) != 0) ? _TRUE : _FALSE; +} + +inline void rtw_clear_scan_deny(_adapter *adapter) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + ATOMIC_SET(&mlmepriv->set_scan_deny, 0); + if (0) + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); +} + +void rtw_set_scan_deny_timer_hdl(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + + rtw_clear_scan_deny(adapter); +} +void rtw_set_scan_deny(_adapter *adapter, u32 ms) +{ + struct mlme_priv *mlmepriv = &adapter->mlmepriv; + if (0) + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); + ATOMIC_SET(&mlmepriv->set_scan_deny, 1); + _set_timer(&mlmepriv->set_scan_deny_timer, ms); +} +#endif + +#ifdef CONFIG_LAYER2_ROAMING +/* +* Select a new roaming candidate from the original @param candidate and @param competitor +* @return _TRUE: candidate is updated +* @return _FALSE: candidate is not updated +*/ +static int rtw_check_roaming_candidate(struct mlme_priv *mlme + , struct wlan_network **candidate, struct wlan_network *competitor) +{ + int updated = _FALSE; + _adapter *adapter = container_of(mlme, _adapter, mlmepriv); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + u8 ch = competitor->network.Configuration.DSConfig; + + if (rtw_chset_search_ch(chset, ch) < 0) + goto exit; + if (IS_DFS_SLAVE_WITH_RD(rfctl) + && !rtw_rfctl_dfs_domain_unknown(rfctl) + && rtw_chset_is_ch_non_ocp(chset, ch)) + goto exit; + +#if defined(CONFIG_RTW_REPEATER_SON) && (!defined(CONFIG_RTW_REPEATER_SON_ROOT)) + if (rtw_rson_isupdate_roamcan(mlme, candidate, competitor)) + goto update; + goto exit; +#endif + + if (is_same_ess(&competitor->network, &mlme->cur_network.network) == _FALSE) + goto exit; + + if (rtw_is_desired_network(adapter, competitor) == _FALSE) + goto exit; + +#ifdef CONFIG_RTW_ROAM_QUICKSCAN + if (competitor->network.PhyInfo.SignalStrength > CONFIG_RTW_ROAM_QUICKSCAN_TH) + adapter->mlmeextpriv.quickscan_next = _TRUE; +#endif + +#ifdef CONFIG_LAYER2_ROAMING + if (mlme->need_to_roam == _FALSE) + goto exit; +#endif + + RTW_INFO("roam candidate:%s %s("MAC_FMT", ch%3u) rssi:%d dBm, age:%5d\n", + (competitor == mlme->cur_network_scanned) ? "*" : " " , + competitor->network.Ssid.Ssid, + MAC_ARG(competitor->network.MacAddress), + competitor->network.Configuration.DSConfig, + (int)competitor->network.Rssi, + rtw_get_passing_time_ms(competitor->last_scanned) + ); + + /* got specific addr to roam */ + if (!is_zero_mac_addr(mlme->roam_tgt_addr)) { + if (_rtw_memcmp(mlme->roam_tgt_addr, competitor->network.MacAddress, ETH_ALEN) == _TRUE) + goto update; + else + goto exit; + } + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_chk_flags(adapter, RTW_FT_PEER_EN)) { + if (rtw_ft_chk_roaming_candidate(adapter, competitor) == _FALSE) + goto exit; + } + +#ifdef CONFIG_RTW_WNM + if (rtw_wnm_btm_diff_bss(adapter) && + rtw_wnm_btm_roam_candidate(adapter, competitor)) { + goto update; + } +#endif +#endif + +#if 1 + if (rtw_get_passing_time_ms(competitor->last_scanned) >= mlme->roam_scanr_exp_ms) + goto exit; + + if (competitor->network.Rssi - mlme->cur_network_scanned->network.Rssi < mlme->roam_rssi_diff_th) + goto exit; + + if (*candidate != NULL && (*candidate)->network.Rssi >= competitor->network.Rssi) + goto exit; +#else + goto exit; +#endif + +update: + *candidate = competitor; + updated = _TRUE; + +exit: + return updated; +} + +int rtw_select_roaming_candidate(struct mlme_priv *mlme) +{ + _irqL irqL; + int ret = _FAIL; + _list *phead; + _adapter *adapter; + _queue *queue = &(mlme->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct wlan_network *candidate = NULL; + + if (mlme->cur_network_scanned == NULL) { + rtw_warn_on(1); + goto exit; + } + + _enter_critical_bh(&(mlme->scanned_queue.lock), &irqL); + phead = get_list_head(queue); + adapter = (_adapter *)mlme->nic_hdl; + + mlme->pscanned = get_next(phead); + + while (!rtw_end_of_queue_search(phead, mlme->pscanned)) { + + pnetwork = LIST_CONTAINOR(mlme->pscanned, struct wlan_network, list); + if (pnetwork == NULL) { + ret = _FAIL; + goto exit; + } + + mlme->pscanned = get_next(mlme->pscanned); + + if (0) + RTW_INFO("%s("MAC_FMT", ch%u) rssi:%d\n" + , pnetwork->network.Ssid.Ssid + , MAC_ARG(pnetwork->network.MacAddress) + , pnetwork->network.Configuration.DSConfig + , (int)pnetwork->network.Rssi); + + rtw_check_roaming_candidate(mlme, &candidate, pnetwork); + + } + + if (candidate == NULL) { + /* if parent note lost the path to root and there is no other cadidate, report disconnection */ +#if defined(CONFIG_RTW_REPEATER_SON) && (!defined(CONFIG_RTW_REPEATER_SON_ROOT)) + struct rtw_rson_struct rson_curr; + u8 rson_score; + + rtw_get_rson_struct(&(mlme->cur_network_scanned->network), &rson_curr); + rson_score = rtw_cal_rson_score(&rson_curr, mlme->cur_network_scanned->network.Rssi); + if (check_fwstate(mlme, WIFI_ASOC_STATE) + && ((rson_score == RTW_RSON_SCORE_NOTCNNT) + || (rson_score == RTW_RSON_SCORE_NOTSUP))) + receive_disconnect(adapter, mlme->cur_network_scanned->network.MacAddress + , WLAN_REASON_EXPIRATION_CHK, _FALSE); +#endif + RTW_INFO("%s: return _FAIL(candidate == NULL)\n", __FUNCTION__); + ret = _FAIL; + goto exit; + } else { +#if defined(CONFIG_RTW_REPEATER_SON) && (!defined(CONFIG_RTW_REPEATER_SON_ROOT)) + struct rtw_rson_struct rson_curr; + u8 rson_score; + + rtw_get_rson_struct(&(candidate->network), &rson_curr); + rson_score = rtw_cal_rson_score(&rson_curr, candidate->network.Rssi); + RTW_INFO("%s: candidate: %s("MAC_FMT", ch:%u) rson_score:%d\n", __FUNCTION__, + candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress), + candidate->network.Configuration.DSConfig, rson_score); +#else + RTW_INFO("%s: candidate: %s("MAC_FMT", ch:%u)\n", __FUNCTION__, + candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress), + candidate->network.Configuration.DSConfig); +#endif + mlme->roam_network = candidate; + + if (_rtw_memcmp(candidate->network.MacAddress, mlme->roam_tgt_addr, ETH_ALEN) == _TRUE) + _rtw_memset(mlme->roam_tgt_addr, 0, ETH_ALEN); + } + + ret = _SUCCESS; +exit: + _exit_critical_bh(&(mlme->scanned_queue.lock), &irqL); + + return ret; +} +#endif /* CONFIG_LAYER2_ROAMING */ + +/* +* Select a new join candidate from the original @param candidate and @param competitor +* @return _TRUE: candidate is updated +* @return _FALSE: candidate is not updated +*/ +static int rtw_check_join_candidate(struct mlme_priv *mlme + , struct wlan_network **candidate, struct wlan_network *competitor) +{ + int updated = _FALSE; + _adapter *adapter = container_of(mlme, _adapter, mlmepriv); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + u8 ch = competitor->network.Configuration.DSConfig; + + if (rtw_chset_search_ch(chset, ch) < 0) + goto exit; + if (IS_DFS_SLAVE_WITH_RD(rfctl) + && !rtw_rfctl_dfs_domain_unknown(rfctl) + && rtw_chset_is_ch_non_ocp(chset, ch)) + goto exit; + +#if defined(CONFIG_RTW_REPEATER_SON) && (!defined(CONFIG_RTW_REPEATER_SON_ROOT)) + s16 rson_score; + struct rtw_rson_struct rson_data; + + if (rtw_rson_choose(candidate, competitor)) { + *candidate = competitor; + rtw_get_rson_struct(&((*candidate)->network), &rson_data); + rson_score = rtw_cal_rson_score(&rson_data, (*candidate)->network.Rssi); + RTW_INFO("[assoc_ssid:%s] new candidate: %s("MAC_FMT", ch%u) rson_score:%d\n", + mlme->assoc_ssid.Ssid, + (*candidate)->network.Ssid.Ssid, + MAC_ARG((*candidate)->network.MacAddress), + (*candidate)->network.Configuration.DSConfig, + rson_score); + return _TRUE; + } + return _FALSE; +#endif + + /* check bssid, if needed */ + if (mlme->assoc_by_bssid == _TRUE) { + if (_rtw_memcmp(competitor->network.MacAddress, mlme->assoc_bssid, ETH_ALEN) == _FALSE) + goto exit; + } + + /* check ssid, if needed */ + if (mlme->assoc_ssid.Ssid[0] && mlme->assoc_ssid.SsidLength) { + if (competitor->network.Ssid.SsidLength != mlme->assoc_ssid.SsidLength + || _rtw_memcmp(competitor->network.Ssid.Ssid, mlme->assoc_ssid.Ssid, mlme->assoc_ssid.SsidLength) == _FALSE + ) + goto exit; + } + + if (rtw_is_desired_network(adapter, competitor) == _FALSE) + goto exit; + +#ifdef CONFIG_LAYER2_ROAMING + if (rtw_to_roam(adapter) > 0) { + if (rtw_get_passing_time_ms(competitor->last_scanned) >= mlme->roam_scanr_exp_ms + || is_same_ess(&competitor->network, &mlme->cur_network.network) == _FALSE + ) + goto exit; + } +#endif + + if (*candidate == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) { + *candidate = competitor; + updated = _TRUE; + } + + if (updated) { + RTW_INFO("[by_bssid:%u][assoc_ssid:%s][to_roam:%u] " + "new candidate: %s("MAC_FMT", ch%u) rssi:%d dBm\n", + mlme->assoc_by_bssid, + mlme->assoc_ssid.Ssid, + rtw_to_roam(adapter), + (*candidate)->network.Ssid.Ssid, + MAC_ARG((*candidate)->network.MacAddress), + (*candidate)->network.Configuration.DSConfig, + (int)(*candidate)->network.Rssi + ); + } + +exit: + return updated; +} + +/* +Calling context: +The caller of the sub-routine will be in critical section... + +The caller must hold the following spinlock + +pmlmepriv->lock + + +*/ + +int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv) +{ + _irqL irqL; + int ret; + _list *phead; + _adapter *adapter; + _queue *queue = &(pmlmepriv->scanned_queue); + struct wlan_network *pnetwork = NULL; + struct wlan_network *candidate = NULL; +#ifdef CONFIG_ANTENNA_DIVERSITY + u8 bSupportAntDiv = _FALSE; +#endif + + adapter = (_adapter *)pmlmepriv->nic_hdl; + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + +#ifdef CONFIG_LAYER2_ROAMING + if (pmlmepriv->roam_network) { + candidate = pmlmepriv->roam_network; + goto candidate_exist; + } +#endif + + phead = get_list_head(queue); + pmlmepriv->pscanned = get_next(phead); + + while (!rtw_end_of_queue_search(phead, pmlmepriv->pscanned)) { + + pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list); + if (pnetwork == NULL) { + ret = _FAIL; + goto exit; + } + + pmlmepriv->pscanned = get_next(pmlmepriv->pscanned); + + if (0) + RTW_INFO("%s("MAC_FMT", ch%u) rssi:%d\n" + , pnetwork->network.Ssid.Ssid + , MAC_ARG(pnetwork->network.MacAddress) + , pnetwork->network.Configuration.DSConfig + , (int)pnetwork->network.Rssi); + + rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork); + + } + + if (candidate == NULL) { + RTW_INFO("%s: return _FAIL(candidate == NULL)\n", __FUNCTION__); +#ifdef CONFIG_WOWLAN + _clr_fwstate_(pmlmepriv, WIFI_ASOC_STATE | WIFI_UNDER_LINKING); +#endif + ret = _FAIL; + goto exit; + } else { + RTW_INFO("%s: candidate: %s("MAC_FMT", ch:%u)\n", __FUNCTION__, + candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress), + candidate->network.Configuration.DSConfig); + goto candidate_exist; + } + +candidate_exist: + + /* check for situation of WIFI_ASOC_STATE */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) { + RTW_INFO("%s: WIFI_ASOC_STATE while ask_for_joinbss!!!\n", __FUNCTION__); + +#if 0 /* for WPA/WPA2 authentication, wpa_supplicant will expect authentication from AP, it is needed to reconnect AP... */ + if (is_same_network(&pmlmepriv->cur_network.network, &candidate->network)) { + RTW_INFO("%s: WIFI_ASOC_STATE and is same network, it needn't join again\n", __FUNCTION__); + + rtw_indicate_connect(adapter);/* rtw_indicate_connect again */ + + ret = 2; + goto exit; + } else +#endif + { + rtw_disassoc_cmd(adapter, 0, 0); + rtw_indicate_disconnect(adapter, 0, _FALSE); + rtw_free_assoc_resources_cmd(adapter, _TRUE, 0); + } + } + +#ifdef CONFIG_ANTENNA_DIVERSITY + rtw_hal_get_def_var(adapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv)); + if (_TRUE == bSupportAntDiv) { + u8 CurrentAntenna; + rtw_hal_get_odm_var(adapter, HAL_ODM_ANTDIV_SELECT, &(CurrentAntenna), NULL); + RTW_INFO("#### Opt_Ant_(%s) , cur_Ant(%s)\n", + (MAIN_ANT == candidate->network.PhyInfo.Optimum_antenna) ? "MAIN_ANT" : "AUX_ANT", + (MAIN_ANT == CurrentAntenna) ? "MAIN_ANT" : "AUX_ANT" + ); + } +#endif + set_fwstate(pmlmepriv, WIFI_UNDER_LINKING); + ret = rtw_joinbss_cmd(adapter, candidate); + +exit: + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + + return ret; +} + +sint rtw_set_auth(_adapter *adapter, struct security_priv *psecuritypriv) +{ + struct cmd_obj *pcmd; + struct setauth_parm *psetauthparm; + struct cmd_priv *pcmdpriv = &(adapter->cmdpriv); + sint res = _SUCCESS; + + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; /* try again */ + goto exit; + } + + psetauthparm = (struct setauth_parm *)rtw_zmalloc(sizeof(struct setauth_parm)); + if (psetauthparm == NULL) { + rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + _rtw_memset(psetauthparm, 0, sizeof(struct setauth_parm)); + psetauthparm->mode = (unsigned char)psecuritypriv->dot11AuthAlgrthm; + + pcmd->cmdcode = CMD_SET_AUTH; + pcmd->parmbuf = (unsigned char *)psetauthparm; + pcmd->cmdsz = (sizeof(struct setauth_parm)); + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + + _rtw_init_listhead(&pcmd->list); + + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + +exit: + + + return res; + +} + + +sint rtw_set_key(_adapter *adapter, struct security_priv *psecuritypriv, sint keyid, u8 set_tx, bool enqueue) +{ + u8 keylen; + struct cmd_obj *pcmd; + struct setkey_parm *psetkeyparm; + struct cmd_priv *pcmdpriv = &(adapter->cmdpriv); + sint res = _SUCCESS; + + + psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm)); + if (psetkeyparm == NULL) { + res = _FAIL; + goto exit; + } + _rtw_memset(psetkeyparm, 0, sizeof(struct setkey_parm)); + + if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { + psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy; + } else { + psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm; + + } + psetkeyparm->keyid = (u8)keyid;/* 0~3 */ + psetkeyparm->set_tx = set_tx; + if (is_wep_enc(psetkeyparm->algorithm)) + adapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid); + + RTW_INFO("==> rtw_set_key algorithm(%x),keyid(%x),key_mask(%x)\n", psetkeyparm->algorithm, psetkeyparm->keyid, adapter->securitypriv.key_mask); + + switch (psetkeyparm->algorithm) { + + case _WEP40_: + keylen = 5; + _rtw_memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen); + break; + case _WEP104_: + keylen = 13; + _rtw_memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen); + break; + case _TKIP_: + keylen = 16; + _rtw_memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen); + break; + case _AES_: + case _GCMP_: + keylen = 16; + _rtw_memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen); + break; + case _GCMP_256_: + case _CCMP_256_: + keylen = 32; + _rtw_memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen); + break; + default: + res = _FAIL; + rtw_mfree((unsigned char *)psetkeyparm, sizeof(struct setkey_parm)); + goto exit; + } + + + if (enqueue) { + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + rtw_mfree((unsigned char *)psetkeyparm, sizeof(struct setkey_parm)); + res = _FAIL; /* try again */ + goto exit; + } + + pcmd->cmdcode =CMD_SET_KEY; + pcmd->parmbuf = (u8 *)psetkeyparm; + pcmd->cmdsz = (sizeof(struct setkey_parm)); + pcmd->rsp = NULL; + pcmd->rspsz = 0; + + _rtw_init_listhead(&pcmd->list); + + /* _rtw_init_sema(&(pcmd->cmd_sem), 0); */ + + res = rtw_enqueue_cmd(pcmdpriv, pcmd); + } else { + setkey_hdl(adapter, (u8 *)psetkeyparm); + rtw_mfree((u8 *) psetkeyparm, sizeof(struct setkey_parm)); + } +exit: + return res; + +} + +#ifdef CONFIG_WMMPS_STA +/* + * rtw_uapsd_use_default_setting + * This function is used for setting default uapsd max sp length to uapsd_max_sp_len + * in qos_priv data structure from registry. In additional, it will also map default uapsd + * ac to each uapsd TID, delivery-enabled and trigger-enabled of corresponding TID. + * + * Arguments: + * @padapter: _adapter pointer. + * + * Auther: Arvin Liu + * Date: 2017/05/03 + */ +void rtw_uapsd_use_default_setting(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + + if (pregistrypriv->uapsd_ac_enable != 0) { + pqospriv->uapsd_max_sp_len = pregistrypriv->uapsd_max_sp_len; + + CLEAR_FLAGS(pqospriv->uapsd_tid); + CLEAR_FLAGS(pqospriv->uapsd_tid_delivery_enabled); + CLEAR_FLAGS(pqospriv->uapsd_tid_trigger_enabled); + + /* check the uapsd setting of AC_VO from registry then map these setting to each TID if necessary */ + if(TEST_FLAG(pregistrypriv->uapsd_ac_enable, DRV_CFG_UAPSD_VO)) { + SET_FLAG(pqospriv->uapsd_tid, WMM_TID7); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID7); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID7); + SET_FLAG(pqospriv->uapsd_tid, WMM_TID6); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID6); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID6); + } + + /* check the uapsd setting of AC_VI from registry then map these setting to each TID if necessary */ + if(TEST_FLAG(pregistrypriv->uapsd_ac_enable, DRV_CFG_UAPSD_VI)) { + SET_FLAG(pqospriv->uapsd_tid, WMM_TID5); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID5); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID5); + SET_FLAG(pqospriv->uapsd_tid, WMM_TID4); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID4); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID4); + } + + /* check the uapsd setting of AC_BK from registry then map these setting to each TID if necessary */ + if(TEST_FLAG(pregistrypriv->uapsd_ac_enable, DRV_CFG_UAPSD_BK)) { + SET_FLAG(pqospriv->uapsd_tid, WMM_TID2); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID2); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID2); + SET_FLAG(pqospriv->uapsd_tid, WMM_TID1); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID1); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID1); + } + + /* check the uapsd setting of AC_BE from registry then map these setting to each TID if necessary */ + if(TEST_FLAG(pregistrypriv->uapsd_ac_enable, DRV_CFG_UAPSD_BE)) { + SET_FLAG(pqospriv->uapsd_tid, WMM_TID3); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID3); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID3); + SET_FLAG(pqospriv->uapsd_tid, WMM_TID0); + SET_FLAG(pqospriv->uapsd_tid_delivery_enabled, WMM_TID0); + SET_FLAG(pqospriv->uapsd_tid_trigger_enabled, WMM_TID0); + } + + RTW_INFO("[WMMPS] UAPSD MAX SP Len = 0x%02x, UAPSD TID enabled = 0x%02x\n", + pqospriv->uapsd_max_sp_len, (u8)pqospriv->uapsd_tid); + } + +} + +/* + * rtw_is_wmmps_mode + * This function is used for checking whether Driver and an AP support uapsd function or not. + * If both of them support uapsd function, it will return true. Otherwise returns false. + * + * Arguments: + * @padapter: _adapter pointer. + * + * Auther: Arvin Liu + * Date: 2017/06/12 + */ +bool rtw_is_wmmps_mode(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + + if ((pqospriv->uapsd_ap_supported) && ((pqospriv->uapsd_tid & BIT_MASK_TID_TC) != 0)) + return _TRUE; + + return _FALSE; +} +#endif /* CONFIG_WMMPS_STA */ + +/* adjust IEs for rtw_joinbss_cmd in WMM */ +int rtw_restruct_wmm_ie(_adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len) +{ +#ifdef CONFIG_WMMPS_STA + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; +#endif /* CONFIG_WMMPS_STA */ + unsigned int ielength = 0; + unsigned int i, j; + u8 qos_info = 0; + + i = 12; /* after the fixed IE */ + while (i < in_len) { + ielength = initial_out_len; + + if (in_ie[i] == 0xDD && in_ie[i + 2] == 0x00 && in_ie[i + 3] == 0x50 && in_ie[i + 4] == 0xF2 && in_ie[i + 5] == 0x02 && i + 5 < in_len) { /* WMM element ID and OUI */ + + /* Append WMM IE to the last index of out_ie */ +#if 0 + for (j = i; j < i + (in_ie[i + 1] + 2); j++) { + out_ie[ielength] = in_ie[j]; + ielength++; + } + out_ie[initial_out_len + 8] = 0x00; /* force the QoS Info Field to be zero */ +#endif + + for (j = i; j < i + 9; j++) { + out_ie[ielength] = in_ie[j]; + ielength++; + } + out_ie[initial_out_len + 1] = 0x07; + out_ie[initial_out_len + 6] = 0x00; + +#ifdef CONFIG_WMMPS_STA + switch(pqospriv->uapsd_max_sp_len) { + case NO_LIMIT: + /* do nothing */ + break; + case TWO_MSDU: + SET_FLAG(qos_info, BIT5); + break; + case FOUR_MSDU: + SET_FLAG(qos_info, BIT6); + break; + case SIX_MSDU: + SET_FLAG(qos_info, BIT5); + SET_FLAG(qos_info, BIT6); + break; + default: + /* do nothing */ + break; + }; + + /* check TID7 and TID6 for AC_VO to set corresponding Qos_info bit in WMM IE */ + if((TEST_FLAG(pqospriv->uapsd_tid, WMM_TID7)) && (TEST_FLAG(pqospriv->uapsd_tid, WMM_TID6))) + SET_FLAG(qos_info, WMM_IE_UAPSD_VO); + /* check TID5 and TID4 for AC_VI to set corresponding Qos_info bit in WMM IE */ + if((TEST_FLAG(pqospriv->uapsd_tid, WMM_TID5)) && (TEST_FLAG(pqospriv->uapsd_tid, WMM_TID4))) + SET_FLAG(qos_info, WMM_IE_UAPSD_VI); + /* check TID2 and TID1 for AC_BK to set corresponding Qos_info bit in WMM IE */ + if((TEST_FLAG(pqospriv->uapsd_tid, WMM_TID2)) && (TEST_FLAG(pqospriv->uapsd_tid, WMM_TID1))) + SET_FLAG(qos_info, WMM_IE_UAPSD_BK); + /* check TID3 and TID0 for AC_BE to set corresponding Qos_info bit in WMM IE */ + if((TEST_FLAG(pqospriv->uapsd_tid, WMM_TID3)) && (TEST_FLAG(pqospriv->uapsd_tid, WMM_TID0))) + SET_FLAG(qos_info, WMM_IE_UAPSD_BE); +#endif /* CONFIG_WMMPS_STA */ + + out_ie[initial_out_len + 8] = qos_info; + + break; + } + + i += (in_ie[i + 1] + 2); /* to the next IE element */ + } + + return ielength; + +} + + +/* + * Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.) + * Added by Annie, 2006-05-07. + * + * Search by BSSID, + * Return Value: + * -1 :if there is no pre-auth key in the table + * >=0 :if there is pre-auth key, and return the entry id + * + * */ + +static int SecIsInPMKIDList(_adapter *Adapter, u8 *bssid) +{ + struct security_priv *psecuritypriv = &Adapter->securitypriv; + int i = 0; + + do { + if ((psecuritypriv->PMKIDList[i].bUsed) && + (_rtw_memcmp(psecuritypriv->PMKIDList[i].Bssid, bssid, ETH_ALEN) == _TRUE)) + break; + else { + i++; + /* continue; */ + } + + } while (i < NUM_PMKID_CACHE); + + if (i == NUM_PMKID_CACHE) { + i = -1;/* Could not find. */ + } else { + /* There is one Pre-Authentication Key for the specific BSSID. */ + } + + return i; + +} + +int rtw_cached_pmkid(_adapter *Adapter, u8 *bssid) +{ + return SecIsInPMKIDList(Adapter, bssid); +} + +int rtw_rsn_sync_pmkid(_adapter *adapter, u8 *ie, uint ie_len, int i_ent) +{ + struct security_priv *sec = &adapter->securitypriv; + struct rsne_info info; + u8 gm_cs[4]; + int i; + + rtw_rsne_info_parse(ie, ie_len, &info); + + if (info.err) { + RTW_WARN(FUNC_ADPT_FMT" rtw_rsne_info_parse error\n" + , FUNC_ADPT_ARG(adapter)); + return 0; + } + + if (i_ent < 0 && info.pmkid_cnt == 0) + goto exit; + + if (info.pmkid_list == NULL) + goto exit; + + if (i_ent >= 0 && info.pmkid_cnt == 1 && _rtw_memcmp(info.pmkid_list, sec->PMKIDList[i_ent].PMKID, 16)) { + RTW_INFO(FUNC_ADPT_FMT" has carried the same PMKID:"KEY_FMT"\n" + , FUNC_ADPT_ARG(adapter), KEY_ARG(&sec->PMKIDList[i_ent].PMKID)); + goto exit; + } + + /* bakcup group mgmt cs */ + if (info.gmcs) + _rtw_memcpy(gm_cs, info.gmcs, 4); + + if (info.pmkid_cnt) { + RTW_INFO(FUNC_ADPT_FMT" remove original PMKID, count:%u\n" + , FUNC_ADPT_ARG(adapter), info.pmkid_cnt); + for (i = 0; i < info.pmkid_cnt; i++) + RTW_INFO(" "KEY_FMT"\n", KEY_ARG(info.pmkid_list + i * 16)); + } + + if (i_ent >= 0) { + RTW_INFO(FUNC_ADPT_FMT" append PMKID:"KEY_FMT"\n" + , FUNC_ADPT_ARG(adapter), KEY_ARG(sec->PMKIDList[i_ent].PMKID)); + + info.pmkid_cnt = 1; /* update new pmkid_cnt */ + _rtw_memcpy(info.pmkid_list, sec->PMKIDList[i_ent].PMKID, 16); + } else + info.pmkid_cnt = 0; /* update new pmkid_cnt */ + + RTW_PUT_LE16(info.pmkid_list - 2, info.pmkid_cnt); + if (info.gmcs) + _rtw_memcpy(info.pmkid_list + 16 * info.pmkid_cnt, gm_cs, 4); + + ie_len = 1 + 1 + 2 + 4 + + 2 + 4 * info.pcs_cnt + + 2 + 4 * info.akm_cnt + + 2 + + 2 + 16 * info.pmkid_cnt + + (info.gmcs ? 4 : 0) + ; + + ie[1] = (u8)(ie_len - 2); + +exit: + return ie_len; +} + +sint rtw_restruct_sec_ie(_adapter *adapter, u8 *out_ie) +{ + u8 authmode = 0x0; + uint ielength = 0; + int iEntry; + + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct security_priv *psecuritypriv = &adapter->securitypriv; + uint ndisauthmode = psecuritypriv->ndisauthtype; + + if ((ndisauthmode == Ndis802_11AuthModeWPA) || (ndisauthmode == Ndis802_11AuthModeWPAPSK)) + authmode = _WPA_IE_ID_; + if ((ndisauthmode == Ndis802_11AuthModeWPA2) || (ndisauthmode == Ndis802_11AuthModeWPA2PSK)) + authmode = _WPA2_IE_ID_; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) { + _rtw_memcpy(out_ie, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len); + ielength = psecuritypriv->wps_ie_len; + + } else if ((authmode == _WPA_IE_ID_) || (authmode == _WPA2_IE_ID_)) { + /* copy RSN or SSN */ + _rtw_memcpy(out_ie, psecuritypriv->supplicant_ie, psecuritypriv->supplicant_ie[1] + 2); + /* debug for CONFIG_IEEE80211W + { + int jj; + printk("supplicant_ie_length=%d &&&&&&&&&&&&&&&&&&&\n", psecuritypriv->supplicant_ie[1]+2); + for(jj=0; jj < psecuritypriv->supplicant_ie[1]+2; jj++) + printk(" %02x ", psecuritypriv->supplicant_ie[jj]); + printk("\n"); + }*/ + ielength = psecuritypriv->supplicant_ie[1] + 2; + rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie); + } + + if (authmode == WLAN_EID_RSN) { + iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid); + ielength = rtw_rsn_sync_pmkid(adapter, out_ie, ielength, iEntry); + } + + return ielength; +} + +void rtw_init_registrypriv_dev_network(_adapter *adapter) +{ + struct registry_priv *pregistrypriv = &adapter->registrypriv; + WLAN_BSSID_EX *pdev_network = &pregistrypriv->dev_network; + u8 *myhwaddr = adapter_mac_addr(adapter); + + + _rtw_memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN); + + _rtw_memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(NDIS_802_11_SSID)); + + pdev_network->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION); + pdev_network->Configuration.BeaconPeriod = 100; +} + +void rtw_update_registrypriv_dev_network(_adapter *adapter) +{ + int sz = 0; + struct registry_priv *pregistrypriv = &adapter->registrypriv; + WLAN_BSSID_EX *pdev_network = &pregistrypriv->dev_network; + struct security_priv *psecuritypriv = &adapter->securitypriv; + struct wlan_network *cur_network = &adapter->mlmepriv.cur_network; + /* struct xmit_priv *pxmitpriv = &adapter->xmitpriv; */ + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + + +#if 0 + pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense; + pxmitpriv->vcs = pregistrypriv->vcs_type; + pxmitpriv->vcs_type = pregistrypriv->vcs_type; + /* pxmitpriv->rts_thresh = pregistrypriv->rts_thresh; */ + pxmitpriv->frag_len = pregistrypriv->frag_thresh; + + adapter->qospriv.qos_option = pregistrypriv->wmm_enable; +#endif + + pdev_network->Privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0) ; /* adhoc no 802.1x */ + + pdev_network->Rssi = 0; + + pdev_network->Configuration.DSConfig = (pregistrypriv->channel); + + if (cur_network->network.InfrastructureMode == Ndis802_11IBSS) { + pdev_network->Configuration.ATIMWindow = (0); + + if (pmlmeext->cur_channel != 0) + pdev_network->Configuration.DSConfig = pmlmeext->cur_channel; + else + pdev_network->Configuration.DSConfig = 1; + } + + pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode); + + /* 1. Supported rates */ + /* 2. IE */ + + /* rtw_set_supported_rate(pdev_network->SupportedRates, pregistrypriv->wireless_mode) ; */ /* will be called in rtw_generate_ie */ + sz = rtw_generate_ie(pregistrypriv); + + pdev_network->IELength = sz; + + pdev_network->Length = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX *)pdev_network); + + /* notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd(); */ + /* pdev_network->IELength = cpu_to_le32(sz); */ + + +} + +void rtw_get_encrypt_decrypt_from_registrypriv(_adapter *adapter) +{ + + + +} + +/* the fucntion is at passive_level */ +void rtw_joinbss_reset(_adapter *padapter) +{ + u8 threshold; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + /* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */ + +#ifdef CONFIG_80211N_HT + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + + pmlmepriv->num_FortyMHzIntolerant = 0; + + pmlmepriv->num_sta_no_ht = 0; + + phtpriv->ampdu_enable = _FALSE;/* reset to disabled */ + +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) + /* TH=1 => means that invalidate usb rx aggregation */ + /* TH=0 => means that validate usb rx aggregation, use init value. */ + if (phtpriv->ht_option) { + if (padapter->registrypriv.wifi_spec == 1) + threshold = 1; + else + threshold = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } else { + threshold = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } +#endif/* #if defined( CONFIG_USB_HCI) || defined (CONFIG_SDIO_HCI) */ + +#endif/* #ifdef CONFIG_80211N_HT */ + +} + + +#ifdef CONFIG_80211N_HT +void rtw_ht_use_default_setting(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + BOOLEAN bHwLDPCSupport = _FALSE, bHwSTBCSupport = _FALSE; +#ifdef CONFIG_BEAMFORMING + BOOLEAN bHwSupportBeamformer = _FALSE, bHwSupportBeamformee = _FALSE; +#endif /* CONFIG_BEAMFORMING */ + + if (pregistrypriv->wifi_spec) + phtpriv->bss_coexist = 1; + else + phtpriv->bss_coexist = 0; + + phtpriv->sgi_40m = TEST_FLAG(pregistrypriv->short_gi, BIT1) ? _TRUE : _FALSE; + phtpriv->sgi_20m = TEST_FLAG(pregistrypriv->short_gi, BIT0) ? _TRUE : _FALSE; + + /* LDPC support */ + rtw_hal_get_def_var(padapter, HAL_DEF_RX_LDPC, (u8 *)&bHwLDPCSupport); + CLEAR_FLAGS(phtpriv->ldpc_cap); + if (bHwLDPCSupport) { + if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT4)) + SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX); + } + rtw_hal_get_def_var(padapter, HAL_DEF_TX_LDPC, (u8 *)&bHwLDPCSupport); + if (bHwLDPCSupport) { + if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT5)) + SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX); + } + if (phtpriv->ldpc_cap) + RTW_INFO("[HT] HAL Support LDPC = 0x%02X\n", phtpriv->ldpc_cap); + + /* STBC */ + rtw_hal_get_def_var(padapter, HAL_DEF_TX_STBC, (u8 *)&bHwSTBCSupport); + CLEAR_FLAGS(phtpriv->stbc_cap); + if (bHwSTBCSupport) { + if (TEST_FLAG(pregistrypriv->stbc_cap, BIT5)) + SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX); + } + rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)&bHwSTBCSupport); + if (bHwSTBCSupport) { + if (TEST_FLAG(pregistrypriv->stbc_cap, BIT4)) + SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX); + } + if (phtpriv->stbc_cap) + RTW_INFO("[HT] HAL Support STBC = 0x%02X\n", phtpriv->stbc_cap); + + /* Beamforming setting */ + CLEAR_FLAGS(phtpriv->beamform_cap); +#ifdef CONFIG_BEAMFORMING +#ifdef RTW_BEAMFORMING_VERSION_2 +#ifdef CONFIG_CONCURRENT_MODE + /* only enable beamforming in STA client mode */ + if (MLME_IS_STA(padapter) && !MLME_IS_GC(padapter)) +#else + if ((MLME_IS_AP(padapter) && !MLME_IS_GO(padapter)) || + (MLME_IS_STA(padapter) && !MLME_IS_GC(padapter))) +#endif +#endif + { + rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, (u8 *)&bHwSupportBeamformer); + rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, (u8 *)&bHwSupportBeamformee); + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT4) && bHwSupportBeamformer) { + SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + RTW_INFO("[HT] HAL Support Beamformer\n"); + } + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT5) && bHwSupportBeamformee) { + SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + RTW_INFO("[HT] HAL Support Beamformee\n"); + } + } +#endif /* CONFIG_BEAMFORMING */ +} +void rtw_build_wmm_ie_ht(_adapter *padapter, u8 *out_ie, uint *pout_len) +{ + unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00}; + int out_len; + u8 *pframe; + + if (padapter->mlmepriv.qospriv.qos_option == 0) { + out_len = *pout_len; + pframe = rtw_set_ie(out_ie + out_len, _VENDOR_SPECIFIC_IE_, + _WMM_IE_Length_, WMM_IE, pout_len); + + padapter->mlmepriv.qospriv.qos_option = 1; + } +} +#if defined(CONFIG_80211N_HT) +/* the fucntion is >= passive_level */ +unsigned int rtw_restructure_ht_ie(_adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel) +{ + u32 ielen, out_len; + u32 rx_packet_offset, max_recvbuf_sz; + HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor; + HT_CAP_AMPDU_DENSITY best_ampdu_density; + unsigned char *p, *pframe; + struct rtw_ieee80211_ht_cap ht_capie; + u8 cbw40_enable = 0, rf_num = 0, rx_stbc_nss = 0, rx_nss = 0; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#ifdef CONFIG_80211AC_VHT + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; +#endif /* CONFIG_80211AC_VHT */ + + phtpriv->ht_option = _FALSE; + + out_len = *pout_len; + + _rtw_memset(&ht_capie, 0, sizeof(struct rtw_ieee80211_ht_cap)); + + ht_capie.cap_info = IEEE80211_HT_CAP_DSSSCCK40; + + if (phtpriv->sgi_20m) + ht_capie.cap_info |= IEEE80211_HT_CAP_SGI_20; + + /* check if 40MHz is allowed according to hal cap and registry */ + if (hal_chk_bw_cap(padapter, BW_CAP_40M)) { + if (channel > 14) { + if (REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40)) + cbw40_enable = 1; + } else { + if (REGSTY_IS_BW_2G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40)) + cbw40_enable = 1; + } + } + + if (cbw40_enable) { + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + u8 oper_bw = CHANNEL_WIDTH_20, oper_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + if (in_ie == NULL) { + /* TDLS: TODO 20/40 issue */ + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + oper_bw = padapter->mlmeextpriv.cur_bwmode; + if (oper_bw > CHANNEL_WIDTH_40) + oper_bw = CHANNEL_WIDTH_40; + } else + /* TDLS: TODO 40? */ + oper_bw = CHANNEL_WIDTH_40; + } else { + p = rtw_get_ie(in_ie, WLAN_EID_HT_OPERATION, &ielen, in_len); + if (p && ielen == HT_OP_IE_LEN) { + if (GET_HT_OP_ELE_STA_CHL_WIDTH(p + 2)) { + switch (GET_HT_OP_ELE_2ND_CHL_OFFSET(p + 2)) { + case SCA: + oper_bw = CHANNEL_WIDTH_40; + oper_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + case SCB: + oper_bw = CHANNEL_WIDTH_40; + oper_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + } + } + } + // IOT issue : AP TP-Link WDR6500 + if(oper_bw == CHANNEL_WIDTH_40){ + p = rtw_get_ie(in_ie, WLAN_EID_HT_CAP, &ielen, in_len); + if (p && ielen == HT_CAP_IE_LEN) { + oper_bw = GET_HT_CAP_ELE_CHL_WIDTH(p + 2) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20; + if(oper_bw == CHANNEL_WIDTH_20) + oper_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + } + } + + /* adjust bw to fit in channel plan setting */ + if (oper_bw == CHANNEL_WIDTH_40 + && oper_offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE /* check this because TDLS has no info to set offset */ + && (!rtw_chset_is_chbw_valid(chset, channel, oper_bw, oper_offset, 1, 1) + || (IS_DFS_SLAVE_WITH_RD(rfctl) + && !rtw_rfctl_dfs_domain_unknown(rfctl) + && rtw_chset_is_chbw_non_ocp(chset, channel, oper_bw, oper_offset)) + ) + ) { + oper_bw = CHANNEL_WIDTH_20; + oper_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + rtw_warn_on(!rtw_chset_is_chbw_valid(chset, channel, oper_bw, oper_offset, 1, 1)); + if (IS_DFS_SLAVE_WITH_RD(rfctl) && !rtw_rfctl_dfs_domain_unknown(rfctl)) + rtw_warn_on(rtw_chset_is_chbw_non_ocp(chset, channel, oper_bw, oper_offset)); + } + + if (oper_bw == CHANNEL_WIDTH_40) { + ht_capie.cap_info |= IEEE80211_HT_CAP_SUP_WIDTH; + if (phtpriv->sgi_40m) + ht_capie.cap_info |= IEEE80211_HT_CAP_SGI_40; + } + + cbw40_enable = oper_bw == CHANNEL_WIDTH_40 ? 1 : 0; + } + + /* todo: disable SM power save mode */ + ht_capie.cap_info |= IEEE80211_HT_CAP_SM_PS; + + /* RX LDPC */ + if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX)) { + ht_capie.cap_info |= IEEE80211_HT_CAP_LDPC_CODING; + RTW_INFO("[HT] Declare supporting RX LDPC\n"); + } + + /* TX STBC */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX)) { + ht_capie.cap_info |= IEEE80211_HT_CAP_TX_STBC; + RTW_INFO("[HT] Declare supporting TX STBC\n"); + } + + /* RX STBC */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) { + if ((pregistrypriv->rx_stbc == 0x3) || /* enable for 2.4/5 GHz */ + ((channel <= 14) && (pregistrypriv->rx_stbc == 0x1)) || /* enable for 2.4GHz */ + ((channel > 14) && (pregistrypriv->rx_stbc == 0x2)) || /* enable for 5GHz */ + (pregistrypriv->wifi_spec == 1)) { + /* HAL_DEF_RX_STBC means STBC RX spatial stream, todo: VHT 4 streams */ + rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)(&rx_stbc_nss)); + SET_HT_CAP_ELE_RX_STBC(&ht_capie, rx_stbc_nss); + RTW_INFO("[HT] Declare supporting RX STBC = %d\n", rx_stbc_nss); + } + } + + /* fill default supported_mcs_set */ + _rtw_memcpy(ht_capie.supp_mcs_set, pmlmeext->default_supported_mcs_set, 16); + + /* update default supported_mcs_set */ + rx_nss = GET_HAL_RX_NSS(padapter); + + switch (rx_nss) { + case 1: + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_1R); + break; + case 2: + #ifdef CONFIG_DISABLE_MCS13TO15 + if (cbw40_enable && pregistrypriv->wifi_spec != 1) + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R_13TO15_OFF); + else + #endif + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R); + break; + case 3: + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_3R); + break; + case 4: + set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_4R); + break; + default: + RTW_WARN("rf_type:%d or rx_nss:%u is not expected\n", GET_HAL_RFPATH(padapter), rx_nss); + } + + { + rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset); + rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz); + if (max_recvbuf_sz - rx_packet_offset >= (8191 - 256)) { + RTW_INFO("%s IEEE80211_HT_CAP_MAX_AMSDU is set\n", __FUNCTION__); + ht_capie.cap_info = ht_capie.cap_info | IEEE80211_HT_CAP_MAX_AMSDU; + } + } + /* + AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k + AMPDU_para [4:2]:Min MPDU Start Spacing + */ + + /* + #if defined(CONFIG_RTL8188E) && defined(CONFIG_SDIO_HCI) + ht_capie.ampdu_params_info = 2; + #else + ht_capie.ampdu_params_info = (IEEE80211_HT_CAP_AMPDU_FACTOR&0x03); + #endif + */ + + if (padapter->driver_rx_ampdu_factor != 0xFF) + max_rx_ampdu_factor = (HT_CAP_AMPDU_FACTOR)padapter->driver_rx_ampdu_factor; + else + rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); + + /* rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); */ + ht_capie.ampdu_params_info = (max_rx_ampdu_factor & 0x03); + + if (padapter->driver_rx_ampdu_spacing != 0xFF) + ht_capie.ampdu_params_info |= ((padapter->driver_rx_ampdu_spacing & 0x07) << 2); + else { + if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_) { + /* + * Todo : Each chip must to ask DD , this chip best ampdu_density setting + * By yiwei.sun + */ + rtw_hal_get_def_var(padapter, HW_VAR_BEST_AMPDU_DENSITY, &best_ampdu_density); + + ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & (best_ampdu_density << 2)); + + } else + ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & 0x00); + } +#ifdef CONFIG_BEAMFORMING + ht_capie.tx_BF_cap_info = 0; + + /* HT Beamformer*/ + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) { + /* Transmit NDP Capable */ + SET_HT_CAP_TXBF_TRANSMIT_NDP_CAP(&ht_capie, 1); + /* Explicit Compressed Steering Capable */ + SET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(&ht_capie, 1); + /* Compressed Steering Number Antennas */ + SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(&ht_capie, 1); + rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMER_CAP, (u8 *)&rf_num); + SET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(&ht_capie, rf_num); + } + + /* HT Beamformee */ + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) { + /* Receive NDP Capable */ + SET_HT_CAP_TXBF_RECEIVE_NDP_CAP(&ht_capie, 1); + /* Explicit Compressed Beamforming Feedback Capable */ + SET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(&ht_capie, 2); + + rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMEE_CAP, (u8 *)&rf_num); +#ifdef CONFIG_80211AC_VHT + /* IOT action suggested by Yu Chen 2017/3/3 */ + if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_BROADCOM) && + !pvhtpriv->ap_bf_cap.is_mu_bfer && + pvhtpriv->ap_bf_cap.su_sound_dim == 2) + rf_num = (rf_num >= 2 ? 2 : rf_num); +#endif + SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(&ht_capie, rf_num); + } +#endif/*CONFIG_BEAMFORMING*/ + + pframe = rtw_set_ie(out_ie + out_len, _HT_CAPABILITY_IE_, + sizeof(struct rtw_ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len); + + phtpriv->ht_option = _TRUE; + + if (in_ie != NULL) { + p = rtw_get_ie(in_ie, _HT_ADD_INFO_IE_, &ielen, in_len); + if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) { + out_len = *pout_len; + pframe = rtw_set_ie(out_ie + out_len, _HT_ADD_INFO_IE_, ielen, p + 2 , pout_len); + } + } + + return phtpriv->ht_option; + +} + +/* the fucntion is > passive_level (in critical_section) */ +void rtw_update_ht_cap(_adapter *padapter, u8 *pie, uint ie_len, u8 channel) +{ + u8 *p, max_ampdu_sz; + int len; + /* struct sta_info *bmc_sta, *psta; */ + struct rtw_ieee80211_ht_cap *pht_capie; + struct ieee80211_ht_addt_info *pht_addtinfo; + /* struct recv_reorder_ctrl *preorder_ctrl; */ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + /* struct recv_priv *precvpriv = &padapter->recvpriv; */ + struct registry_priv *pregistrypriv = &padapter->registrypriv; + /* struct wlan_network *pcur_network = &(pmlmepriv->cur_network);; */ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 cbw40_enable = 0; + + + if (!phtpriv->ht_option) + return; + + if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable)) + return; + + RTW_INFO("+rtw_update_ht_cap()\n"); + + /* maybe needs check if ap supports rx ampdu. */ + if ((phtpriv->ampdu_enable == _FALSE) && (pregistrypriv->ampdu_enable == 1)) { + if (pregistrypriv->wifi_spec == 1) { + /* remove this part because testbed AP should disable RX AMPDU */ + /* phtpriv->ampdu_enable = _FALSE; */ + phtpriv->ampdu_enable = _TRUE; + } else + phtpriv->ampdu_enable = _TRUE; + } + + + /* check Max Rx A-MPDU Size */ + len = 0; + p = rtw_get_ie(pie + sizeof(NDIS_802_11_FIXED_IEs), _HT_CAPABILITY_IE_, &len, ie_len - sizeof(NDIS_802_11_FIXED_IEs)); + if (p && len > 0) { + pht_capie = (struct rtw_ieee80211_ht_cap *)(p + 2); + max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR); + max_ampdu_sz = 1 << (max_ampdu_sz + 3); /* max_ampdu_sz (kbytes); */ + + /* RTW_INFO("rtw_update_ht_cap(): max_ampdu_sz=%d\n", max_ampdu_sz); */ + phtpriv->rx_ampdu_maxlen = max_ampdu_sz; + + } + + + len = 0; + p = rtw_get_ie(pie + sizeof(NDIS_802_11_FIXED_IEs), _HT_ADD_INFO_IE_, &len, ie_len - sizeof(NDIS_802_11_FIXED_IEs)); + if (p && len > 0) { + pht_addtinfo = (struct ieee80211_ht_addt_info *)(p + 2); + /* todo: */ + } + + if (hal_chk_bw_cap(padapter, BW_CAP_40M)) { + if (channel > 14) { + if (REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40)) + cbw40_enable = 1; + } else { + if (REGSTY_IS_BW_2G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40)) + cbw40_enable = 1; + } + } + + /* update cur_bwmode & cur_ch_offset */ + if ((cbw40_enable) && + (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1)) && + (pmlmeinfo->HT_info.infos[0] & BIT(2))) { + int i; + u8 rx_nss = 0; + + rx_nss = GET_HAL_RX_NSS(padapter); + + /* update the MCS set */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i]; + + /* update the MCS rates */ + switch (rx_nss) { + case 1: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R); + break; + case 2: + #ifdef CONFIG_DISABLE_MCS13TO15 + if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1) + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF); + else + #endif + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R); + break; + case 3: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_3R); + break; + case 4: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_4R); + break; + default: + RTW_WARN("rx_nss:%u is not expected\n", rx_nss); + } + + /* switch to the 40M Hz mode accoring to the AP */ + /* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */ + switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) { + case EXTCHNL_OFFSET_UPPER: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + + case EXTCHNL_OFFSET_LOWER: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + + default: + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + RTW_INFO("%s : ch offset is not assigned for HT40 mod , update cur_bwmode=%u, cur_ch_offset=%u\n", + __func__, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset); + break; + } + } + + /* */ + /* Config SM Power Save setting */ + /* */ + pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) { +#if 0 + u8 i; + /* update the MCS rates */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i]; +#endif + RTW_INFO("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __FUNCTION__); + } + + /* */ + /* Config current HT Protection mode. */ + /* */ + pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; +} +#endif + +#ifdef CONFIG_TDLS +void rtw_issue_addbareq_cmd_tdls(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct sta_info *ptdls_sta = NULL; + u8 issued; + int priority; + struct ht_priv *phtpriv; + + priority = pattrib->priority; + + if (pattrib->direct_link == _TRUE) { + ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst); + if ((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) { + phtpriv = &ptdls_sta->htpriv; + + if ((phtpriv->ht_option == _TRUE) && (phtpriv->ampdu_enable == _TRUE)) { + issued = (phtpriv->agg_enable_bitmap >> priority) & 0x1; + issued |= (phtpriv->candidate_tid_bitmap >> priority) & 0x1; + + if (0 == issued) { + RTW_INFO("[%s], p=%d\n", __FUNCTION__, priority); + ptdls_sta->htpriv.candidate_tid_bitmap |= BIT((u8)priority); + rtw_addbareq_cmd(padapter, (u8)priority, pattrib->dst); + } + } + } + } +} +#endif /* CONFIG_TDLS */ + +#ifdef CONFIG_80211N_HT +static u8 rtw_issue_addbareq_check(_adapter *padapter, struct xmit_frame *pxmitframe, u8 issue_when_busy) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct registry_priv *pregistry = &padapter->registrypriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + s32 bmcst = IS_MCAST(pattrib->ra); + + if (bmcst) + return _FALSE; + + if (pregistry->tx_quick_addba_req == 0) { + if ((issue_when_busy == _TRUE) && (pmlmepriv->LinkDetectInfo.bBusyTraffic == _FALSE)) + return _FALSE; + + if (pmlmepriv->LinkDetectInfo.NumTxOkInPeriod < 100) + return _FALSE; + } + + return _TRUE; +} + +void rtw_issue_addbareq_cmd(_adapter *padapter, struct xmit_frame *pxmitframe, u8 issue_when_busy) +{ + u8 issued; + int priority; + struct sta_info *psta = NULL; + struct ht_priv *phtpriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + if (rtw_issue_addbareq_check(padapter,pxmitframe, issue_when_busy) == _FALSE) + return; + + priority = pattrib->priority; + +#ifdef CONFIG_TDLS + rtw_issue_addbareq_cmd_tdls(padapter, pxmitframe); +#endif /* CONFIG_TDLS */ + + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return; + } + + if (psta == NULL) { + RTW_INFO("%s, psta==NUL\n", __func__); + return; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return; + } + + + phtpriv = &psta->htpriv; + + if ((phtpriv->ht_option == _TRUE) && (phtpriv->ampdu_enable == _TRUE)) { + issued = (phtpriv->agg_enable_bitmap >> priority) & 0x1; + issued |= (phtpriv->candidate_tid_bitmap >> priority) & 0x1; + + if (0 == issued) { + RTW_INFO("rtw_issue_addbareq_cmd, p=%d\n", priority); + psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority); + rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra); + } + } + +} +#endif /* CONFIG_80211N_HT */ +void rtw_append_exented_cap(_adapter *padapter, u8 *out_ie, uint *pout_len) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; +#ifdef CONFIG_80211AC_VHT + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; +#endif /* CONFIG_80211AC_VHT */ + u8 cap_content[8] = { 0 }; + u8 *pframe; + u8 null_content[8] = {0}; + + if (phtpriv->bss_coexist) + SET_EXT_CAPABILITY_ELE_BSS_COEXIST(cap_content, 1); + +#ifdef CONFIG_80211AC_VHT + if (pvhtpriv->vht_option) + SET_EXT_CAPABILITY_ELE_OP_MODE_NOTIF(cap_content, 1); +#endif /* CONFIG_80211AC_VHT */ +#ifdef CONFIG_RTW_WNM + rtw_wnm_set_ext_cap_btm(cap_content, 1); +#endif + +#ifdef CONFIG_RTW_MBO + rtw_mbo_set_ext_cap_internw(cap_content, 1); +#endif + /* + From 802.11 specification,if a STA does not support any of capabilities defined + in the Extended Capabilities element, then the STA is not required to + transmit the Extended Capabilities element. + */ + if (_FALSE == _rtw_memcmp(cap_content, null_content, 8)) + pframe = rtw_set_ie(out_ie + *pout_len, EID_EXTCapability, 8, cap_content , pout_len); +} +#endif + +#ifdef CONFIG_LAYER2_ROAMING +inline void rtw_set_to_roam(_adapter *adapter, u8 to_roam) +{ + if (to_roam == 0) + adapter->mlmepriv.to_join = _FALSE; + adapter->mlmepriv.to_roam = to_roam; +} + +inline u8 rtw_dec_to_roam(_adapter *adapter) +{ + adapter->mlmepriv.to_roam--; + return adapter->mlmepriv.to_roam; +} + +inline u8 rtw_to_roam(_adapter *adapter) +{ + return adapter->mlmepriv.to_roam; +} + +void rtw_roaming(_adapter *padapter, struct wlan_network *tgt_network) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + _rtw_roaming(padapter, tgt_network); + _exit_critical_bh(&pmlmepriv->lock, &irqL); +} +void _rtw_roaming(_adapter *padapter, struct wlan_network *tgt_network) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *cur_network = &pmlmepriv->cur_network; + int do_join_r; + + if (0 < rtw_to_roam(padapter)) { + RTW_INFO("roaming from %s("MAC_FMT"), length:%d\n", + cur_network->network.Ssid.Ssid, MAC_ARG(cur_network->network.MacAddress), + cur_network->network.Ssid.SsidLength); + _rtw_memcpy(&pmlmepriv->assoc_ssid, &cur_network->network.Ssid, sizeof(NDIS_802_11_SSID)); + pmlmepriv->assoc_ch = 0; + pmlmepriv->assoc_by_bssid = _FALSE; + +#ifdef CONFIG_WAPI_SUPPORT + rtw_wapi_return_all_sta_info(padapter); +#endif + + while (1) { + do_join_r = rtw_do_join(padapter); + if (_SUCCESS == do_join_r) + break; + else { + RTW_INFO("roaming do_join return %d\n", do_join_r); + rtw_dec_to_roam(padapter); + + if (rtw_to_roam(padapter) > 0) + continue; + else { + RTW_INFO("%s(%d) -to roaming fail, indicate_disconnect\n", __FUNCTION__, __LINE__); +#ifdef CONFIG_RTW_80211R + rtw_ft_clr_flags(padapter, RTW_FT_PEER_EN|RTW_FT_PEER_OTD_EN); + rtw_ft_reset_status(padapter); +#endif + rtw_indicate_disconnect(padapter, 0, _FALSE); + break; + } + } + } + } + +} +#endif /* CONFIG_LAYER2_ROAMING */ + +bool rtw_adjust_chbw(_adapter *adapter, u8 req_ch, u8 *req_bw, u8 *req_offset) +{ + struct registry_priv *regsty = adapter_to_regsty(adapter); + u8 allowed_bw; + + if (req_ch < 14) + allowed_bw = REGSTY_BW_2G(regsty); + else if (req_ch == 14) + allowed_bw = CHANNEL_WIDTH_20; + else + allowed_bw = REGSTY_BW_5G(regsty); + + allowed_bw = hal_largest_bw(adapter, allowed_bw); + + if (allowed_bw == CHANNEL_WIDTH_80 && *req_bw > CHANNEL_WIDTH_80) + *req_bw = CHANNEL_WIDTH_80; + else if (allowed_bw == CHANNEL_WIDTH_40 && *req_bw > CHANNEL_WIDTH_40) + *req_bw = CHANNEL_WIDTH_40; + else if (allowed_bw == CHANNEL_WIDTH_20 && *req_bw > CHANNEL_WIDTH_20) { + *req_bw = CHANNEL_WIDTH_20; + *req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } else + return _FALSE; + + return _TRUE; +} + +sint rtw_linked_check(_adapter *padapter) +{ + if (MLME_IS_AP(padapter) || MLME_IS_MESH(padapter) + || MLME_IS_ADHOC(padapter) || MLME_IS_ADHOC_MASTER(padapter) + ) { + if (padapter->stapriv.asoc_sta_count > 2) + return _TRUE; + } else { + /* Station mode */ + if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _TRUE) + return _TRUE; + } + return _FALSE; +} +/*#define DBG_ADAPTER_STATE_CHK*/ +u8 rtw_is_adapter_up(_adapter *padapter) +{ + if (padapter == NULL) + return _FALSE; + + if (RTW_CANNOT_RUN(padapter)) { + #ifdef DBG_ADAPTER_STATE_CHK + RTW_INFO(FUNC_ADPT_FMT " FALSE -bDriverStopped(%s) bSurpriseRemoved(%s)\n" + , FUNC_ADPT_ARG(padapter) + , rtw_is_drv_stopped(padapter) ? "True" : "False" + , rtw_is_surprise_removed(padapter) ? "True" : "False"); + #endif + return _FALSE; + } + + if (!rtw_is_hw_init_completed(padapter)) { + #ifdef DBG_ADAPTER_STATE_CHK + RTW_INFO(FUNC_ADPT_FMT " FALSE -(hw_init_completed == _FALSE)\n", FUNC_ADPT_ARG(padapter)); + #endif + return _FALSE; + } + + if (padapter->bup == _FALSE) { + #ifdef DBG_ADAPTER_STATE_CHK + RTW_INFO(FUNC_ADPT_FMT " FALSE -(bup == _FALSE)\n", FUNC_ADPT_ARG(padapter)); + #endif + return _FALSE; + } + + return _TRUE; +} + +bool is_miracast_enabled(_adapter *adapter) +{ + bool enabled = 0; +#ifdef CONFIG_WFD + struct wifi_display_info *wfdinfo = &adapter->wfd_info; + + enabled = (wfdinfo->stack_wfd_mode & (MIRACAST_SOURCE | MIRACAST_SINK)) + || (wfdinfo->op_wfd_mode & (MIRACAST_SOURCE | MIRACAST_SINK)); +#endif + + return enabled; +} + +bool rtw_chk_miracast_mode(_adapter *adapter, u8 mode) +{ + bool ret = 0; +#ifdef CONFIG_WFD + struct wifi_display_info *wfdinfo = &adapter->wfd_info; + + ret = (wfdinfo->stack_wfd_mode & mode) || (wfdinfo->op_wfd_mode & mode); +#endif + + return ret; +} + +const char *get_miracast_mode_str(int mode) +{ + if (mode == MIRACAST_SOURCE) + return "SOURCE"; + else if (mode == MIRACAST_SINK) + return "SINK"; + else if (mode == (MIRACAST_SOURCE | MIRACAST_SINK)) + return "SOURCE&SINK"; + else if (mode == MIRACAST_DISABLED) + return "DISABLED"; + else + return "INVALID"; +} + +#ifdef CONFIG_WFD +static bool wfd_st_match_rule(_adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port) +{ + struct wifi_display_info *wfdinfo = &adapter->wfd_info; + + if (ntohs(*((u16 *)local_port)) == wfdinfo->rtsp_ctrlport + || ntohs(*((u16 *)local_port)) == wfdinfo->tdls_rtsp_ctrlport + || ntohs(*((u16 *)remote_port)) == wfdinfo->peer_rtsp_ctrlport) + return _TRUE; + return _FALSE; +} + +static struct st_register wfd_st_reg = { + .s_proto = 0x06, + .rule = wfd_st_match_rule, +}; +#endif /* CONFIG_WFD */ + +inline void rtw_wfd_st_switch(struct sta_info *sta, bool on) +{ +#ifdef CONFIG_WFD + if (on) + rtw_st_ctl_register(&sta->st_ctl, SESSION_TRACKER_REG_ID_WFD, &wfd_st_reg); + else + rtw_st_ctl_unregister(&sta->st_ctl, SESSION_TRACKER_REG_ID_WFD); +#endif +} + +void dump_arp_pkt(void *sel, u8 *da, u8 *sa, u8 *arp, bool tx) +{ + RTW_PRINT_SEL(sel, "%s ARP da="MAC_FMT", sa="MAC_FMT"\n" + , tx ? "send" : "recv", MAC_ARG(da), MAC_ARG(sa)); + RTW_PRINT_SEL(sel, "htype=%u, ptype=0x%04x, hlen=%u, plen=%u, oper=%u\n" + , GET_ARP_HTYPE(arp), GET_ARP_PTYPE(arp), GET_ARP_HLEN(arp) + , GET_ARP_PLEN(arp), GET_ARP_OPER(arp)); + RTW_PRINT_SEL(sel, "sha="MAC_FMT", spa="IP_FMT"\n" + , MAC_ARG(ARP_SENDER_MAC_ADDR(arp)), IP_ARG(ARP_SENDER_IP_ADDR(arp))); + RTW_PRINT_SEL(sel, "tha="MAC_FMT", tpa="IP_FMT"\n" + , MAC_ARG(ARP_TARGET_MAC_ADDR(arp)), IP_ARG(ARP_TARGET_IP_ADDR(arp))); +} + diff --git a/core/rtw_mlme_ext.c b/core/rtw_mlme_ext.c new file mode 100644 index 0000000..389316d --- /dev/null +++ b/core/rtw_mlme_ext.c @@ -0,0 +1,16843 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_MLME_EXT_C_ + +#include +#include + +struct mlme_handler mlme_sta_tbl[] = { + {WIFI_ASSOCREQ, "OnAssocReq", &OnAssocReq}, + {WIFI_ASSOCRSP, "OnAssocRsp", &OnAssocRsp}, + {WIFI_REASSOCREQ, "OnReAssocReq", &OnAssocReq}, + {WIFI_REASSOCRSP, "OnReAssocRsp", &OnAssocRsp}, + {WIFI_PROBEREQ, "OnProbeReq", &OnProbeReq}, + {WIFI_PROBERSP, "OnProbeRsp", &OnProbeRsp}, + + /*---------------------------------------------------------- + below 2 are reserved + -----------------------------------------------------------*/ + {0, "DoReserved", &DoReserved}, + {0, "DoReserved", &DoReserved}, + {WIFI_BEACON, "OnBeacon", &OnBeacon}, + {WIFI_ATIM, "OnATIM", &OnAtim}, + {WIFI_DISASSOC, "OnDisassoc", &OnDisassoc}, + {WIFI_AUTH, "OnAuth", &OnAuthClient}, + {WIFI_DEAUTH, "OnDeAuth", &OnDeAuth}, + {WIFI_ACTION, "OnAction", &OnAction}, + {WIFI_ACTION_NOACK, "OnActionNoAck", &OnAction}, +}; + +#ifdef _CONFIG_NATIVEAP_MLME_ +struct mlme_handler mlme_ap_tbl[] = { + {WIFI_ASSOCREQ, "OnAssocReq", &OnAssocReq}, + {WIFI_ASSOCRSP, "OnAssocRsp", &OnAssocRsp}, + {WIFI_REASSOCREQ, "OnReAssocReq", &OnAssocReq}, + {WIFI_REASSOCRSP, "OnReAssocRsp", &OnAssocRsp}, + {WIFI_PROBEREQ, "OnProbeReq", &OnProbeReq}, + {WIFI_PROBERSP, "OnProbeRsp", &OnProbeRsp}, + + /*---------------------------------------------------------- + below 2 are reserved + -----------------------------------------------------------*/ + {0, "DoReserved", &DoReserved}, + {0, "DoReserved", &DoReserved}, + {WIFI_BEACON, "OnBeacon", &OnBeacon}, + {WIFI_ATIM, "OnATIM", &OnAtim}, + {WIFI_DISASSOC, "OnDisassoc", &OnDisassoc}, + {WIFI_AUTH, "OnAuth", &OnAuth}, + {WIFI_DEAUTH, "OnDeAuth", &OnDeAuth}, + {WIFI_ACTION, "OnAction", &OnAction}, + {WIFI_ACTION_NOACK, "OnActionNoAck", &OnAction}, +}; +#endif + +struct action_handler OnAction_tbl[] = { + {RTW_WLAN_CATEGORY_SPECTRUM_MGMT, "ACTION_SPECTRUM_MGMT", on_action_spct}, + {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos}, + {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls}, + {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back}, + {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public}, + {RTW_WLAN_CATEGORY_RADIO_MEAS, "ACTION_RADIO_MEAS", &on_action_rm}, + {RTW_WLAN_CATEGORY_FT, "ACTION_FT", &OnAction_ft}, + {RTW_WLAN_CATEGORY_HT, "ACTION_HT", &OnAction_ht}, +#ifdef CONFIG_IEEE80211W + {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query}, +#else + {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved}, +#endif /* CONFIG_IEEE80211W */ +#ifdef CONFIG_RTW_WNM + {RTW_WLAN_CATEGORY_WNM, "ACTION_WNM", &on_action_wnm}, +#endif + {RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved}, +#ifdef CONFIG_RTW_MESH + {RTW_WLAN_CATEGORY_MESH, "ACTION_MESH", &on_action_mesh}, + {RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &on_action_self_protected}, +#endif + {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm}, + {RTW_WLAN_CATEGORY_VHT, "ACTION_VHT", &OnAction_vht}, + {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p}, +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + {RTW_WLAN_CATEGORY_TBTX, "ACTION_TBTX_TOKEN", &OnAction_tbtx_token} +#endif +}; + + +u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0}; + +/************************************************** +OUI definitions for the vendor specific IE +***************************************************/ +unsigned char RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01}; +unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02}; +unsigned char WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04}; +unsigned char P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09}; +unsigned char WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A}; +unsigned char DPP_OUI[] = {0x50, 0x6F, 0x9A, 0x1A}; +unsigned char MULTI_AP_OUI[] = {0x50, 0x6F, 0x9A, 0x1B}; + +unsigned char WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01}; +unsigned char WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + +unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02}; +unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02}; + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +unsigned char REALTEK_TBTX_IE[] = {0x00, 0xe0, 0x4c, 0x01, 0x00, 0x00, 0x00, 0x00}; +#endif +extern unsigned char REALTEK_96B_IE[]; + +static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set + , struct p2p_channels *channel_list) +{ + struct registry_priv *regsty = adapter_to_regsty(padapter); + + struct p2p_oper_class_map op_class[] = { + { IEEE80211G, 81, 1, 13, 1, BW20 }, + { IEEE80211G, 82, 14, 14, 1, BW20 }, +#if 0 /* Do not enable HT40 on 2 GHz */ + { IEEE80211G, 83, 1, 9, 1, BW40PLUS }, + { IEEE80211G, 84, 5, 13, 1, BW40MINUS }, +#endif + { IEEE80211A, 115, 36, 48, 4, BW20 }, + { IEEE80211A, 116, 36, 44, 8, BW40PLUS }, + { IEEE80211A, 117, 40, 48, 8, BW40MINUS }, + { IEEE80211A, 124, 149, 161, 4, BW20 }, + { IEEE80211A, 125, 149, 169, 4, BW20 }, + { IEEE80211A, 126, 149, 157, 8, BW40PLUS }, + { IEEE80211A, 127, 153, 161, 8, BW40MINUS }, + { -1, 0, 0, 0, 0, BW20 } + }; + + int cla, op; + + cla = 0; + + for (op = 0; op_class[op].op_class; op++) { + u8 ch; + struct p2p_oper_class_map *o = &op_class[op]; + struct p2p_reg_class *reg = NULL; + + for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) { + if (rtw_chset_search_ch(channel_set, ch) == -1) + continue; +#if defined(CONFIG_80211N_HT) || defined(CONFIG_80211AC_VHT) + if ((padapter->registrypriv.ht_enable == 0) && (o->inc == 8)) + continue; + + if ((REGSTY_IS_BW_5G_SUPPORT(regsty, CHANNEL_WIDTH_40)) && + ((o->bw == BW40MINUS) || (o->bw == BW40PLUS))) + continue; +#endif + if (reg == NULL) { + reg = &channel_list->reg_class[cla]; + cla++; + reg->reg_class = o->op_class; + reg->channels = 0; + } + reg->channel[reg->channels] = ch; + reg->channels++; + } + } + channel_list->reg_classes = cla; + +} + +#if CONFIG_TXPWR_LIMIT +void rtw_txpwr_init_regd(struct rf_ctl_t *rfctl) +{ + u8 regd; + struct regd_exc_ent *exc; + struct txpwr_lmt_ent *ent; + _irqL irqL; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + rfctl->regd_name = NULL; + + if (rfctl->txpwr_regd_num == 0) { + RTW_PRINT("there is no any txpwr_regd\n"); + goto release_lock; + } + + /* search from exception mapping */ + exc = _rtw_regd_exc_search(rfctl + , rfctl->country_ent ? rfctl->country_ent->alpha2 : NULL + , rfctl->ChannelPlan); + if (exc) { + u8 has_country = (exc->country[0] == '\0' && exc->country[1] == '\0') ? 0 : 1; + + if (strcmp(exc->regd_name, regd_str(TXPWR_LMT_NONE)) == 0) + rfctl->regd_name = regd_str(TXPWR_LMT_NONE); + else if (strcmp(exc->regd_name, regd_str(TXPWR_LMT_WW)) == 0) + rfctl->regd_name = regd_str(TXPWR_LMT_WW); + else { + ent = _rtw_txpwr_lmt_get_by_name(rfctl, exc->regd_name); + if (ent) + rfctl->regd_name = ent->regd_name; + } + + RTW_PRINT("exception mapping country:%c%c domain:0x%02x to%s regd_name:%s\n" + , has_country ? exc->country[0] : '0' + , has_country ? exc->country[1] : '0' + , exc->domain + , rfctl->regd_name ? "" : " unknown" + , exc->regd_name + ); + if (rfctl->regd_name) + goto release_lock; + } + +#ifdef CONFIG_REGD_SRC_FROM_OS + if (rfctl->regd_src == REGD_SRC_OS) { + if (IS_ALPHA2_WORLDWIDE(rfctl->country_ent->alpha2)) + rfctl->regd_name = regd_str(TXPWR_LMT_WW); + else { + char alpha2[3] = { + rfctl->country_ent->alpha2[0], rfctl->country_ent->alpha2[1], 0}; + + ent = _rtw_txpwr_lmt_get_by_name(rfctl, alpha2); + if (ent) + rfctl->regd_name = ent->regd_name; + } + + if (rfctl->regd_name) { + RTW_PRINT("mapping country:%c%c to regd_name:%s\n" + , rfctl->country_ent->alpha2[0] + , rfctl->country_ent->alpha2[1] + , rfctl->regd_name + ); + goto release_lock; + } + + if (rfctl->ChannelPlan == RTW_CHPLAN_UNSPECIFIED) { + rfctl->regd_name = regd_str(TXPWR_LMT_WW); + RTW_PRINT("mapping unsupported country:%c%c to regd_name:%s\n" + , rfctl->country_ent->alpha2[0] + , rfctl->country_ent->alpha2[1] + , rfctl->regd_name + ); + goto release_lock; + } + } +#endif + + /* follow default channel plan mapping */ + regd = rtw_chplan_get_default_regd(rfctl->ChannelPlan); + if (regd == TXPWR_LMT_NONE) + rfctl->regd_name = regd_str(TXPWR_LMT_NONE); + else if (regd == TXPWR_LMT_WW) + rfctl->regd_name = regd_str(TXPWR_LMT_WW); + else { + ent = _rtw_txpwr_lmt_get_by_name(rfctl, regd_str(regd)); + if (ent) + rfctl->regd_name = ent->regd_name; + } + + RTW_PRINT("default mapping domain:0x%02x to%s regd_name:%s\n" + , rfctl->ChannelPlan + , rfctl->regd_name ? "" : " unknown" + , regd_str(regd) + ); + if (rfctl->regd_name) + goto release_lock; + + switch (regd) { + /* + * To support older chips without new predefined regd: + * - use FCC if IC or CHILE or MEXICO not found + * - use ETSI if KCC or ACMA not found + */ + case TXPWR_LMT_IC: + case TXPWR_LMT_KCC: + case TXPWR_LMT_NCC: + case TXPWR_LMT_ACMA: + case TXPWR_LMT_CHILE: + case TXPWR_LMT_MEXICO: + if (regd == TXPWR_LMT_IC || regd == TXPWR_LMT_NCC || regd == TXPWR_LMT_CHILE || regd == TXPWR_LMT_MEXICO) + regd = TXPWR_LMT_FCC; + else if (regd == TXPWR_LMT_KCC || regd == TXPWR_LMT_ACMA) + regd = TXPWR_LMT_ETSI; + ent = _rtw_txpwr_lmt_get_by_name(rfctl, regd_str(regd)); + if (ent) + rfctl->regd_name = ent->regd_name; + RTW_PRINT("alternate regd_name:%s %s\n" + , regd_str(regd) + , rfctl->regd_name ? "is used" : "not found" + ); + if (rfctl->regd_name) + break; + /* fall through */ + default: + rfctl->regd_name = regd_str(TXPWR_LMT_WW); + RTW_PRINT("assign %s for default case\n", regd_str(TXPWR_LMT_WW)); + break; + }; + +release_lock: + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); +} +#endif /* CONFIG_TXPWR_LIMIT */ + +int rtw_rfctl_init(_adapter *adapter) +{ + struct registry_priv *regsty = adapter_to_regsty(adapter); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + int ret; + + _rtw_mutex_init(&rfctl->offch_mutex); + +#if CONFIG_TXPWR_LIMIT + _rtw_mutex_init(&rfctl->txpwr_lmt_mutex); + _rtw_init_listhead(&rfctl->reg_exc_list); + _rtw_init_listhead(&rfctl->txpwr_lmt_list); +#endif + + rfctl->ch_sel_within_same_band = 1; + +#ifdef CONFIG_DFS_MASTER + rfctl->dfs_region_domain = regsty->dfs_region_domain; + rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED; + rtw_init_timer(&(rfctl->radar_detect_timer), adapter, rtw_dfs_rd_timer_hdl, rfctl); +#endif +#if CONFIG_DFS_SLAVE_WITH_RADAR_DETECT + rfctl->dfs_slave_with_rd = 1; +#endif + + if (regsty->antenna_gain != UNSPECIFIED_MBM) + rfctl->antenna_gain = regsty->antenna_gain; + + ret = op_class_pref_init(adapter); + if (ret != _SUCCESS) + op_class_pref_deinit(adapter); + + return ret; +} + +void rtw_rfctl_deinit(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + + _rtw_mutex_free(&rfctl->offch_mutex); + +#if CONFIG_TXPWR_LIMIT + rtw_regd_exc_list_free(rfctl); + rtw_txpwr_lmt_list_free(rfctl); + _rtw_mutex_free(&rfctl->txpwr_lmt_mutex); +#endif + +#ifdef CONFIG_REGD_SRC_FROM_OS + if (rfctl->regd_src == REGD_SRC_OS) + rtw_mfree((void *)rfctl->country_ent, sizeof(struct country_chplan)); +#endif + + op_class_pref_deinit(adapter); +} + +void rtw_rfctl_chplan_init(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + + rfctl->max_chan_nums = init_channel_set(adapter); + op_class_pref_apply_regulatory(adapter, REG_CHANGE); + init_channel_list(adapter, rfctl->channel_set, &rfctl->channel_list); +} + +void rtw_rfctl_update_op_mode(struct rf_ctl_t *rfctl, u8 ifbmp_mod, u8 if_op) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + _adapter *iface; + struct mlme_ext_priv *mlmeext; + u8 op_class = 0; + u8 op_ch = 0; + s16 op_txpwr_max; + u8 if_op_class[CONFIG_IFACE_NUMBER] = {0}; + u8 if_op_ch[CONFIG_IFACE_NUMBER] = {0}; + u8 ch, bw, offset; + u8 u_ch = 0, u_bw, u_offset; + bool notify = 0; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + mlmeext = &iface->mlmeextpriv; + + if (ifbmp_mod & BIT(i)) { + if (!if_op) + continue; + } else if (!MLME_IS_ASOC(iface) || MLME_IS_OPCH_SW(iface)) + continue; + + ch = mlmeext->cur_channel; + bw = mlmeext->cur_bwmode; + offset = mlmeext->cur_ch_offset; + if_op_class[i] = rtw_get_op_class_by_chbw(ch, bw, offset); + if_op_ch[i] = if_op_class[i] ? ch : 0; + + if (!u_ch) { + u_ch = ch; + u_bw = bw; + u_offset = offset; + } else { + rtw_warn_on(!rtw_is_chbw_grouped(u_ch, u_bw, u_offset, ch, bw, offset)); + rtw_sync_chbw(&ch, &bw, &offset, &u_ch, &u_bw, &u_offset); + } + } + + op_class = rtw_get_op_class_by_chbw(u_ch, u_bw, u_offset); + op_ch = op_class ? u_ch : 0; + op_txpwr_max = rtw_rfctl_get_oper_txpwr_max_mbm(rfctl, u_ch, u_bw, u_offset, ifbmp_mod, if_op, 1); + + if (op_class != rfctl->op_class + || op_ch != rfctl->op_ch + || op_txpwr_max != rfctl->op_txpwr_max + || _rtw_memcmp(if_op_class, rfctl->if_op_class, sizeof(u8) * CONFIG_IFACE_NUMBER) == _FALSE + || _rtw_memcmp(if_op_ch, rfctl->if_op_ch, sizeof(u8) * CONFIG_IFACE_NUMBER) == _FALSE) + notify = 1; + + rfctl->op_class = op_class; + rfctl->op_ch = op_ch; + rfctl->op_txpwr_max = op_txpwr_max; + _rtw_memcpy(rfctl->if_op_class, if_op_class, sizeof(u8) * CONFIG_IFACE_NUMBER); + _rtw_memcpy(rfctl->if_op_ch, if_op_ch, sizeof(u8) * CONFIG_IFACE_NUMBER); + + if (0) + RTW_INFO("radio: %u,%u,%u %d notify:%d\n", u_ch, u_bw, u_offset, op_txpwr_max, notify); + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + mlmeext = &iface->mlmeextpriv; + + if (ifbmp_mod & BIT(i)) { + if (!if_op) + continue; + } else if (!MLME_IS_ASOC(iface)) + continue; + if (0) + RTW_INFO(ADPT_FMT": %u,%u,%u\n", ADPT_ARG(iface) + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset); + } + + if (notify) + rtw_nlrtw_radio_opmode_notify(rfctl); + +#ifdef CONFIG_PLATFORM_CMAP_INTFS + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + cmap_intfs_nl_bss_status_event(iface, 0); + } +#endif +} + +inline u8 rtw_rfctl_get_dfs_domain(struct rf_ctl_t *rfctl) +{ +#ifdef CONFIG_DFS_MASTER + return rfctl->dfs_region_domain; +#else + return RTW_DFS_REGD_NONE; +#endif +} + +inline u8 rtw_rfctl_dfs_domain_unknown(struct rf_ctl_t *rfctl) +{ +#ifdef CONFIG_DFS_MASTER + return rtw_rfctl_get_dfs_domain(rfctl) == RTW_DFS_REGD_NONE; +#else + return 1; +#endif +} + +#ifdef CONFIG_DFS_MASTER +/* +* called in rtw_dfs_rd_enable() +* assume the request channel coverage is DFS range +* base on the current status and the request channel coverage to check if need to reset complete CAC time +*/ +bool rtw_is_cac_reset_needed(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset) +{ + bool needed = _FALSE; + u32 cur_hi, cur_lo, hi, lo; + + if (rfctl->radar_detected == 1) { + needed = _TRUE; + goto exit; + } + + if (rfctl->radar_detect_ch == 0) { + needed = _TRUE; + goto exit; + } + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) { + RTW_ERR("request detection range ch:%u, bw:%u, offset:%u\n", ch, bw, offset); + rtw_warn_on(1); + } + + if (rtw_chbw_to_freq_range(rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset, &cur_hi, &cur_lo) == _FALSE) { + RTW_ERR("cur detection range ch:%u, bw:%u, offset:%u\n", rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset); + rtw_warn_on(1); + } + + if (hi <= lo || cur_hi <= cur_lo) { + RTW_ERR("hi:%u, lo:%u, cur_hi:%u, cur_lo:%u\n", hi, lo, cur_hi, cur_lo); + rtw_warn_on(1); + } + + if (rtw_is_range_a_in_b(hi, lo, cur_hi, cur_lo)) { + /* request is in current detect range */ + goto exit; + } + + /* check if request channel coverage has new range and the new range is in DFS range */ + if (!rtw_is_range_overlap(hi, lo, cur_hi, cur_lo)) { + /* request has no overlap with current */ + needed = _TRUE; + } else if (rtw_is_range_a_in_b(cur_hi, cur_lo, hi, lo)) { + /* request is supper set of current */ + if ((hi != cur_hi && rtw_chset_is_dfs_range(rfctl->channel_set, hi, cur_hi)) + || (lo != cur_lo && rtw_chset_is_dfs_range(rfctl->channel_set, cur_lo, lo))) + needed = _TRUE; + } else { + /* request is not supper set of current, but has overlap */ + if ((lo < cur_lo && rtw_chset_is_dfs_range(rfctl->channel_set, cur_lo, lo)) + || (hi > cur_hi && rtw_chset_is_dfs_range(rfctl->channel_set, hi, cur_hi))) + needed = _TRUE; + } + +exit: + return needed; +} + +bool _rtw_rfctl_overlap_radar_detect_ch(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset) +{ + bool ret = _FALSE; + u32 hi = 0, lo = 0; + u32 r_hi = 0, r_lo = 0; + int i; + + if (rfctl->radar_detect_by_others) + goto exit; + + if (rfctl->radar_detect_ch == 0) + goto exit; + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) { + rtw_warn_on(1); + goto exit; + } + + if (rtw_chbw_to_freq_range(rfctl->radar_detect_ch + , rfctl->radar_detect_bw, rfctl->radar_detect_offset + , &r_hi, &r_lo) == _FALSE) { + rtw_warn_on(1); + goto exit; + } + + if (rtw_is_range_overlap(hi, lo, r_hi, r_lo)) + ret = _TRUE; + +exit: + return ret; +} + +bool rtw_rfctl_overlap_radar_detect_ch(struct rf_ctl_t *rfctl) +{ + return _rtw_rfctl_overlap_radar_detect_ch(rfctl + , rfctl_to_dvobj(rfctl)->oper_channel + , rfctl_to_dvobj(rfctl)->oper_bwmode + , rfctl_to_dvobj(rfctl)->oper_ch_offset); +} + +bool rtw_rfctl_is_tx_blocked_by_ch_waiting(struct rf_ctl_t *rfctl) +{ + return rtw_rfctl_overlap_radar_detect_ch(rfctl) && IS_CH_WAITING(rfctl); +} + +bool rtw_chset_is_chbw_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset) +{ + bool ret = _FALSE; + u32 hi = 0, lo = 0; + int i; + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) + goto exit; + + for (i = 0; i < MAX_CHANNEL_NUM && ch_set[i].ChannelNum != 0; i++) { + if (!rtw_ch2freq(ch_set[i].ChannelNum)) { + rtw_warn_on(1); + continue; + } + + if (!CH_IS_NON_OCP(&ch_set[i])) + continue; + + if (lo <= rtw_ch2freq(ch_set[i].ChannelNum) + && rtw_ch2freq(ch_set[i].ChannelNum) <= hi + ) { + ret = _TRUE; + break; + } + } + +exit: + return ret; +} + +bool rtw_chset_is_ch_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch) +{ + return rtw_chset_is_chbw_non_ocp(ch_set, ch, CHANNEL_WIDTH_20, HAL_PRIME_CHNL_OFFSET_DONT_CARE); +} + +u32 rtw_chset_get_ch_non_ocp_ms(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset) +{ + int ms = 0; + systime current_time; + u32 hi = 0, lo = 0; + int i; + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) + goto exit; + + current_time = rtw_get_current_time(); + + for (i = 0; i < MAX_CHANNEL_NUM && ch_set[i].ChannelNum != 0; i++) { + if (!rtw_ch2freq(ch_set[i].ChannelNum)) { + rtw_warn_on(1); + continue; + } + + if (!CH_IS_NON_OCP(&ch_set[i])) + continue; + + if (lo <= rtw_ch2freq(ch_set[i].ChannelNum) + && rtw_ch2freq(ch_set[i].ChannelNum) <= hi + ) { + if (rtw_systime_to_ms(ch_set[i].non_ocp_end_time - current_time) > ms) + ms = rtw_systime_to_ms(ch_set[i].non_ocp_end_time - current_time); + } + } + +exit: + return ms; +} + +/** + * rtw_chset_update_non_ocp - update non_ocp_end_time according to the given @ch, @bw, @offset into @ch_set + * @ch_set: the given channel set + * @ch: channel number on which radar is detected + * @bw: bandwidth on which radar is detected + * @offset: bandwidth offset on which radar is detected + * @ms: ms to add from now to update non_ocp_end_time, ms < 0 means use NON_OCP_TIME_MS + */ +static bool _rtw_chset_update_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset, int ms) +{ + u32 hi = 0, lo = 0; + int i; + bool updated = 0; + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) + goto exit; + + for (i = 0; i < MAX_CHANNEL_NUM && ch_set[i].ChannelNum != 0; i++) { + if (!rtw_ch2freq(ch_set[i].ChannelNum)) { + rtw_warn_on(1); + continue; + } + + if (lo <= rtw_ch2freq(ch_set[i].ChannelNum) + && rtw_ch2freq(ch_set[i].ChannelNum) <= hi + ) { + if (ms >= 0) + ch_set[i].non_ocp_end_time = rtw_get_current_time() + rtw_ms_to_systime(ms); + else + ch_set[i].non_ocp_end_time = rtw_get_current_time() + rtw_ms_to_systime(NON_OCP_TIME_MS); + ch_set[i].flags |= RTW_CHF_NON_OCP; + updated = 1; + } + } + +exit: + return updated; +} + +inline bool rtw_chset_update_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset) +{ + return _rtw_chset_update_non_ocp(ch_set, ch, bw, offset, -1); +} + +inline bool rtw_chset_update_non_ocp_ms(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset, int ms) +{ + return _rtw_chset_update_non_ocp(ch_set, ch, bw, offset, ms); +} + +static bool rtw_chset_chk_non_ocp_finish_for_chbw(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset) +{ + RT_CHANNEL_INFO *ch_set = rfctl->channel_set; + u8 cch; + u8 *op_chs; + u8 op_ch_num; + int i; + int ch_idx; + bool ret = 0; + + cch = rtw_get_center_ch(ch, bw, offset); + + if (!rtw_get_op_chs_by_cch_bw(cch, bw, &op_chs, &op_ch_num)) + goto exit; + + for (i = 0; i < op_ch_num; i++) { + if (0) + RTW_INFO("%u,%u,%u - cch:%u, bw:%u, op_ch:%u\n", ch, bw, offset, cch, bw, *(op_chs + i)); + ch_idx = rtw_chset_search_ch(ch_set, *(op_chs + i)); + if (ch_idx == -1) + break; + if (!(ch_set[ch_idx].flags & RTW_CHF_NON_OCP) || CH_IS_NON_OCP(&ch_set[ch_idx])) + break; + } + + if (op_ch_num != 0 && i == op_ch_num) { + ret = 1; + /* clear RTTW_CHF_NON_OCP flag */ + for (i = 0; i < op_ch_num; i++) { + ch_idx = rtw_chset_search_ch(ch_set, *(op_chs + i)); + ch_set[ch_idx].flags &= ~RTW_CHF_NON_OCP; + } + rtw_nlrtw_nop_finish_event(dvobj_get_primary_adapter(rfctl_to_dvobj(rfctl)), cch, bw); + } + +exit: + return ret; +} + +/* called by watchdog to clear RTW_CHF_NON_OCP and generate NON_OCP finish event */ +void rtw_chset_chk_non_ocp_finish(struct rf_ctl_t *rfctl) +{ + u8 ch, bw, offset; + int i; + + bw = CHANNEL_WIDTH_160; + while (1) { + for (i = 0; i < rfctl->max_chan_nums; i++) { + ch = rfctl->channel_set[i].ChannelNum; + if (!(rfctl->channel_set[i].flags & RTW_CHF_NON_OCP)) + continue; + if (!rtw_get_offset_by_chbw(ch, bw, &offset)) + continue; + + rtw_chset_chk_non_ocp_finish_for_chbw(rfctl, ch, bw, offset); + } + if (bw-- == CHANNEL_WIDTH_20) + break; + } +} + +u32 rtw_get_ch_waiting_ms(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset, u32 *r_non_ocp_ms, u32 *r_cac_ms) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + u32 non_ocp_ms; + u32 cac_ms; + u8 in_rd_range = 0; /* if in current radar detection range*/ + + if (rtw_chset_is_chbw_non_ocp(rfctl->channel_set, ch, bw, offset)) + non_ocp_ms = rtw_chset_get_ch_non_ocp_ms(rfctl->channel_set, ch, bw, offset); + else + non_ocp_ms = 0; + + if (rfctl->radar_detect_enabled) { + u32 cur_hi, cur_lo, hi, lo; + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) { + RTW_ERR("input range ch:%u, bw:%u, offset:%u\n", ch, bw, offset); + rtw_warn_on(1); + } + + if (rtw_chbw_to_freq_range(rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset, &cur_hi, &cur_lo) == _FALSE) { + RTW_ERR("cur detection range ch:%u, bw:%u, offset:%u\n", rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset); + rtw_warn_on(1); + } + + if (rtw_is_range_a_in_b(hi, lo, cur_hi, cur_lo)) + in_rd_range = 1; + } + + if (!rtw_chset_is_dfs_chbw(rfctl->channel_set, ch, bw, offset)) + cac_ms = 0; + else if (in_rd_range && !non_ocp_ms) { + if (IS_CH_WAITING(rfctl)) + cac_ms = rtw_systime_to_ms(rfctl->cac_end_time - rtw_get_current_time()); + else + cac_ms = 0; + } else if (rtw_is_long_cac_ch(ch, bw, offset, rtw_rfctl_get_dfs_domain(rfctl))) + cac_ms = CAC_TIME_CE_MS; + else + cac_ms = CAC_TIME_MS; + + if (r_non_ocp_ms) + *r_non_ocp_ms = non_ocp_ms; + if (r_cac_ms) + *r_cac_ms = cac_ms; + + return non_ocp_ms + cac_ms; +} + +void rtw_reset_cac(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset) +{ + u32 non_ocp_ms; + u32 cac_ms; + + rtw_get_ch_waiting_ms(rfctl + , ch + , bw + , offset + , &non_ocp_ms + , &cac_ms + ); + + rfctl->cac_start_time = rtw_get_current_time() + rtw_ms_to_systime(non_ocp_ms); + rfctl->cac_end_time = rfctl->cac_start_time + rtw_ms_to_systime(cac_ms); + + /* skip special value */ + if (rfctl->cac_start_time == RTW_CAC_STOPPED) { + rfctl->cac_start_time++; + rfctl->cac_end_time++; + } + if (rfctl->cac_end_time == RTW_CAC_STOPPED) + rfctl->cac_end_time++; +} + +u32 rtw_force_stop_cac(struct rf_ctl_t *rfctl, u32 timeout_ms) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + systime start; + u32 pass_ms; + + start = rtw_get_current_time(); + + rfctl->cac_force_stop = 1; + + while (rtw_get_passing_time_ms(start) <= timeout_ms + && IS_UNDER_CAC(rfctl) + ) { + if (dev_is_surprise_removed(dvobj) || dev_is_drv_stopped(dvobj)) + break; + rtw_msleep_os(20); + } + + if (IS_UNDER_CAC(rfctl)) { + if (!dev_is_surprise_removed(dvobj) && !dev_is_drv_stopped(dvobj)) + RTW_INFO("%s waiting for cac stop timeout!\n", __func__); + } + + rfctl->cac_force_stop = 0; + + pass_ms = rtw_get_passing_time_ms(start); + + return pass_ms; +} +#endif /* CONFIG_DFS_MASTER */ + +/* choose channel with shortest waiting (non ocp + cac) time */ +bool rtw_choose_shortest_waiting_ch(struct rf_ctl_t *rfctl, u8 sel_ch, u8 max_bw + , u8 *dec_ch, u8 *dec_bw, u8 *dec_offset + , u8 e_flags, u8 d_flags, u8 cur_ch, bool by_int_info, u8 mesh_only) +{ +#ifndef DBG_CHOOSE_SHORTEST_WAITING_CH +#define DBG_CHOOSE_SHORTEST_WAITING_CH 0 +#endif + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); +#ifdef CONFIG_RTW_ACS + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(dvobj_get_primary_adapter(dvobj)); +#endif + struct registry_priv *regsty = dvobj_to_regsty(dvobj); + u8 ch, bw, offset; + u8 ch_c = 0, bw_c = 0, offset_c = 0; + int i; + u32 min_waiting_ms = 0; + u16 int_factor_c = 0; + + if (!dec_ch || !dec_bw || !dec_offset) { + rtw_warn_on(1); + return _FALSE; + } + + RTW_INFO("%s: sel_ch:%u max_bw:%u e_flags:0x%02x d_flags:0x%02x cur_ch:%u within_sb:%d%s%s\n" + , __func__, sel_ch, max_bw, e_flags, d_flags, cur_ch, rfctl->ch_sel_within_same_band + , by_int_info ? " int" : "", mesh_only ? " mesh_only" : ""); + + /* full search and narrow bw judegement first to avoid potetial judegement timing issue */ + for (bw = CHANNEL_WIDTH_20; bw <= max_bw; bw++) { + if (!hal_is_bw_support(dvobj_get_primary_adapter(dvobj), bw)) + continue; + + for (i = 0; i < rfctl->max_chan_nums; i++) { + u32 non_ocp_ms = 0; + u32 cac_ms = 0; + u32 waiting_ms = 0; + u16 int_factor = 0; + bool dfs_ch; + bool non_ocp; + bool long_cac; + + ch = rfctl->channel_set[i].ChannelNum; + if (sel_ch) { + if (ch != sel_ch) + continue; + } else if (rfctl->ch_sel_within_same_band && !rtw_is_same_band(cur_ch, ch)) + continue; + + if (ch > 14) { + if (bw > REGSTY_BW_5G(regsty)) + continue; + } else { + if (bw > REGSTY_BW_2G(regsty)) + continue; + } + + if (mesh_only && ch >= 5 && ch <= 9 && bw > CHANNEL_WIDTH_20) + continue; + + if (!rtw_get_offset_by_chbw(ch, bw, &offset)) + continue; + + if (!rtw_chset_is_chbw_valid(rfctl->channel_set, ch, bw, offset, 0, 0)) + continue; + + if ((e_flags & RTW_CHF_DFS) || (d_flags & RTW_CHF_DFS)) { + dfs_ch = rtw_chset_is_dfs_chbw(rfctl->channel_set, ch, bw, offset); + if (((e_flags & RTW_CHF_DFS) && !dfs_ch) + || ((d_flags & RTW_CHF_DFS) && dfs_ch)) + continue; + } + + if ((e_flags & RTW_CHF_LONG_CAC) || (d_flags & RTW_CHF_LONG_CAC)) { + long_cac = rtw_is_long_cac_ch(ch, bw, offset, rtw_rfctl_get_dfs_domain(rfctl)); + if (((e_flags & RTW_CHF_LONG_CAC) && !long_cac) + || ((d_flags & RTW_CHF_LONG_CAC) && long_cac)) + continue; + } + + if ((e_flags & RTW_CHF_NON_OCP) || (d_flags & RTW_CHF_NON_OCP)) { + non_ocp = rtw_chset_is_chbw_non_ocp(rfctl->channel_set, ch, bw, offset); + if (((e_flags & RTW_CHF_NON_OCP) && !non_ocp) + || ((d_flags & RTW_CHF_NON_OCP) && non_ocp)) + continue; + } + + #ifdef CONFIG_DFS_MASTER + waiting_ms = rtw_get_ch_waiting_ms(rfctl, ch, bw, offset, &non_ocp_ms, &cac_ms); + #endif + + #ifdef CONFIG_RTW_ACS + if (by_int_info) { + /* for now, consider only primary channel */ + int_factor = hal_data->acs.interference_time[i]; + } + #endif + + if (DBG_CHOOSE_SHORTEST_WAITING_CH) + RTW_INFO("%s:%u,%u,%u %u(non_ocp:%u, cac:%u), int:%u\n" + , __func__, ch, bw, offset, waiting_ms, non_ocp_ms, cac_ms, int_factor); + + if (ch_c == 0 + /* first: smaller wating time */ + || min_waiting_ms > waiting_ms + /* then: less interference */ + || (min_waiting_ms == waiting_ms && int_factor_c > int_factor) + /* then: wider bw */ + || (min_waiting_ms == waiting_ms && int_factor_c == int_factor && bw > bw_c) + /* if all condition equal, same channel -> same band prefer */ + || (min_waiting_ms == waiting_ms && int_factor_c == int_factor && bw == bw_c + && ((cur_ch != ch_c && cur_ch == ch) + || (!rtw_is_same_band(cur_ch, ch_c) && rtw_is_same_band(cur_ch, ch))) + ) + ) { + ch_c = ch; + bw_c = bw; + offset_c = offset; + min_waiting_ms = waiting_ms; + int_factor_c = int_factor; + } + } + } + + if (ch_c != 0) { + RTW_INFO("%s: select %u,%u,%u waiting_ms:%u\n" + , __func__, ch_c, bw_c, offset_c, min_waiting_ms); + *dec_ch = ch_c; + *dec_bw = bw_c; + *dec_offset = offset_c; + return _TRUE; + } else { + RTW_INFO("%s: not found\n", __func__); + if (d_flags == 0) + rtw_warn_on(1); + } + + return _FALSE; +} + +#ifdef CONFIG_PROC_DEBUG +#define RTW_CHF_FMT "%s%s%s%s%s%s" + +#define RTW_CHF_ARG_NO_IR(flags) (flags & RTW_CHF_NO_IR) ? " NO_IR" : "" +#define RTW_CHF_ARG_DFS(flags) , (flags & RTW_CHF_DFS) ? " DFS" : "" +#define RTW_CHF_ARG_NO_HT40U(flags) , (flags & RTW_CHF_NO_HT40U) ? " NO_40M+" : "" +#define RTW_CHF_ARG_NO_HT40L(flags) , (flags & RTW_CHF_NO_HT40L) ? " NO_40M-" : "" +#define RTW_CHF_ARG_NO_80MHZ(flags) , (flags & RTW_CHF_NO_80MHZ) ? " NO_80M" : "" +#define RTW_CHF_ARG_NO_160MHZ(flags) , (flags & RTW_CHF_NO_160MHZ) ? " NO_160M" : "" + +#define RTW_CHF_ARG(flags) \ + RTW_CHF_ARG_NO_IR(flags) \ + RTW_CHF_ARG_DFS(flags) \ + RTW_CHF_ARG_NO_HT40U(flags) \ + RTW_CHF_ARG_NO_HT40L(flags) \ + RTW_CHF_ARG_NO_80MHZ(flags) \ + RTW_CHF_ARG_NO_160MHZ(flags) + +void dump_chset(void *sel, RT_CHANNEL_INFO *ch_set, u8 chset_num) +{ + char buf[8]; + u8 i; + + RTW_PRINT_SEL(sel, "%-3s %-4s %-4s flags\n", "ch", "freq", "nocp"); + + for (i = 0; i < MAX_CHANNEL_NUM && i < chset_num && ch_set[i].ChannelNum != 0; i++) { + #ifdef CONFIG_DFS_MASTER + if ((ch_set[i].flags & RTW_CHF_DFS) && CH_IS_NON_OCP(&ch_set[i])) + snprintf(buf, 8, "%d", rtw_systime_to_ms(ch_set[i].non_ocp_end_time - rtw_get_current_time()) / 1000); + else + #endif + snprintf(buf, 8, "0"); + + RTW_PRINT_SEL(sel, "%3u %4u %4s"RTW_CHF_FMT"\n" + , ch_set[i].ChannelNum, rtw_ch2freq(ch_set[i].ChannelNum), buf + , RTW_CHF_ARG(ch_set[i].flags) + ); + } + + RTW_PRINT_SEL(sel, "total ch number:%d\n", i); +} + +void dump_cur_chset(void *sel, struct rf_ctl_t *rfctl) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + struct registry_priv *regsty = dvobj_to_regsty(dvobj); + struct get_chplan_resp *chplan; + int i; + + if (rtw_get_chplan_cmd(dvobj_get_primary_adapter(dvobj), RTW_CMDF_WAIT_ACK, &chplan) == _FAIL) + return; + + RTW_PRINT_SEL(sel, "regd_src:%s(%d)\n", regd_src_str(chplan->regd_src), chplan->regd_src); + + if (chplan->has_country) + dump_country_chplan(sel, &chplan->country_ent); + else + RTW_PRINT_SEL(sel, "chplan:0x%02X\n", chplan->channel_plan); + +#if CONFIG_TXPWR_LIMIT + RTW_PRINT_SEL(sel, "PLS regd:%s\n", chplan->regd_name); +#endif + +#ifdef CONFIG_DFS_MASTER + RTW_PRINT_SEL(sel, "dfs_domain:%s(%u)\n", rtw_dfs_regd_str(chplan->dfs_domain), chplan->dfs_domain); +#endif + + for (i = 0; i < MAX_CHANNEL_NUM; i++) + if (regsty->excl_chs[i] != 0) + break; + + if (i < MAX_CHANNEL_NUM) { + RTW_PRINT_SEL(sel, "excl_chs:"); + for (i = 0; i < MAX_CHANNEL_NUM; i++) { + if (regsty->excl_chs[i] == 0) + break; + _RTW_PRINT_SEL(sel, "%u ", regsty->excl_chs[i]); + } + _RTW_PRINT_SEL(sel, "\n"); + } + + dump_chset(sel, chplan->chset, chplan->chset_num); + + rtw_vmfree(chplan, sizeof(struct get_chplan_resp) + sizeof(RT_CHANNEL_INFO) * chplan->chset_num); +} +#endif + +/* + * Search the @param ch in given @param ch_set + * @ch_set: the given channel set + * @ch: the given channel number + * + * return the index of channel_num in channel_set, -1 if not found + */ +int rtw_chset_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch) +{ + int i; + + if (ch == 0) + return -1; + + for (i = 0; i < MAX_CHANNEL_NUM && ch_set[i].ChannelNum != 0; i++) { + if (ch == ch_set[i].ChannelNum) + return i; + } + + return -1; +} + +/* + * Check if the @param ch, bw, offset is valid for the given @param ch_set + * @ch_set: the given channel set + * @ch: the given channel number + * @bw: the given bandwidth + * @offset: the given channel offset + * + * return valid (1) or not (0) + */ +u8 rtw_chset_is_chbw_valid(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset + , bool allow_primary_passive, bool allow_passive) +{ + u8 cch; + u8 *op_chs; + u8 op_ch_num; + u8 valid = 0; + int i; + int ch_idx; + + cch = rtw_get_center_ch(ch, bw, offset); + + if (!rtw_get_op_chs_by_cch_bw(cch, bw, &op_chs, &op_ch_num)) + goto exit; + + for (i = 0; i < op_ch_num; i++) { + if (0) + RTW_INFO("%u,%u,%u - cch:%u, bw:%u, op_ch:%u\n", ch, bw, offset, cch, bw, *(op_chs + i)); + ch_idx = rtw_chset_search_ch(ch_set, *(op_chs + i)); + if (ch_idx == -1) + break; + if (ch_set[ch_idx].flags & RTW_CHF_NO_IR) { + if ((!allow_primary_passive && ch_set[ch_idx].ChannelNum == ch) + || (!allow_passive && ch_set[ch_idx].ChannelNum != ch)) + break; + } + if (bw >= CHANNEL_WIDTH_40) { + if ((ch_set[ch_idx].flags & RTW_CHF_NO_HT40U) && i % 2 == 0) + break; + if ((ch_set[ch_idx].flags & RTW_CHF_NO_HT40L) && i % 2 == 1) + break; + } + if (bw >= CHANNEL_WIDTH_80 && (ch_set[ch_idx].flags & RTW_CHF_NO_80MHZ)) + break; + if (bw >= CHANNEL_WIDTH_160 && (ch_set[ch_idx].flags & RTW_CHF_NO_160MHZ)) + break; + } + + if (op_ch_num != 0 && i == op_ch_num) + valid = 1; + +exit: + return valid; +} + +/** + * rtw_chset_sync_chbw - obey g_ch, adjust g_bw, g_offset, bw, offset to fit in channel plan + * @ch_set: channel plan to check + * @req_ch: pointer of the request ch, may be modified further + * @req_bw: pointer of the request bw, may be modified further + * @req_offset: pointer of the request offset, may be modified further + * @g_ch: pointer of the ongoing group ch + * @g_bw: pointer of the ongoing group bw, may be modified further + * @g_offset: pointer of the ongoing group offset, may be modified further + * @allow_primary_passive: if allow passive primary ch when deciding chbw + * @allow_passive: if allow passive ch (not primary) when deciding chbw + */ +void rtw_chset_sync_chbw(RT_CHANNEL_INFO *ch_set, u8 *req_ch, u8 *req_bw, u8 *req_offset + , u8 *g_ch, u8 *g_bw, u8 *g_offset, bool allow_primary_passive, bool allow_passive) +{ + u8 r_ch, r_bw, r_offset; + u8 u_ch, u_bw, u_offset; + u8 cur_bw = *req_bw; + + while (1) { + r_ch = *req_ch; + r_bw = cur_bw; + r_offset = *req_offset; + u_ch = *g_ch; + u_bw = *g_bw; + u_offset = *g_offset; + + rtw_sync_chbw(&r_ch, &r_bw, &r_offset, &u_ch, &u_bw, &u_offset); + + if (rtw_chset_is_chbw_valid(ch_set, r_ch, r_bw, r_offset, allow_primary_passive, allow_passive)) + break; + if (cur_bw == CHANNEL_WIDTH_20) { + rtw_warn_on(1); + break; + } + cur_bw--; + }; + + *req_ch = r_ch; + *req_bw = r_bw; + *req_offset = r_offset; + *g_ch = u_ch; + *g_bw = u_bw; + *g_offset = u_offset; +} + +/* + * Check the @param ch is fit with setband setting of @param adapter + * @adapter: the given adapter + * @ch: the given channel number + * + * return _TRUE when check valid, _FALSE not valid + */ +bool rtw_mlme_band_check(_adapter *adapter, const u32 ch) +{ + if (adapter->setband == WIFI_FREQUENCY_BAND_AUTO /* 2.4G and 5G */ + || (adapter->setband == WIFI_FREQUENCY_BAND_2GHZ && ch < 35) /* 2.4G only */ + || (adapter->setband == WIFI_FREQUENCY_BAND_5GHZ && ch > 35) /* 5G only */ + ) + return _TRUE; + return _FALSE; +} +inline void RTW_SET_SCAN_BAND_SKIP(_adapter *padapter, int skip_band) +{ + int bs = ATOMIC_READ(&padapter->bandskip); + + bs |= skip_band; + ATOMIC_SET(&padapter->bandskip, bs); +} + +inline void RTW_CLR_SCAN_BAND_SKIP(_adapter *padapter, int skip_band) +{ + int bs = ATOMIC_READ(&padapter->bandskip); + + bs &= ~(skip_band); + ATOMIC_SET(&padapter->bandskip, bs); +} +inline int RTW_GET_SCAN_BAND_SKIP(_adapter *padapter) +{ + return ATOMIC_READ(&padapter->bandskip); +} + +#define RTW_IS_SCAN_BAND_SKIP(padapter, skip_band) (ATOMIC_READ(&padapter->bandskip) & (skip_band)) + +bool rtw_mlme_ignore_chan(_adapter *adapter, const u32 ch) +{ + if (RTW_IS_SCAN_BAND_SKIP(adapter, BAND_24G) && ch < 35) /* SKIP 2.4G Band channel */ + return _TRUE; + if (RTW_IS_SCAN_BAND_SKIP(adapter, BAND_5G) && ch > 35) /* SKIP 5G Band channel */ + return _TRUE; + + return _FALSE; +} + + +/**************************************************************************** + +Following are the initialization functions for WiFi MLME + +*****************************************************************************/ + +int init_hw_mlme_ext(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + u8 rx_bar_enble = _TRUE; + + /* + * Sync driver status and hardware setting + */ + + /* Modify to make sure first time change channel(band) would be done properly */ + pHalData->current_channel = 0; + pHalData->current_channel_bw = CHANNEL_WIDTH_MAX; + #if CONFIG_IEEE80211_BAND_5GHZ + pHalData->current_band_type = BAND_MAX; + #else + pHalData->current_band_type = BAND_ON_2_4G; + #endif + + /* set_opmode_cmd(padapter, infra_client_with_mlme); */ /* removed */ + rtw_hal_set_hwreg(padapter, HW_VAR_ENABLE_RX_BAR, &rx_bar_enble); + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + return _SUCCESS; +} + +void init_mlme_default_rate_set(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char end_set[1] = {0xff}; + u8 offset_datarate = 0; + u8 offset_basicrate = 0; +#ifdef CONFIG_80211N_HT + unsigned char supported_mcs_set[16] = {0xff, 0xff, 0xff, 0xff, 0x00, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; +#endif + + if (IsSupportedTxCCK(padapter->registrypriv.wireless_mode)) { + + unsigned char datarate_b[B_MODE_RATE_NUM] ={_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_}; + _rtw_memcpy(pmlmeext->datarate, datarate_b, B_MODE_RATE_NUM); + _rtw_memcpy(pmlmeext->basicrate, datarate_b, B_MODE_RATE_NUM); + offset_datarate += B_MODE_RATE_NUM; + offset_basicrate += B_MODE_RATE_NUM; + RTW_INFO("%s: support CCK\n", __func__); + } + if(IsSupportedTxOFDM(padapter->registrypriv.wireless_mode)) { + unsigned char datarate_g[G_MODE_RATE_NUM] ={_6M_RATE_, _9M_RATE_, _12M_RATE_, _18M_RATE_,_24M_RATE_, _36M_RATE_, _48M_RATE_, _54M_RATE_}; + unsigned char basicrate_g[G_MODE_BASIC_RATE_NUM] = {_6M_RATE_, _12M_RATE_, _24M_RATE_}; + _rtw_memcpy(pmlmeext->datarate + offset_datarate, datarate_g, G_MODE_RATE_NUM); + _rtw_memcpy(pmlmeext->basicrate + offset_basicrate,basicrate_g, G_MODE_BASIC_RATE_NUM); + offset_datarate += G_MODE_RATE_NUM; + offset_basicrate += G_MODE_BASIC_RATE_NUM; + RTW_INFO("%s: support OFDM\n", __func__); + + } + _rtw_memcpy(pmlmeext->datarate + offset_datarate, end_set, 1); + _rtw_memcpy(pmlmeext->basicrate + offset_basicrate, end_set, 1); + +#ifdef CONFIG_80211N_HT + if( padapter->registrypriv.ht_enable && is_supported_ht(padapter->registrypriv.wireless_mode)) + _rtw_memcpy(pmlmeext->default_supported_mcs_set, supported_mcs_set, sizeof(pmlmeext->default_supported_mcs_set)); +#endif +} + +static void init_mlme_ext_priv_value(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + ATOMIC_SET(&pmlmeext->event_seq, 0); + pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */ +#ifdef CONFIG_IEEE80211W + pmlmeext->sa_query_seq = 0; +#endif + pmlmeext->cur_channel = padapter->registrypriv.channel; + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + pmlmeext->retry = 0; + + pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode; + init_mlme_default_rate_set(padapter); + + if ((pmlmeext->cur_channel > 14) || ((padapter->registrypriv.wireless_mode & WIRELESS_11B) == 0)) + pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB; + else + pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB; + + mlmeext_set_scan_state(pmlmeext, SCAN_DISABLE); + pmlmeext->sitesurvey_res.channel_idx = 0; + pmlmeext->sitesurvey_res.bss_cnt = 0; + pmlmeext->sitesurvey_res.scan_ch_ms = SURVEY_TO; + pmlmeext->sitesurvey_res.rx_ampdu_accept = RX_AMPDU_ACCEPT_INVALID; + pmlmeext->sitesurvey_res.rx_ampdu_size = RX_AMPDU_SIZE_INVALID; +#ifdef CONFIG_SCAN_BACKOP +#ifdef CONFIG_LAYER2_ROAMING + if (padapter->mlmepriv.roam_flags & BIT2) + mlmeext_assign_scan_backop_flags_sta(pmlmeext, SS_BACKOP_EN | SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME); + else +#endif + mlmeext_assign_scan_backop_flags_sta(pmlmeext, /*SS_BACKOP_EN |*/ SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME); + #ifdef CONFIG_AP_MODE + #ifdef CONFIG_CUSTOMER_EZVIZ_CHIME2 + mlmeext_assign_scan_backop_flags_ap(pmlmeext, SS_BACKOP_EN | SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME | SS_BACKOP_EN_NL); + #else + mlmeext_assign_scan_backop_flags_ap(pmlmeext, SS_BACKOP_EN | SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME); + #endif + #endif + #ifdef CONFIG_RTW_MESH + mlmeext_assign_scan_backop_flags_mesh(pmlmeext, /*SS_BACKOP_EN | */SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME); + #endif + pmlmeext->sitesurvey_res.scan_cnt = 0; + pmlmeext->sitesurvey_res.scan_cnt_max = RTW_SCAN_NUM_OF_CH; + pmlmeext->sitesurvey_res.backop_ms = RTW_BACK_OP_CH_MS; +#endif +#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL) + pmlmeext->sitesurvey_res.is_sw_antdiv_bl_scan = 0; +#endif + pmlmeext->scan_abort = _FALSE; + + pmlmeinfo->state = WIFI_FW_NULL_STATE; + pmlmeinfo->reauth_count = 0; + pmlmeinfo->reassoc_count = 0; + pmlmeinfo->link_count = 0; + pmlmeinfo->auth_seq = 0; + pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open; + pmlmeinfo->key_index = 0; + pmlmeinfo->iv = 0; + + pmlmeinfo->enc_algo = _NO_PRIVACY_; + pmlmeinfo->authModeToggle = 0; + + _rtw_memset(pmlmeinfo->chg_txt, 0, 128); + + pmlmeinfo->slotTime = SHORT_SLOT_TIME; + pmlmeinfo->preamble_mode = PREAMBLE_AUTO; + + pmlmeinfo->dialogToken = 0; + + pmlmeext->action_public_rxseq = 0xffff; + pmlmeext->action_public_dialog_token = 0xff; +#ifdef ROKU_PRIVATE +/*infra mode, used to store AP's info*/ + _rtw_memset(pmlmeinfo->SupportedRates_infra_ap, 0, NDIS_802_11_LENGTH_RATES_EX); + pmlmeinfo->ht_vht_received = 0; +#endif /* ROKU_PRIVATE */ +#ifdef CONFIG_WRITE_BCN_LEN_TO_FW + pmlmeinfo->last_bcn_len = 0; +#endif +} + +void init_mlme_ext_timer(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + rtw_init_timer(&pmlmeext->survey_timer, padapter, survey_timer_hdl, padapter); + rtw_init_timer(&pmlmeext->link_timer, padapter, link_timer_hdl, padapter); +#ifdef CONFIG_RTW_80211R + rtw_init_timer(&pmlmeext->ft_link_timer, padapter, rtw_ft_link_timer_hdl, padapter); + rtw_init_timer(&pmlmeext->ft_roam_timer, padapter, rtw_ft_roam_timer_hdl, padapter); +#endif + +#ifdef CONFIG_RTW_REPEATER_SON + rtw_init_timer(&pmlmeext->rson_scan_timer, padapter, rson_timer_hdl, padapter); +#endif +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + rtw_init_timer(&pmlmeext->tbtx_xmit_timer, padapter, rtw_tbtx_xmit_timer_hdl, padapter); + rtw_init_timer(&pmlmeext->tbtx_token_dispatch_timer, padapter, rtw_tbtx_token_dispatch_timer_hdl, padapter); +#endif +#ifdef CONFIG_DFS + rtw_init_timer(&pmlmeext->csa_timer, padapter->pnetdev, csa_timer_hdl, padapter); +#endif +} + +int init_mlme_ext_priv(_adapter *padapter) +{ + int res = _SUCCESS; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */ + /* _rtw_memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv)); */ + + pmlmeext->padapter = padapter; + + /* fill_fwpriv(padapter, &(pmlmeext->fwpriv)); */ + + init_mlme_ext_priv_value(padapter); + pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq; + + init_mlme_ext_timer(padapter); + +#ifdef CONFIG_AP_MODE + init_mlme_ap_info(padapter); +#endif + + pmlmeext->last_scan_time = 0; + pmlmeext->mlmeext_init = _TRUE; + + +#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + pmlmeext->active_keep_alive_check = _TRUE; +#else + pmlmeext->active_keep_alive_check = _FALSE; +#endif + +#ifdef DBG_FIXED_CHAN + pmlmeext->fixed_chan = 0xFF; +#endif + + pmlmeext->tsf_update_pause_factor = pregistrypriv->tsf_update_pause_factor; + pmlmeext->tsf_update_restore_factor = pregistrypriv->tsf_update_restore_factor; + +#ifdef CONFIG_SUPPORT_STATIC_SMPS + pmlmeext->ssmps_en = _FALSE; + pmlmeext->ssmps_tx_tp_th = SSMPS_TX_TP_TH;/*Mbps*/ + pmlmeext->ssmps_rx_tp_th = SSMPS_RX_TP_TH;/*Mbps*/ + #ifdef DBG_STATIC_SMPS + pmlmeext->ssmps_test = _FALSE; + #endif +#endif + +#ifdef CONFIG_CTRL_TXSS_BY_TP + pmlmeext->txss_ctrl_en = _TRUE; + pmlmeext->txss_tp_th = TXSS_TP_TH; + pmlmeext->txss_tp_chk_cnt = TXSS_TP_CHK_CNT; +#endif + + return res; + +} + +void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext) +{ + _adapter *padapter = pmlmeext->padapter; + + if (!padapter) + return; + + if (rtw_is_drv_stopped(padapter)) { + _cancel_timer_ex(&pmlmeext->survey_timer); + _cancel_timer_ex(&pmlmeext->link_timer); +#ifdef CONFIG_DFS + _cancel_timer_ex(&pmlmeext->csa_timer); +#endif /* CONFIG_DFS */ + } +} + +#ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL +static u8 cmp_pkt_chnl_diff(_adapter *padapter, u8 *pframe, uint packet_len) +{ + /* if the channel is same, return 0. else return channel differential */ + uint len; + u8 channel; + u8 *p; + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_); + if (p) { + channel = *(p + 2); + if (padapter->mlmeextpriv.cur_channel >= channel) + return padapter->mlmeextpriv.cur_channel - channel; + else + return channel - padapter->mlmeextpriv.cur_channel; + } else + return 0; +} +#endif /* CONFIG_PATCH_JOIN_WRONG_CHANNEL */ + +static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame) +{ + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 *pframe = precv_frame->u.hdr.rx_data; + + if (ptable->func) { + /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */ + if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN) && + !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + { + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); + + if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) != _TRUE) + return; + + if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _TRUE) + return; + + if ( pwdev_priv->pno_mac_addr[0] == 0xFF) + return; + + if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_pno_mac_addr(padapter), ETH_ALEN)) + return; + } +#else + return; +#endif + + ptable->func(padapter, precv_frame); + } + +} + +void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame) +{ + int index; + struct mlme_handler *ptable; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, get_addr2_ptr(pframe)); + struct recv_priv *precvpriv = &padapter->recvpriv; + + +#if 0 + { + u8 *pbuf; + pbuf = GetAddr1Ptr(pframe); + RTW_INFO("A1-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf + 1), *(pbuf + 2), *(pbuf + 3), *(pbuf + 4), *(pbuf + 5)); + pbuf = get_addr2_ptr(pframe); + RTW_INFO("A2-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf + 1), *(pbuf + 2), *(pbuf + 3), *(pbuf + 4), *(pbuf + 5)); + pbuf = GetAddr3Ptr(pframe); + RTW_INFO("A3-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf + 1), *(pbuf + 2), *(pbuf + 3), *(pbuf + 4), *(pbuf + 5)); + } +#endif + + if (GetFrameType(pframe) != WIFI_MGT_TYPE) { + return; + } + + /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */ + if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN) && + !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + { + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); + + if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) != _TRUE) + return; + + if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _TRUE) + return; + + if ( pwdev_priv->pno_mac_addr[0] == 0xFF) + return; + + if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_pno_mac_addr(padapter), ETH_ALEN)) + return; + } +#else + return; +#endif + + ptable = mlme_sta_tbl; + + index = get_frame_sub_type(pframe) >> 4; + +#ifdef CONFIG_TDLS + if ((index << 4) == WIFI_ACTION) { + /* category==public (4), action==TDLS_DISCOVERY_RESPONSE */ + if (*(pframe + 24) == RTW_WLAN_CATEGORY_PUBLIC && *(pframe + 25) == TDLS_DISCOVERY_RESPONSE) { + RTW_INFO("[TDLS] Recv %s from "MAC_FMT"\n", rtw_tdls_action_txt(TDLS_DISCOVERY_RESPONSE), MAC_ARG(get_addr2_ptr(pframe))); + On_TDLS_Dis_Rsp(padapter, precv_frame); + } + } +#endif /* CONFIG_TDLS */ + + if (index >= (sizeof(mlme_sta_tbl) / sizeof(struct mlme_handler))) { + return; + } + ptable += index; + +#if 1 + if (psta != NULL) { + if (GetRetry(pframe)) { + if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum) { + /* drop the duplicate management frame */ + precvpriv->dbg_rx_dup_mgt_frame_drop_count++; + RTW_INFO("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num); + return; + } + } + psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num; + } +#else + + if (GetRetry(pframe)) { + /* return; */ + } +#endif + +#ifdef CONFIG_AP_MODE + switch (get_frame_sub_type(pframe)) { + case WIFI_AUTH: + if (MLME_IS_AP(padapter) || MLME_IS_MESH(padapter)) + ptable->func = &OnAuth; + else + ptable->func = &OnAuthClient; + /* fall through */ + case WIFI_ASSOCREQ: + case WIFI_REASSOCREQ: + _mgt_dispatcher(padapter, ptable, precv_frame); + #ifdef CONFIG_HOSTAPD_MLME + if (MLME_IS_AP(padapter)) + rtw_hostapd_mlme_rx(padapter, precv_frame); + #endif + break; + case WIFI_PROBEREQ: + _mgt_dispatcher(padapter, ptable, precv_frame); + #ifdef CONFIG_HOSTAPD_MLME + if (MLME_IS_AP(padapter)) + rtw_hostapd_mlme_rx(padapter, precv_frame); + #endif + break; + case WIFI_BEACON: + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + case WIFI_ACTION: + _mgt_dispatcher(padapter, ptable, precv_frame); + break; + default: + _mgt_dispatcher(padapter, ptable, precv_frame); + #ifdef CONFIG_HOSTAPD_MLME + if (MLME_IS_AP(padapter)) + rtw_hostapd_mlme_rx(padapter, precv_frame); + #endif + break; + } +#else + + _mgt_dispatcher(padapter, ptable, precv_frame); + +#endif + +} + +#ifdef CONFIG_P2P +u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da) +{ + bool response = _TRUE; + +#ifdef CONFIG_IOCTL_CFG80211 + if (padapter->wdinfo.driver_interface == DRIVER_CFG80211) { + if (rtw_cfg80211_get_is_roch(padapter) == _FALSE + || rtw_get_oper_ch(padapter) != padapter->wdinfo.listen_channel + || adapter_wdev_data(padapter)->p2p_enabled == _FALSE + || padapter->mlmepriv.wps_probe_resp_ie == NULL + || padapter->mlmepriv.p2p_probe_resp_ie == NULL + ) { +#ifdef CONFIG_DEBUG_CFG80211 + RTW_INFO(ADPT_FMT" DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n" + , ADPT_ARG(padapter) + , adapter_wdev_data(padapter)->p2p_enabled + , padapter->mlmepriv.wps_probe_resp_ie + , padapter->mlmepriv.p2p_probe_resp_ie); + RTW_INFO(ADPT_FMT" DON'T issue_probersp_p2p: is_ro_ch:%d, op_ch:%d, p2p_listen_channel:%d\n" + , ADPT_ARG(padapter) + , rtw_cfg80211_get_is_roch(padapter) + , rtw_get_oper_ch(padapter) + , padapter->wdinfo.listen_channel); +#endif + response = _FALSE; + } + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + if (padapter->wdinfo.driver_interface == DRIVER_WEXT) { + /* do nothing if the device name is empty */ + if (!padapter->wdinfo.device_name_len) + response = _FALSE; + } + + if (response == _TRUE) + issue_probersp_p2p(padapter, da); + + return _SUCCESS; +} +#endif /* CONFIG_P2P */ + + +/**************************************************************************** + +Following are the callback functions for each subtype of the management frames + +*****************************************************************************/ + +unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int ielen; + unsigned char *p; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur = &(pmlmeinfo->network); + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + u8 is_valid_p2p_probereq = _FALSE; + +#ifdef CONFIG_ATMEL_RC_PATCH + u8 *target_ie = NULL, *wps_ie = NULL; + u8 *start; + uint search_len = 0, wps_ielen = 0, target_ielen = 0; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; +#endif + +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + u8 wifi_test_chk_rate = 1; + +#ifdef CONFIG_IOCTL_CFG80211 + if ((pwdinfo->driver_interface == DRIVER_CFG80211) + && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) + && (GET_CFG80211_REPORT_MGMT(adapter_wdev_data(padapter), IEEE80211_STYPE_PROBE_REQ) == _TRUE) + ) { + rtw_cfg80211_rx_probe_request(padapter, precv_frame); + return _SUCCESS; + } +#endif /* CONFIG_IOCTL_CFG80211 */ + + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && + !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) && + !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) && + !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) && + !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) + ) { + /* Commented by Albert 2011/03/17 */ + /* mcs_rate = 0->CCK 1M rate */ + /* mcs_rate = 1->CCK 2M rate */ + /* mcs_rate = 2->CCK 5.5M rate */ + /* mcs_rate = 3->CCK 11M rate */ + /* In the P2P mode, the driver should not support the CCK rate */ + + /* Commented by Kurt 2012/10/16 */ + /* IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */ + if (padapter->registrypriv.wifi_spec == 1) { + if (pattrib->data_rate <= DESC_RATE11M) + wifi_test_chk_rate = 0; + } + + if (wifi_test_chk_rate == 1) { + is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len); + if (is_valid_p2p_probereq == _TRUE) { + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) { + /* FIXME */ + if (padapter->wdinfo.driver_interface == DRIVER_WEXT) + report_survey_event(padapter, precv_frame); + + p2p_listen_state_process(padapter, get_sa(pframe)); + + return _SUCCESS; + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + goto _continue; + } + } + } + +_continue: +#endif /* CONFIG_P2P */ + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) + return _SUCCESS; + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _FALSE && + check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE | WIFI_MESH_STATE) == _FALSE) + return _SUCCESS; + + + /* RTW_INFO("+OnProbeReq\n"); */ + + +#ifdef CONFIG_ATMEL_RC_PATCH + wps_ie = rtw_get_wps_ie( + pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_, + NULL, &wps_ielen); + if (wps_ie) + target_ie = rtw_get_wps_attr_content(wps_ie, wps_ielen, WPS_ATTR_MANUFACTURER, NULL, &target_ielen); + if ((target_ie && (target_ielen == 4)) && (_TRUE == _rtw_memcmp((void *)target_ie, "Ozmo", 4))) { + /* psta->flag_atmel_rc = 1; */ + unsigned char *sa_addr = get_sa(pframe); + printk("%s: Find Ozmo RC -- %02x:%02x:%02x:%02x:%02x:%02x \n\n", + __func__, *sa_addr, *(sa_addr + 1), *(sa_addr + 2), *(sa_addr + 3), *(sa_addr + 4), *(sa_addr + 5)); + _rtw_memcpy(pstapriv->atmel_rc_pattern, get_sa(pframe), ETH_ALEN); + } +#endif + + +#ifdef CONFIG_AUTO_AP_MODE + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE && + pmlmepriv->cur_network.join_res == _TRUE) { + _irqL irqL; + struct sta_info *psta; + u8 *mac_addr, *peer_addr; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A}; + /* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */ + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + if (!p || ielen != 14) + goto _non_rc_device; + + if (!_rtw_memcmp(p + 2, RC_OUI, sizeof(RC_OUI))) + goto _non_rc_device; + + if (!_rtw_memcmp(p + 6, get_sa(pframe), ETH_ALEN)) { + RTW_INFO("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __FUNCTION__, + MAC_ARG(get_sa(pframe)), MAC_ARG(p + 6)); + + goto _non_rc_device; + } + + RTW_INFO("%s, got the pairing device("MAC_FMT")\n", __FUNCTION__, MAC_ARG(get_sa(pframe))); + + /* new a station */ + psta = rtw_get_stainfo(pstapriv, get_sa(pframe)); + if (psta == NULL) { + /* allocate a new one */ + RTW_INFO("going to alloc stainfo for rc="MAC_FMT"\n", MAC_ARG(get_sa(pframe))); + psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe)); + if (psta == NULL) { + /* TODO: */ + RTW_INFO(" Exceed the upper limit of supported clients...\n"); + return _SUCCESS; + } + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&psta->asoc_list)) { + psta->expire_to = pstapriv->expire_to; + rtw_list_insert_tail(&psta->asoc_list, &pstapriv->asoc_list); + pstapriv->asoc_list_cnt++; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt++; + #endif + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + /* generate pairing ID */ + mac_addr = adapter_mac_addr(padapter); + peer_addr = psta->cmn.mac_addr; + psta->pid = (u16)(((mac_addr[4] << 8) + mac_addr[5]) + ((peer_addr[4] << 8) + peer_addr[5])); + + /* update peer stainfo */ + psta->isrc = _TRUE; + + /* AID assignment */ + if (psta->cmn.aid > 0) + RTW_INFO(FUNC_ADPT_FMT" old AID=%d\n", FUNC_ADPT_ARG(padapter), psta->cmn.aid); + else { + if (!rtw_aid_alloc(padapter, psta)) { + RTW_INFO(FUNC_ADPT_FMT" no room for more AIDs\n", FUNC_ADPT_ARG(padapter)); + return _SUCCESS; + } + RTW_INFO(FUNC_ADPT_FMT" allocate new AID=%d\n", FUNC_ADPT_ARG(padapter), psta->cmn.aid); + } + + psta->qos_option = 1; + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + psta->ieee8021x_blocked = _FALSE; +#ifdef CONFIG_80211N_HT + if(padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode)) { + psta->htpriv.ht_option = _TRUE; + psta->htpriv.ampdu_enable = _FALSE; + psta->htpriv.sgi_20m = _FALSE; + psta->htpriv.sgi_40m = _FALSE; + psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ + } +#endif + + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE); + + _rtw_memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + _enter_critical_bh(&psta->lock, &irqL); + psta->state |= WIFI_ASOC_STATE; + _exit_critical_bh(&psta->lock, &irqL); + + report_add_sta_event(padapter, psta->cmn.mac_addr); + + } + + issue_probersp(padapter, get_sa(pframe), _FALSE); + + return _SUCCESS; + + } + +_non_rc_device: + + return _SUCCESS; + +#endif /* CONFIG_AUTO_AP_MODE */ + + +#ifdef CONFIG_CONCURRENT_MODE + if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) && + rtw_mi_buddy_check_fwstate(padapter, WIFI_UNDER_LINKING | WIFI_UNDER_SURVEY)) { + /* don't process probe req */ + return _SUCCESS; + } +#endif + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + + /* check (wildcard) SSID */ + if (p != NULL) { + if (is_valid_p2p_probereq == _TRUE) + goto _issue_probersp; + + if ((ielen != 0 && _FALSE == _rtw_memcmp((void *)(p + 2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) + || (ielen == 0 && pmlmeinfo->hidden_ssid_mode)) + goto exit; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, WLAN_EID_MESH_ID, (int *)&ielen, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + if (!p) + goto exit; + if (ielen != 0 && _rtw_memcmp((void *)(p + 2), (void *)cur->mesh_id.Ssid, cur->mesh_id.SsidLength) == _FALSE) + goto exit; + } + #endif + +_issue_probersp: + if (((check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE && + pmlmepriv->cur_network.join_res == _TRUE)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) { + /* RTW_INFO("+issue_probersp during ap mode\n"); */ + issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq); + } + + } + +exit: + return _SUCCESS; + +} + +unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + u8 *pframe = precv_frame->u.hdr.rx_data; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif + + +#ifdef CONFIG_P2P + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) { + if (_TRUE == pwdinfo->tx_prov_disc_info.benable) { + if (_rtw_memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr, get_addr2_ptr(pframe), ETH_ALEN)) { + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) { + pwdinfo->tx_prov_disc_info.benable = _FALSE; + issue_p2p_provision_request(padapter, + pwdinfo->tx_prov_disc_info.ssid.Ssid, + pwdinfo->tx_prov_disc_info.ssid.SsidLength, + pwdinfo->tx_prov_disc_info.peerDevAddr); + } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + pwdinfo->tx_prov_disc_info.benable = _FALSE; + issue_p2p_provision_request(padapter, + NULL, + 0, + pwdinfo->tx_prov_disc_info.peerDevAddr); + } + } + } + return _SUCCESS; + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) { + if (_TRUE == pwdinfo->nego_req_info.benable) { + RTW_INFO("[%s] P2P State is GONEGO ING!\n", __FUNCTION__); + if (_rtw_memcmp(pwdinfo->nego_req_info.peerDevAddr, get_addr2_ptr(pframe), ETH_ALEN)) { + pwdinfo->nego_req_info.benable = _FALSE; + issue_p2p_GO_request(padapter, pwdinfo->nego_req_info.peerDevAddr); + } + } + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) { + if (_TRUE == pwdinfo->invitereq_info.benable) { + RTW_INFO("[%s] P2P_STATE_TX_INVITE_REQ!\n", __FUNCTION__); + if (_rtw_memcmp(pwdinfo->invitereq_info.peer_macaddr, get_addr2_ptr(pframe), ETH_ALEN)) { + pwdinfo->invitereq_info.benable = _FALSE; + issue_p2p_invitation_request(padapter, pwdinfo->invitereq_info.peer_macaddr); + } + } + } +#endif + + + if ((mlmeext_chk_scan_state(pmlmeext, SCAN_PROCESS)) + || (MLME_IS_MESH(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE)) + #ifdef CONFIG_RTW_REPEATER_SON + || (padapter->rtw_rson_scanstage == RSON_SCAN_PROCESS) + #endif + ) { + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN) + && (pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + ) { + if (!rtw_check_bcn_info(padapter, pframe, precv_frame->u.hdr.len)) { + RTW_PRINT(FUNC_ADPT_FMT" ap has changed, disconnect now\n", FUNC_ADPT_ARG(padapter)); + receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0, _FALSE); + } + } + + rtw_mi_report_survey_event(padapter, precv_frame); + return _SUCCESS; + } + +#if 0 /* move to validate_recv_mgnt_frame */ + if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) { + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta != NULL) + psta->sta_stats.rx_mgnt_pkts++; + } + } +#endif + + return _SUCCESS; + +} + +/* for 11n Logo 4.2.31/4.2.32 */ +#ifdef CONFIG_AP_MODE +static void rtw_check_legacy_ap(_adapter *padapter, u8 *pframe, u32 len) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (!padapter->registrypriv.wifi_spec) + return; + + if(!MLME_IS_AP(padapter)) + return; + + if (pmlmeext->bstart_bss == _TRUE) { + int left; + unsigned char *pos; + struct rtw_ieee802_11_elems elems; +#ifdef CONFIG_80211N_HT + u16 cur_op_mode; +#endif + /* checking IEs */ + left = len - sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_; + pos = pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_; + if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed) { + RTW_INFO("%s: parse fail for "MAC_FMT"\n", __func__, MAC_ARG(GetAddr3Ptr(pframe))); + return; + } +#ifdef CONFIG_80211N_HT + cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK; +#endif + /* for legacy ap */ + if (elems.ht_capabilities == NULL && elems.ht_capabilities_len == 0) { + + if (0) + RTW_INFO("%s: "MAC_FMT" is legacy ap\n", __func__, MAC_ARG(GetAddr3Ptr(pframe))); + + ATOMIC_SET(&pmlmepriv->olbc, _TRUE); + ATOMIC_SET(&pmlmepriv->olbc_ht, _TRUE); + } + } +} +#endif /* CONFIG_AP_MODE */ + +unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame) +{ + struct sta_info *psta; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + WLAN_BSSID_EX *pbss; + int ret = _SUCCESS; +#ifdef CONFIG_TDLS + struct sta_info *ptdls_sta; + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; +#ifdef CONFIG_TDLS_CH_SW + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; +#endif +#endif /* CONFIG_TDLS */ +#ifdef CONFIG_LAYER2_ROAMING + _irqL irqL; +#endif + + if (validate_beacon_len(pframe, len) == _FALSE) + return _SUCCESS; + + if (mlmeext_chk_scan_state(pmlmeext, SCAN_PROCESS) + || (MLME_IS_MESH(padapter) && check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) + ) { + if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN) + && (pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + ) { + if (!rtw_check_bcn_info(padapter, pframe, len)) { + RTW_PRINT(FUNC_ADPT_FMT" ap has changed, disconnect now\n", FUNC_ADPT_ARG(padapter)); + receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0, _FALSE); + } + } + + rtw_mi_report_survey_event(padapter, precv_frame); + return _SUCCESS; + } + +#ifdef CONFIG_WRITE_BCN_LEN_TO_FW + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) + && (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))) { + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + + if (pmlmeinfo->last_bcn_len != pattrib->pkt_len) { + pmlmeinfo->last_bcn_len = pattrib->pkt_len; + rtw_write_bcnlen_to_fw_cmd(padapter, pattrib->pkt_len); + } + } +#endif + +#ifdef CONFIG_RTW_REPEATER_SON + if (padapter->rtw_rson_scanstage == RSON_SCAN_PROCESS) + rtw_mi_report_survey_event(padapter, precv_frame); +#endif + +#ifdef CONFIG_AP_MODE + rtw_check_legacy_ap(padapter, pframe, len); +#endif + + if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) { + if ((pmlmeinfo->state & WIFI_FW_AUTH_NULL) + && (rtw_sta_linking_test_wait_done() || pmlmeext->join_abort) + ) { + if (rtw_sta_linking_test_force_fail() || pmlmeext->join_abort) { + set_link_timer(pmlmeext, 1); + return _SUCCESS; + } +#ifdef CONFIG_LAYER2_ROAMING + _enter_critical_bh(&pmlmepriv->clnt_auth_lock, &irqL); +#endif + /* we should update current network before auth, or some IE is wrong */ + pbss = (WLAN_BSSID_EX *)rtw_malloc(sizeof(WLAN_BSSID_EX)); + if (pbss) { + if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) { + struct beacon_keys recv_beacon; + + update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE); + + /* update bcn keys */ + if (rtw_get_bcn_keys(padapter, pframe, len, &recv_beacon) == _TRUE) { + RTW_INFO("%s: beacon keys ready\n", __func__); + _rtw_memcpy(&pmlmepriv->cur_beacon_keys, + &recv_beacon, sizeof(recv_beacon)); + if (is_hidden_ssid(recv_beacon.ssid, recv_beacon.ssid_len)) { + _rtw_memcpy(pmlmepriv->cur_beacon_keys.ssid, pmlmeinfo->network.Ssid.Ssid, IW_ESSID_MAX_SIZE); + pmlmepriv->cur_beacon_keys.ssid_len = pmlmeinfo->network.Ssid.SsidLength; + } + } else { + RTW_ERR("%s: get beacon keys failed\n", __func__); + _rtw_memset(&pmlmepriv->cur_beacon_keys, 0, sizeof(recv_beacon)); + } + #ifdef CONFIG_BCN_CNT_CONFIRM_HDL + pmlmepriv->new_beacon_cnts = 0; + #endif + } + rtw_mfree((u8 *)pbss, sizeof(WLAN_BSSID_EX)); + } + + /* check the vendor of the assoc AP */ + pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe + sizeof(struct rtw_ieee80211_hdr_3addr), len - sizeof(struct rtw_ieee80211_hdr_3addr)); + + /* update TSF Value */ + update_TSF(pmlmeext, pframe, len); + pmlmeext->bcn_cnt = 0; + pmlmeext->last_bcn_cnt = 0; + +#ifdef CONFIG_P2P_PS + /* Comment by YiWei , in wifi p2p spec the "3.3 P2P Power Management" , "These mechanisms are available in a P2P Group in which only P2P Devices are associated." */ + /* process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN)); */ +#endif /* CONFIG_P2P_PS */ + +#if defined(CONFIG_P2P) && defined(CONFIG_CONCURRENT_MODE) + if (padapter->registrypriv.wifi_spec) { + if (process_p2p_cross_connect_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN)) == _FALSE) { + if (rtw_mi_buddy_check_mlmeinfo_state(padapter, WIFI_FW_AP_STATE)) { + RTW_PRINT("no issue auth, P2P cross-connect does not permit\n "); + return _SUCCESS; + } + } + } +#endif /* CONFIG_P2P CONFIG_P2P and CONFIG_CONCURRENT_MODE */ + + /* start auth */ + start_clnt_auth(padapter); +#ifdef CONFIG_LAYER2_ROAMING + _exit_critical_bh(&pmlmepriv->clnt_auth_lock, &irqL); +#endif + return _SUCCESS; + } + + if (((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) { + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta != NULL) { +#ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL + /* Merge from 8712 FW code */ + if (cmp_pkt_chnl_diff(padapter, pframe, len) != 0) { + /* join wrong channel, deauth and reconnect */ + issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING); + + report_del_sta_event(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL, _TRUE, _FALSE); + pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS); + return _SUCCESS; + } +#endif /* CONFIG_PATCH_JOIN_WRONG_CHANNEL */ +#ifdef CONFIG_RTW_80211R + rtw_ft_update_bcn(padapter, precv_frame); +#endif + ret = rtw_check_bcn_info(padapter, pframe, len); + if (!ret) { + RTW_PRINT(FUNC_ADPT_FMT" ap has changed, disconnect now\n", FUNC_ADPT_ARG(padapter)); + receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0, _FALSE); + return _SUCCESS; + } + /* update WMM, ERP in the beacon */ + /* todo: the timer is used instead of the number of the beacon received */ + if ((sta_rx_pkts(psta) & 0xf) == 0) { + /* RTW_INFO("update_bcn_info\n"); */ + update_beacon_info(padapter, pframe, len, psta); + } + + pmlmepriv->cur_network_scanned->network.Rssi = precv_frame->u.hdr.attrib.phy_info.recv_signal_power; + pmlmeext->bcn_cnt++; +#ifdef CONFIG_BCN_RECV_TIME + rtw_rx_bcn_time_update(padapter, len, precv_frame->u.hdr.attrib.data_rate); +#endif +#ifdef CONFIG_TDLS +#ifdef CONFIG_TDLS_CH_SW + if (rtw_tdls_is_chsw_allowed(padapter) == _TRUE) { + /* Send TDLS Channel Switch Request when receiving Beacon */ + if ((padapter->tdlsinfo.chsw_info.ch_sw_state & TDLS_CH_SW_INITIATOR_STATE) && (ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) + && (pmlmeext->cur_channel == rtw_get_oper_ch(padapter))) { + ptdls_sta = rtw_get_stainfo(&padapter->stapriv, padapter->tdlsinfo.chsw_info.addr); + if (ptdls_sta != NULL) { + if (ptdls_sta->tdls_sta_state | TDLS_LINKED_STATE) + _set_timer(&ptdls_sta->stay_on_base_chnl_timer, TDLS_CH_SW_STAY_ON_BASE_CHNL_TIMEOUT); + } + } + } +#endif +#endif /* CONFIG_TDLS */ + + #if CONFIG_DFS + process_csa_ie(padapter + , pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_ + , len - (WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_)); + #endif + +#ifdef CONFIG_P2P_PS + process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN)); +#endif /* CONFIG_P2P_PS */ + + if (pmlmeext->tsf_update_required && pmlmeext->en_hw_update_tsf) + rtw_enable_hw_update_tsf_cmd(padapter); + +#if 0 /* move to validate_recv_mgnt_frame */ + psta->sta_stats.rx_mgnt_pkts++; +#endif + } + + } else if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) { + u8 rate_set[16]; + u8 rate_num = 0; + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta != NULL) { + /* + * update WMM, ERP in the beacon + * todo: the timer is used instead of the number of the beacon received + */ + if ((sta_rx_pkts(psta) & 0xf) == 0) + update_beacon_info(padapter, pframe, len, psta); + + if (pmlmeext->tsf_update_required && pmlmeext->en_hw_update_tsf) + rtw_enable_hw_update_tsf_cmd(padapter); + } else { + rtw_ies_get_supported_rate(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_, rate_set, &rate_num); + if (rate_num == 0) { + RTW_INFO(FUNC_ADPT_FMT" RX beacon with no supported rate\n", FUNC_ADPT_ARG(padapter)); + goto _END_ONBEACON_; + } + + psta = rtw_alloc_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta == NULL) { + RTW_INFO(FUNC_ADPT_FMT" Exceed the upper limit of supported clients\n", FUNC_ADPT_ARG(padapter)); + goto _END_ONBEACON_; + } + + psta->expire_to = pstapriv->adhoc_expire_to; + + _rtw_memcpy(psta->bssrateset, rate_set, rate_num); + psta->bssratelen = rate_num; + + /* update TSF Value */ + update_TSF(pmlmeext, pframe, len); + + /* report sta add event */ + report_add_sta_event(padapter, get_addr2_ptr(pframe)); + } + } + } + +_END_ONBEACON_: + + return _SUCCESS; + +} + +#ifdef CONFIG_AP_MODE +static u32 rtw_get_sta_num_by_state(_adapter *padapter, u32 state) +{ + _irqL irqL; + _list *plist, *phead; + u32 index, sta_num = 0; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &(padapter->stapriv); + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (index = 0; index < NUM_STA; index++) { + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info , hash_list); + if ((psta->state & (state))) + sta_num++; + plist = get_next(plist); + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + /* RTW_INFO("%s : waiting for %u sta under linking \n", __func__, sta_num); */ + return sta_num; +} + +static u8 rtw_defs_attack_chk(_adapter *padapter) +{ + struct mlme_priv *mlme = &(padapter->mlmepriv); + u8 is_reject = _FALSE; + u32 sta_limit = 0; + u32 stime = rtw_systime_to_ms(rtw_get_current_time()); + static u32 ptime = 0; + + /* RTW_INFO("%s : ptime=%u, stime=%u, diff=%u\n", __func__, ptime, stime, (stime - ptime)); */ + if ((ptime > 0) && ((stime - ptime) < mlme->defs_lmt_time)) { + sta_limit = rtw_get_sta_num_by_state(padapter, WIFI_FW_LINKING_STATE); + if (sta_limit >= mlme->defs_lmt_sta) + is_reject = _TRUE; + } + + ptime = stime; + /* RTW_INFO("%s : current linking num=%u\n", __func__, sta_limit); */ + return is_reject; +} +#endif + +unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame) +{ +#ifdef CONFIG_AP_MODE + _irqL irqL; + unsigned int auth_mode, seq, ie_len; + unsigned char *sa, *p; + u16 algorithm; + int status; + static struct sta_info stat; + struct sta_info *pstat = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + u8 offset = 0; + + +#ifdef CONFIG_CONCURRENT_MODE + if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) && + rtw_mi_buddy_check_fwstate(padapter, WIFI_UNDER_LINKING | WIFI_UNDER_SURVEY)) { + /* don't process auth request; */ + return _SUCCESS; + } +#endif /* CONFIG_CONCURRENT_MODE */ + + if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE) + return _FAIL; + + if (!MLME_IS_ASOC(padapter)) + return _SUCCESS; + +#if defined(CONFIG_IOCTL_CFG80211) && defined(CONFIG_RTW_MESH) + if (MLME_IS_MESH(padapter)) + return rtw_mesh_on_auth(padapter, precv_frame); +#endif + + RTW_INFO("+OnAuth\n"); + + sa = get_addr2_ptr(pframe); + + auth_mode = psecuritypriv->dot11AuthAlgrthm; + + if (GetPrivacy(pframe)) { + u8 *iv; + struct rx_pkt_attrib *prxattrib = &(precv_frame->u.hdr.attrib); + + prxattrib->hdrlen = WLAN_HDR_A3_LEN; + prxattrib->encrypt = _WEP40_; + + iv = pframe + prxattrib->hdrlen; + prxattrib->key_index = ((iv[3] >> 6) & 0x3); + + prxattrib->iv_len = 4; + prxattrib->icv_len = 4; + + rtw_wep_decrypt(padapter, (u8 *)precv_frame); + + offset = 4; + } + + algorithm = le16_to_cpu(*(u16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset)); + seq = le16_to_cpu(*(u16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2)); + + RTW_INFO("auth alg=%x, seq=%X\n", algorithm, seq); + + if (rtw_check_invalid_mac_address(sa, _FALSE)){ + RTW_INFO("%s : reject invalid AUTH-req "MAC_FMT"\n", + __func__, MAC_ARG(get_addr2_ptr(pframe))); + return _FAIL; + } + + if(rtw_defs_attack_chk(padapter)) { + struct sta_info *_psta; + _psta = rtw_get_stainfo(pstapriv, sa); + if ((_psta == NULL) || !(_psta->state & WIFI_FW_ASSOC_SUCCESS)) { + status = _STATS_REFUSED_TEMPORARILY_; + RTW_ERR("%s : refused temporarily for sa "MAC_FMT" !\n", __func__, MAC_ARG(sa)); + goto auth_fail; + } + } + + if (rtw_ap_linking_test_force_auth_fail()) { + status = rtw_ap_linking_test_force_auth_fail(); + RTW_INFO(FUNC_ADPT_FMT" force auth fail with status:%u\n" + , FUNC_ADPT_ARG(padapter), status); + goto auth_fail; + } + + if ((auth_mode == 2) && (algorithm != WLAN_AUTH_SAE) && + (psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && + (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)) + auth_mode = 0; + + if ((algorithm > 0 && auth_mode == 0) || /* rx a shared-key auth but shared not enabled */ + (algorithm == 0 && auth_mode == 1)) { /* rx a open-system auth but shared-key is enabled */ + RTW_INFO("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n", + algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]); + + status = _STATS_NO_SUPP_ALG_; + + goto auth_fail; + } + +#if CONFIG_RTW_MACADDR_ACL + if (rtw_access_ctrl(padapter, sa) == _FALSE) { + status = _STATS_UNABLE_HANDLE_STA_; + goto auth_fail; + } +#endif + + pstat = rtw_get_stainfo(pstapriv, sa); + if (pstat == NULL) { + + /* allocate a new one */ + RTW_INFO("going to alloc stainfo for sa="MAC_FMT"\n", MAC_ARG(sa)); + pstat = rtw_alloc_stainfo(pstapriv, sa); + if (pstat == NULL) { + RTW_INFO(" Exceed the upper limit of supported clients...\n"); + status = _STATS_UNABLE_HANDLE_STA_; + goto auth_fail; + } + + pstat->state = WIFI_FW_AUTH_NULL; + pstat->auth_seq = 0; + + /* pstat->flags = 0; */ + /* pstat->capability = 0; */ + } else { +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&pstat->asoc_list) == _FALSE) { + rtw_list_delete(&pstat->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (pstat->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + if (pstat->expire_to > 0) + ;/* TODO: STA re_auth within expire_to */ + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + if (seq == 1) + ; /* TODO: STA re_auth and auth timeout */ + + } + } + +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + _enter_critical_bh(&pstapriv->auth_list_lock, &irqL); + if (rtw_is_list_empty(&pstat->auth_list)) { + + rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list); + pstapriv->auth_list_cnt++; + } + _exit_critical_bh(&pstapriv->auth_list_lock, &irqL); + } + + if (pstat->auth_seq == 0) + pstat->expire_to = pstapriv->auth_to; + +#ifdef CONFIG_IOCTL_CFG80211 + if (GET_CFG80211_REPORT_MGMT(adapter_wdev_data(padapter), IEEE80211_STYPE_AUTH) == _TRUE) { + if ((algorithm == WLAN_AUTH_SAE) && + (auth_mode == dot11AuthAlgrthm_8021X)) { + pstat->authalg = algorithm; + + rtw_cfg80211_rx_mframe(padapter, precv_frame, NULL); + return _SUCCESS; + } + } +#endif /* CONFIG_IOCTL_CFG80211 */ + + if ((pstat->auth_seq + 1) != seq) { + RTW_INFO("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n", + seq, pstat->auth_seq + 1); + status = _STATS_OUT_OF_AUTH_SEQ_; + goto auth_fail; + } + + if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3)) { + if (seq == 1) { +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + pstat->state &= ~WIFI_FW_AUTH_NULL; + pstat->state |= WIFI_FW_AUTH_SUCCESS; + pstat->expire_to = pstapriv->assoc_to; + } + pstat->authalg = algorithm; + } else { + RTW_INFO("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n", + seq, pstat->auth_seq + 1); + status = _STATS_OUT_OF_AUTH_SEQ_; + goto auth_fail; + } + } else { /* shared system or auto authentication */ + if (seq == 1) { + /* prepare for the challenging txt... */ + + /* get_random_bytes((void *)pstat->chg_txt, 128); */ /* TODO: */ + _rtw_memset((void *)pstat->chg_txt, 78, 128); +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + pstat->state &= ~WIFI_FW_AUTH_NULL; + pstat->state |= WIFI_FW_AUTH_STATE; + } + pstat->authalg = algorithm; + pstat->auth_seq = 2; + } else if (seq == 3) { + /* checking for challenging txt... */ + RTW_INFO("checking for challenging txt...\n"); + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len, + len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4); + + if ((p == NULL) || (ie_len <= 0)) { + RTW_INFO("auth rejected because challenge failure!(1)\n"); + status = _STATS_CHALLENGE_FAIL_; + goto auth_fail; + } + + if (_rtw_memcmp((void *)(p + 2), pstat->chg_txt, 128)) { +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + pstat->state &= (~WIFI_FW_AUTH_STATE); + pstat->state |= WIFI_FW_AUTH_SUCCESS; + /* challenging txt is correct... */ + pstat->expire_to = pstapriv->assoc_to; + } + } else { + RTW_INFO("auth rejected because challenge failure!\n"); + status = _STATS_CHALLENGE_FAIL_; + goto auth_fail; + } + } else { + RTW_INFO("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n", + seq, pstat->auth_seq + 1); + status = _STATS_OUT_OF_AUTH_SEQ_; + goto auth_fail; + } + } + + + /* Now, we are going to issue_auth... */ + pstat->auth_seq = seq + 1; + +#ifdef CONFIG_NATIVEAP_MLME + issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_)); +#endif + + if ((pstat->state & WIFI_FW_AUTH_SUCCESS) || (pstat->state & WIFI_FW_ASSOC_SUCCESS)) + pstat->auth_seq = 0; + + + return _SUCCESS; + +auth_fail: + + if (pstat) + rtw_free_stainfo(padapter , pstat); + + pstat = &stat; + _rtw_memset((char *)pstat, '\0', sizeof(stat)); + pstat->auth_seq = 2; + _rtw_memcpy(pstat->cmn.mac_addr, sa, ETH_ALEN); + +#ifdef CONFIG_NATIVEAP_MLME + issue_auth(padapter, pstat, (unsigned short)status); +#endif + +#endif /* CONFIG_AP_MODE */ + return _FAIL; + +} + +unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int seq, len, status, algthm, offset; + unsigned char *p; + unsigned int go2asoc = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; + uint pkt_len = precv_frame->u.hdr.len; + + RTW_INFO("%s\n", __FUNCTION__); + +#ifdef CONFIG_IOCTL_CFG80211 + if (GET_CFG80211_REPORT_MGMT(adapter_wdev_data(padapter), IEEE80211_STYPE_AUTH) == _TRUE) { + if (rtw_sec_chk_auth_type(padapter, MLME_AUTHTYPE_SAE)) { + if (rtw_cached_pmkid(padapter, get_my_bssid(&pmlmeinfo->network)) != -1) { + RTW_INFO("SAE: PMKSA cache entry found\n"); + goto normal; + } + rtw_cfg80211_rx_mframe(padapter, precv_frame, NULL); + return _SUCCESS; + } + } + +normal: +#endif /* CONFIG_IOCTL_CFG80211 */ + + /* check A1 matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN)) + return _SUCCESS; + + if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE) || pmlmeext->join_abort) + return _SUCCESS; + + offset = (GetPrivacy(pframe)) ? 4 : 0; + + algthm = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset)); + seq = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2)); + status = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4)); + + if (status != 0) { + RTW_INFO("clnt auth fail, status: %d\n", status); + if (status == 13) { /* && pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */ + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) + pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open; + else + pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; + /* pmlmeinfo->reauth_count = 0; */ + } + + pmlmeinfo->auth_status = status; + set_link_timer(pmlmeext, 1); + goto authclnt_fail; + } + + if (seq == 2) { + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) { + /* legendary shared system */ + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len, + pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_); + + if (p == NULL) { + /* RTW_INFO("marc: no challenge text?\n"); */ + goto authclnt_fail; + } + + _rtw_memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len); + pmlmeinfo->auth_seq = 3; + issue_auth(padapter, NULL, 0); + set_link_timer(pmlmeext, REAUTH_TO); + + return _SUCCESS; + } else { + /* open, or 802.11r FTAA system */ + go2asoc = 1; + } + } else if (seq == 4) { + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) + go2asoc = 1; + else + goto authclnt_fail; + } else { + /* this is also illegal */ + /* RTW_INFO("marc: clnt auth failed due to illegal seq=%x\n", seq); */ + goto authclnt_fail; + } + + if (go2asoc) { +#ifdef CONFIG_RTW_80211R + if (rtw_ft_update_auth_rsp_ies(padapter, pframe, pkt_len)) + return _SUCCESS; +#endif + RTW_PRINT("auth success, start assoc\n"); + start_clnt_assoc(padapter); + return _SUCCESS; + } + +authclnt_fail: + + /* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */ + + return _FAIL; + +} + +unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame) +{ +#ifdef CONFIG_AP_MODE + _irqL irqL; + u16 listen_interval; + struct rtw_ieee802_11_elems elems; + struct sta_info *pstat; + unsigned char reassoc, *pos; + int left; + unsigned short status = _STATS_SUCCESSFUL_; + unsigned short frame_type, ie_offset = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur = &(pmlmeinfo->network); + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint pkt_len = precv_frame->u.hdr.len; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u8 p2p_status_code = P2P_STATUS_SUCCESS; + u8 *p2pie; + u32 p2pielen = 0; +#endif /* CONFIG_P2P */ +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + u8 sta_tbtx_enable = _FALSE; +#endif + +#ifdef CONFIG_CONCURRENT_MODE + if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) && + rtw_mi_buddy_check_fwstate(padapter, WIFI_UNDER_LINKING | WIFI_UNDER_SURVEY)) { + /* don't process assoc request; */ + return _SUCCESS; + } +#endif /* CONFIG_CONCURRENT_MODE */ + + if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE) + return _FAIL; + + if (rtw_check_invalid_mac_address(get_addr2_ptr(pframe), _FALSE)) { + RTW_INFO("%s : reject invalid ASSOC-req "MAC_FMT"\n", + __func__, MAC_ARG(get_addr2_ptr(pframe))); + return _FAIL; + } + + frame_type = get_frame_sub_type(pframe); + if (frame_type == WIFI_ASSOCREQ) { + reassoc = 0; + ie_offset = _ASOCREQ_IE_OFFSET_; + } else { /* WIFI_REASSOCREQ */ + reassoc = 1; + ie_offset = _REASOCREQ_IE_OFFSET_; + } + + + if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) { + RTW_INFO("handle_assoc(reassoc=%d) - too short payload (len=%lu)" + "\n", reassoc, (unsigned long)pkt_len); + return _FAIL; + } + + pstat = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (pstat == (struct sta_info *)NULL) { + status = _RSON_CLS2_; + goto asoc_class2_error; + } + + RTW_INFO("%s\n", __FUNCTION__); + + if (pstat->authalg == WLAN_AUTH_SAE) { + /* WPA3-SAE */ + if (((pstat->state) & WIFI_FW_AUTH_NULL)) { + /* TODO: + Queue AssocReq and Proccess + by external auth trigger. */ + RTW_INFO("%s: wait external auth trigger\n", __func__); + return _SUCCESS; + } + } + + /* check if this stat has been successfully authenticated/assocated */ + if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) { + if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) { + status = _RSON_CLS2_; + goto asoc_class2_error; + } else { + pstat->state &= (~WIFI_FW_ASSOC_SUCCESS); + pstat->state |= WIFI_FW_ASSOC_STATE; + } + } else { + pstat->state &= (~WIFI_FW_AUTH_SUCCESS); + pstat->state |= WIFI_FW_ASSOC_STATE; + } + +#if 0/* todo:tkip_countermeasures */ + if (hapd->tkip_countermeasures) { + resp = WLAN_REASON_MICHAEL_MIC_FAILURE; + goto fail; + } +#endif + + if (rtw_ap_linking_test_force_asoc_fail()) { + status = rtw_ap_linking_test_force_asoc_fail(); + RTW_INFO(FUNC_ADPT_FMT" force asoc fail with status:%u\n" + , FUNC_ADPT_ARG(padapter), status); + goto OnAssocReqFail; + } + + /* now parse all ieee802_11 ie to point to elems */ + left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset); + pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset); + if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed) { + RTW_INFO("STA " MAC_FMT " sent invalid association request\n", + MAC_ARG(pstat->cmn.mac_addr)); + status = _STATS_FAILURE_; + goto OnAssocReqFail; + } + + rtw_ap_parse_sta_capability(padapter, pstat, pframe + WLAN_HDR_A3_LEN); + + listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN + 2); +#if 0/* todo: */ + /* check listen_interval */ + if (listen_interval > hapd->conf->max_listen_interval) { + hostapd_logger(hapd, mgmt->sa, HOSTAPD_MODULE_IEEE80211, + HOSTAPD_LEVEL_DEBUG, + "Too large Listen Interval (%d)", + listen_interval); + resp = WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE; + goto fail; + } + + pstat->listen_interval = listen_interval; +#endif + + /* now we should check all the fields... */ + /* checking SSID */ + if (elems.ssid == NULL + || elems.ssid_len == 0 + || elems.ssid_len != cur->Ssid.SsidLength + || _rtw_memcmp(elems.ssid, cur->Ssid.Ssid, cur->Ssid.SsidLength) == _FALSE + ) { + status = _STATS_FAILURE_; + goto OnAssocReqFail; + } + + /* (Extended) Supported rates */ + status = rtw_ap_parse_sta_supported_rates(padapter, pstat + , pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset); + if (status != _STATS_SUCCESSFUL_) + goto OnAssocReqFail; + + /* check RSN/WPA/WPS */ + status = rtw_ap_parse_sta_security_ie(padapter, pstat, &elems); + if (status != _STATS_SUCCESSFUL_) + goto OnAssocReqFail; + + /* check if there is WMM IE & support WWM-PS */ + rtw_ap_parse_sta_wmm_ie(padapter, pstat + , pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset); + +#ifdef CONFIG_RTS_FULL_BW + /*check vendor IE*/ + rtw_parse_sta_vendor_ie_8812(padapter, pstat + , pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset); +#endif/*CONFIG_RTS_FULL_BW*/ + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (elems.tbtx_cap && elems.tbtx_cap_len != 0) { + if(rtw_is_tbtx_capabilty(elems.tbtx_cap, elems.tbtx_cap_len)) { + sta_tbtx_enable = _TRUE; + } + } + +#endif + + rtw_ap_parse_sta_ht_ie(padapter, pstat, &elems); + rtw_ap_parse_sta_vht_ie(padapter, pstat, &elems); + + if (((pstat->flags & WLAN_STA_HT) || (pstat->flags & WLAN_STA_VHT)) && + ((pstat->wpa2_pairwise_cipher & WPA_CIPHER_TKIP) || + (pstat->wpa_pairwise_cipher & WPA_CIPHER_TKIP))) { + + RTW_INFO("(V)HT: " MAC_FMT " tried to use TKIP with (V)HT association\n", MAC_ARG(pstat->cmn.mac_addr)); + + pstat->flags &= ~WLAN_STA_HT; + pstat->flags &= ~WLAN_STA_VHT; + /*status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; + * goto OnAssocReqFail; + */ + } + +#ifdef CONFIG_P2P + pstat->is_p2p_device = _FALSE; + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen); + if (p2pie) { + pstat->is_p2p_device = _TRUE; + p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat); + if (p2p_status_code > 0) { + pstat->p2p_status_code = p2p_status_code; + status = _STATS_CAP_FAIL_; + goto OnAssocReqFail; + } + } +#ifdef CONFIG_WFD + rtw_process_wfd_ies(padapter, pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset, __func__); +#endif + } + pstat->p2p_status_code = p2p_status_code; +#endif /* CONFIG_P2P */ + + rtw_ap_parse_sta_multi_ap_ie(padapter, pstat, pos, left); + +#ifdef CONFIG_RTW_REPEATER_SON + if (rtw_rson_ap_check_sta(padapter, pframe, pkt_len, ie_offset)) + goto OnAssocReqFail; +#endif + + /* TODO: identify_proprietary_vendor_ie(); */ + /* Realtek proprietary IE */ + /* identify if this is Broadcom sta */ + /* identify if this is ralink sta */ + /* Customer proprietary IE */ + +#ifdef CONFIG_RTW_80211K + rtw_ap_parse_sta_rm_en_cap(padapter, pstat, &elems); +#endif + + /* AID assignment */ + if (pstat->cmn.aid > 0) + RTW_INFO(FUNC_ADPT_FMT" old AID=%d\n", FUNC_ADPT_ARG(padapter), pstat->cmn.aid); + else { + if (!rtw_aid_alloc(padapter, pstat)) { + RTW_INFO(FUNC_ADPT_FMT" no room for more AIDs\n", FUNC_ADPT_ARG(padapter)); + status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA; + goto OnAssocReqFail; + } + RTW_INFO(FUNC_ADPT_FMT" allocate new AID=%d\n", FUNC_ADPT_ARG(padapter), pstat->cmn.aid); + } + + pstat->state &= (~WIFI_FW_ASSOC_STATE); + pstat->state |= WIFI_FW_ASSOC_SUCCESS; + /* RTW_INFO("==================%s, %d, (%x), bpairwise_key_installed=%d, MAC:"MAC_FMT"\n" + , __func__, __LINE__, pstat->state, pstat->bpairwise_key_installed, MAC_ARG(pstat->cmn.mac_addr)); */ +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + _enter_critical_bh(&pstapriv->auth_list_lock, &irqL); + if (!rtw_is_list_empty(&pstat->auth_list)) { + rtw_list_delete(&pstat->auth_list); + pstapriv->auth_list_cnt--; + } + _exit_critical_bh(&pstapriv->auth_list_lock, &irqL); + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&pstat->asoc_list)) { + pstat->expire_to = pstapriv->expire_to; + rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list); + pstapriv->asoc_list_cnt++; +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (sta_tbtx_enable) { + pstat->tbtx_enable = _TRUE; + pstapriv->tbtx_asoc_list_cnt++; + } +#endif + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + } + + /* now the station is qualified to join our BSS... */ + if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) { +#ifdef CONFIG_NATIVEAP_MLME +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + /* .1 bss_cap_update & sta_info_update */ + bss_cap_update_on_sta_join(padapter, pstat); + sta_info_update(padapter, pstat); + } +#ifdef CONFIG_IEEE80211W + if (pstat->bpairwise_key_installed == _TRUE && (pstat->flags & WLAN_STA_MFP)) + status = _STATS_REFUSED_TEMPORARILY_; +#endif /* CONFIG_IEEE80211W */ + /* .2 issue assoc rsp before notify station join event. */ + if (frame_type == WIFI_ASSOCREQ) + issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP); + else + issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP); + +#ifdef CONFIG_IOCTL_CFG80211 + _enter_critical_bh(&pstat->lock, &irqL); + if (pstat->passoc_req) { + rtw_mfree(pstat->passoc_req, pstat->assoc_req_len); + pstat->passoc_req = NULL; + pstat->assoc_req_len = 0; + } + + pstat->passoc_req = rtw_zmalloc(pkt_len); + if (pstat->passoc_req) { + _rtw_memcpy(pstat->passoc_req, pframe, pkt_len); + pstat->assoc_req_len = pkt_len; + } + _exit_critical_bh(&pstat->lock, &irqL); +#endif /* CONFIG_IOCTL_CFG80211 */ +#ifdef CONFIG_IEEE80211W + if ((pstat->bpairwise_key_installed != _TRUE && (pstat->flags & WLAN_STA_MFP)) + || !(pstat->flags & WLAN_STA_MFP)) +#endif /* CONFIG_IEEE80211W */ + { + /* .3-(1) report sta add event */ + report_add_sta_event(padapter, pstat->cmn.mac_addr); + } +#ifdef CONFIG_IEEE80211W + if (pstat->bpairwise_key_installed == _TRUE && (pstat->flags & WLAN_STA_MFP)) { + RTW_INFO(MAC_FMT"\n", MAC_ARG(pstat->cmn.mac_addr)); + issue_action_SA_Query(padapter, pstat->cmn.mac_addr, 0, 0, IEEE80211W_RIGHT_KEY); + } +#endif /* CONFIG_IEEE80211W */ +#endif /* CONFIG_NATIVEAP_MLME */ + } + + return _SUCCESS; + +asoc_class2_error: + +#ifdef CONFIG_NATIVEAP_MLME + issue_deauth(padapter, (void *)get_addr2_ptr(pframe), status); +#endif + + return _FAIL; + +OnAssocReqFail: + + +#ifdef CONFIG_NATIVEAP_MLME + /* pstat->cmn.aid = 0; */ + if (frame_type == WIFI_ASSOCREQ) + issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP); + else + issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP); +#endif + + +#endif /* CONFIG_AP_MODE */ + + return _FAIL; + +} + +#if defined(CONFIG_LAYER2_ROAMING) && defined(CONFIG_RTW_80211K) +void rtw_roam_nb_discover(_adapter *padapter, u8 bfroce) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + u8 nb_req_issue = _FALSE; + + if (!check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) + return; + + if (!rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE)) + return; + + psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress); + if (!psta) + return; + + if (bfroce || (!pmlmepriv->nb_info.nb_rpt_is_same)) + nb_req_issue = _TRUE; + + if (nb_req_issue && (psta->rm_en_cap[0] & RTW_RRM_NB_RPT_EN)) + rm_add_nb_req(padapter, psta); +} +#endif + +unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame) +{ + uint i; + int res; + unsigned short status; + PNDIS_802_11_VARIABLE_IEs pIE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + /* WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); */ + u8 *pframe = precv_frame->u.hdr.rx_data; + uint pkt_len = precv_frame->u.hdr.len; +#ifdef CONFIG_WAPI_SUPPORT + PNDIS_802_11_VARIABLE_IEs pWapiIE = NULL; +#endif + + RTW_INFO("%s\n", __FUNCTION__); + + /* check A1 matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN)) + return _SUCCESS; + + if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)) || pmlmeext->join_abort) + return _SUCCESS; + + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + return _SUCCESS; + + _cancel_timer_ex(&pmlmeext->link_timer); + + /* status */ + status = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 2)); + if (status > 0) { + RTW_INFO("assoc reject, status code: %d\n", status); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + res = -4; + goto report_assoc_result; + } + + /* get capabilities */ + pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); + + /* set slot time */ + pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20; + + /* AID */ + res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 4)) & 0x3fff); + + /* check aid value */ + if (res < 1 || res > 2007) { + RTW_INFO("assoc reject, aid: %d\n", res); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + res = -4; + goto report_assoc_result; + } + + /* following are moved to join event callback function */ + /* to handle HT, WMM, rate adaptive, update MAC reg */ + /* for not to handle the synchronous IO in the tasklet */ + for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6)) /* WMM */ + WMM_param_handler(padapter, pIE); +#if defined(CONFIG_P2P) && defined(CONFIG_WFD) + else if (_rtw_memcmp(pIE->data, WFD_OUI, 4)) /* WFD */ + rtw_process_wfd_ie(padapter, (u8 *)pIE, pIE->Length, __func__); +#endif + break; + +#ifdef CONFIG_WAPI_SUPPORT + case _WAPI_IE_: + pWapiIE = pIE; + break; +#endif + + case _HT_CAPABILITY_IE_: /* HT caps */ + HT_caps_handler(padapter, pIE); +#ifdef ROKU_PRIVATE + HT_caps_handler_infra_ap(padapter, pIE); +#endif /* ROKU_PRIVATE */ + break; + + case _HT_EXTRA_INFO_IE_: /* HT info */ + HT_info_handler(padapter, pIE); + break; + +#ifdef CONFIG_80211AC_VHT + case EID_VHTCapability: + VHT_caps_handler(padapter, pIE); +#ifdef ROKU_PRIVATE + VHT_caps_handler_infra_ap(padapter, pIE); +#endif /* ROKU_PRIVATE */ + break; + + case EID_VHTOperation: + VHT_operation_handler(padapter, pIE); + break; +#endif + + case _ERPINFO_IE_: + ERP_IE_handler(padapter, pIE); + break; +#ifdef CONFIG_TDLS + case _EXT_CAP_IE_: + if (check_ap_tdls_prohibited(pIE->data, pIE->Length) == _TRUE) + padapter->tdlsinfo.ap_prohibited = _TRUE; + if (check_ap_tdls_ch_switching_prohibited(pIE->data, pIE->Length) == _TRUE) + padapter->tdlsinfo.ch_switch_prohibited = _TRUE; + break; +#endif /* CONFIG_TDLS */ + +#ifdef CONFIG_RTW_80211K + case _EID_RRM_EN_CAP_IE_: + RM_IE_handler(padapter, pIE); + break; +#endif + +#ifdef ROKU_PRIVATE + /* Infra mode, used to store AP's info , Parse the supported rates from AssocRsp */ + case _SUPPORTEDRATES_IE_: + Supported_rate_infra_ap(padapter, pIE); + break; + + case _EXT_SUPPORTEDRATES_IE_: + Extended_Supported_rate_infra_ap(padapter, pIE); + break; +#endif /* ROKU_PRIVATE */ + default: + break; + } + + i += (pIE->Length + 2); + } + +#ifdef CONFIG_WAPI_SUPPORT + rtw_wapi_on_assoc_ok(padapter, pIE); +#endif + + pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE); + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + + /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */ + UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates); + +report_assoc_result: + if (res > 0) + rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len); + else + rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len); + + report_join_res(padapter, res, status); + +#if defined(CONFIG_LAYER2_ROAMING) && defined(CONFIG_RTW_80211K) + rtw_roam_nb_discover(padapter, _TRUE); +#endif + return _SUCCESS; +} + +unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame) +{ + unsigned short reason; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ + + /* check A3 */ + if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))) + return _SUCCESS; + + RTW_INFO(FUNC_ADPT_FMT" - Start to Disconnect\n", FUNC_ADPT_ARG(padapter)); + +#ifdef CONFIG_P2P + if (pwdinfo->rx_invitereq_info.scan_op_ch_only) { + _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey); + _set_timer(&pwdinfo->reset_ch_sitesurvey, 10); + } +#endif /* CONFIG_P2P */ + + reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); + +#ifdef CONFIG_AP_MODE + if (MLME_IS_AP(padapter)) { + _irqL irqL; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + /* rtw_free_stainfo(padapter, psta); */ + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + + RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM\n" + , FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe)); + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta) { + u8 updated = _FALSE; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&psta->asoc_list) == _FALSE) { + rtw_list_delete(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + updated = ap_free_sta(padapter, psta, _FALSE, reason, _TRUE); + + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL); + } + + + return _SUCCESS; + } else +#endif + if (!MLME_IS_MESH(padapter)) { + int ignore_received_deauth = 0; + + /* Commented by Albert 20130604 */ + /* Before sending the auth frame to start the STA/GC mode connection with AP/GO, */ + /* we will send the deauth first. */ + /* However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth. */ + /* Added the following code to avoid this case. */ + if ((pmlmeinfo->state & WIFI_FW_AUTH_STATE) || + (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)) { + if (reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA) + ignore_received_deauth = 1; + else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) { + /* TODO: 802.11r */ + ignore_received_deauth = 1; + } + } + + RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM, ignore=%d\n" + , FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe), ignore_received_deauth); + + if (0 == ignore_received_deauth) + receive_disconnect(padapter, get_addr2_ptr(pframe), reason, _FALSE); + } + pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE; + return _SUCCESS; + +} + +unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame) +{ + unsigned short reason; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ + + /* check A3 */ + if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))) + return _SUCCESS; + + RTW_INFO(FUNC_ADPT_FMT" - Start to Disconnect\n", FUNC_ADPT_ARG(padapter)); + +#ifdef CONFIG_P2P + if (pwdinfo->rx_invitereq_info.scan_op_ch_only) { + _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey); + _set_timer(&pwdinfo->reset_ch_sitesurvey, 10); + } +#endif /* CONFIG_P2P */ + + reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); + +#ifdef CONFIG_AP_MODE + if (MLME_IS_AP(padapter)) { + _irqL irqL; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + /* rtw_free_stainfo(padapter, psta); */ + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */ + + RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM\n" + , FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe)); + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta) { + u8 updated = _FALSE; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + if (rtw_is_list_empty(&psta->asoc_list) == _FALSE) { + rtw_list_delete(&psta->asoc_list); + pstapriv->asoc_list_cnt--; + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (psta->tbtx_enable) + pstapriv->tbtx_asoc_list_cnt--; + #endif + updated = ap_free_sta(padapter, psta, _FALSE, reason, _TRUE); + + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL); + } + + return _SUCCESS; + } else +#endif + if (!MLME_IS_MESH(padapter)) { + RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM\n" + , FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe)); + + #ifdef CONFIG_RTW_WNM + if (rtw_wnm_try_btm_roam_imnt(padapter) > 0) + #endif + receive_disconnect(padapter, get_addr2_ptr(pframe), reason, _FALSE); + } + pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE; + return _SUCCESS; + +} + +unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame) +{ + RTW_INFO("%s\n", __FUNCTION__); + return _SUCCESS; +} + +unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len) +{ + unsigned int ret = _FAIL; + struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(mlmeext->mlmext_info); + + if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) { + ret = _SUCCESS; + goto exit; + } + + if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) { + + int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1; + int ch_offset = -1; + u8 bwmode; + struct ieee80211_info_element *ie; + + RTW_INFO(FUNC_NDEV_FMT" from "MAC_FMT"\n", + FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->cmn.mac_addr)); + + for_each_ie(ie, ies, ies_len) { + if (ie->id == WLAN_EID_CHANNEL_SWITCH) { + ch_switch_mode = ie->data[0]; + ch = ie->data[1]; + ch_switch_cnt = ie->data[2]; + RTW_INFO("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n", + ch_switch_mode, ch, ch_switch_cnt); + } else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) { + ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]); + RTW_INFO("ch_offset:%d\n", ch_offset); + } + } + + if (ch == -1) + return _SUCCESS; + + if (ch_offset == -1) + bwmode = mlmeext->cur_bwmode; + else + bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ? + CHANNEL_WIDTH_20 : CHANNEL_WIDTH_40; + + ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset; + + /* todo: + * 1. the decision of channel switching + * 2. things after channel switching + */ + + ret = rtw_set_chbw_cmd(padapter, ch, bwmode, ch_offset, 0); + } + +exit: + return ret; +} + +unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint frame_len = precv_frame->u.hdr.len; + u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 category; + u8 action; + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + + if (!psta) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT) + goto exit; + + action = frame_body[1]; + + RTW_INFO(FUNC_ADPT_FMT" action:%u\n", FUNC_ADPT_ARG(padapter), action); + + switch (action) { + case RTW_WLAN_ACTION_SPCT_MSR_REQ: + case RTW_WLAN_ACTION_SPCT_MSR_RPRT: + case RTW_WLAN_ACTION_SPCT_TPC_REQ: + case RTW_WLAN_ACTION_SPCT_TPC_RPRT: + break; + case RTW_WLAN_ACTION_SPCT_CHL_SWITCH: +#ifdef CONFIG_SPCT_CH_SWITCH + ret = on_action_spct_ch_switch(padapter, psta + , frame_body + 2, frame_len - (frame_body - pframe) - 2); +#elif CONFIG_DFS + if (MLME_IS_STA(padapter) && MLME_IS_ASOC(padapter)) { + process_csa_ie(padapter + , frame_body + 2, frame_len - (frame_body - pframe) - 2); + } +#endif + break; + default: + break; + } + +exit: + return ret; +} + +unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame) +{ + return _SUCCESS; +} + +unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame) +{ + return _SUCCESS; +} + +#ifdef CONFIG_RTW_WNM +unsigned int on_action_wnm(_adapter *adapter, union recv_frame *rframe) +{ + unsigned int ret = _FAIL; + struct sta_info *sta = NULL; + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + struct sta_priv *stapriv = &(adapter->stapriv); + u8 *frame = rframe->u.hdr.rx_data; + u32 frame_len = rframe->u.hdr.len; + u8 *frame_body = (u8 *)(frame + sizeof(struct rtw_ieee80211_hdr_3addr)); + u32 frame_body_len = frame_len - sizeof(struct rtw_ieee80211_hdr_3addr); + u8 category, action; + int cnt = 0; + char msg[16]; + + sta = rtw_get_stainfo(stapriv, get_addr2_ptr(frame)); + if (!sta) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_WNM) + goto exit; + + action = frame_body[1]; + + switch (action) { +#ifdef CONFIG_RTW_80211R + case RTW_WLAN_ACTION_WNM_BTM_REQ: + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) { + RTW_INFO("WNM: BSS Transition Management Request recv.\n"); + rtw_wnm_process_btm_req(adapter, frame_body, frame_body_len); + } + ret = _SUCCESS; + break; +#endif + case RTW_WLAN_ACTION_WNM_BTM_RSP: + if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE) && + (pmlmepriv->nb_info.features & RTW_WNM_FEATURE_BTM_REQ_EN)) { + struct btm_rsp_hdr rsp; + u32 sz; + + RTW_INFO("WNM: BSS Transition Management Response recv.\n"); + sz = rtw_wnm_btm_rsp_candidates_sz_get(adapter, + frame_body, frame_body_len); + _rtw_memset(&rsp, 0, sizeof(rsp)); + + if (sz > 0) + rsp.pcandidates = rtw_zmalloc(sz); + + rtw_wnm_process_btm_rsp(adapter, frame_body, frame_body_len, &rsp); + /* TODO : handle candidates info in rsp.pcandidates for upper-layer services */ + #ifdef CONFIG_PLATFORM_CMAP_INTFS + cmap_intfs_nl_btm_resp_event(adapter, sta->cmn.mac_addr, + adapter_mac_addr(adapter), + rsp.status, rsp.bssid, + rsp.pcandidates, + rsp.candidates_num); + #endif + if (0 && rsp.pcandidates && (rsp.candidates_num > 0)) + RTW_INFO_DUMP("pcandidates : ", rsp.pcandidates, sz); + + if ((sz > 0) && (rsp.pcandidates != NULL)) + rtw_mfree(rsp.pcandidates, sz); + } + /* fall through */ + default: + #ifdef CONFIG_IOCTL_CFG80211 + cnt += sprintf((msg + cnt), "ACT_WNM %u", action); + rtw_cfg80211_rx_action(adapter, rframe, msg); + #endif + ret = _SUCCESS; + break; + } + +exit: + return ret; +} +#endif /* CONFIG_RTW_WNM */ + +/** + * rtw_rx_ampdu_size - Get the target RX AMPDU buffer size for the specific @adapter + * @adapter: the adapter to get target RX AMPDU buffer size + * + * Returns: the target RX AMPDU buffer size + */ +u8 rtw_rx_ampdu_size(_adapter *adapter) +{ + u8 size; + HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor; + +#ifdef CONFIG_BT_COEXIST + if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(adapter) == _TRUE) { + size = rtw_btcoex_GetAMPDUSize(adapter); + goto exit; + } +#endif + + /* for scan */ + if (!mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_DISABLE) + && !mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_COMPLETE) + && adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_size != RX_AMPDU_SIZE_INVALID + ) { + size = adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_size; + goto exit; + } + + /* default value based on max_rx_ampdu_factor */ + if (adapter->driver_rx_ampdu_factor != 0xFF) + max_rx_ampdu_factor = (HT_CAP_AMPDU_FACTOR)adapter->driver_rx_ampdu_factor; + else + rtw_hal_get_def_var(adapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor); + + /* In Maximum A-MPDU Length Exponent subfield of A-MPDU Parameters field of HT Capabilities element, + the unit of max_rx_ampdu_factor are octets. 8K, 16K, 32K, 64K is right. + But the buffer size subfield of Block Ack Parameter Set field in ADDBA action frame indicates + the number of buffers available for this particular TID. Each buffer is equal to max. size of + MSDU or AMSDU. + The size variable means how many MSDUs or AMSDUs, it's not Kbytes. + */ + if (MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor) + size = 64; + else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor) + size = 32; + else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor) + size = 16; + else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor) + size = 8; + else + size = 64; + +exit: + + if (size > 127) + size = 127; + + return size; +} + +/** + * rtw_rx_ampdu_is_accept - Get the permission if RX AMPDU should be set up for the specific @adapter + * @adapter: the adapter to get the permission if RX AMPDU should be set up + * + * Returns: accept or not + */ +bool rtw_rx_ampdu_is_accept(_adapter *adapter) +{ + bool accept; + + if (adapter->fix_rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID) { + accept = adapter->fix_rx_ampdu_accept; + goto exit; + } + +#ifdef CONFIG_BT_COEXIST + if (rtw_btcoex_IsBTCoexRejectAMPDU(adapter) == _TRUE) { + accept = _FALSE; + goto exit; + } +#endif + + /* for scan */ + if (!mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_DISABLE) + && !mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_COMPLETE) + && adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID + ) { + accept = adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept; + goto exit; + } + + /* default value for other cases */ + accept = adapter->mlmeextpriv.mlmext_info.bAcceptAddbaReq; + +exit: + return accept; +} + +/** + * rtw_rx_ampdu_set_size - Set the target RX AMPDU buffer size for the specific @adapter and specific @reason + * @adapter: the adapter to set target RX AMPDU buffer size + * @size: the target RX AMPDU buffer size to set + * @reason: reason for the target RX AMPDU buffer size setting + * + * Returns: whether the target RX AMPDU buffer size is changed + */ +bool rtw_rx_ampdu_set_size(_adapter *adapter, u8 size, u8 reason) +{ + bool is_adj = _FALSE; + struct mlme_ext_priv *mlmeext; + struct mlme_ext_info *mlmeinfo; + + mlmeext = &adapter->mlmeextpriv; + mlmeinfo = &mlmeext->mlmext_info; + + if (reason == RX_AMPDU_DRV_FIXED) { + if (adapter->fix_rx_ampdu_size != size) { + adapter->fix_rx_ampdu_size = size; + is_adj = _TRUE; + RTW_INFO(FUNC_ADPT_FMT" fix_rx_ampdu_size:%u\n", FUNC_ADPT_ARG(adapter), size); + } + } else if (reason == RX_AMPDU_DRV_SCAN) { + struct ss_res *ss = &adapter->mlmeextpriv.sitesurvey_res; + + if (ss->rx_ampdu_size != size) { + ss->rx_ampdu_size = size; + is_adj = _TRUE; + RTW_INFO(FUNC_ADPT_FMT" ss.rx_ampdu_size:%u\n", FUNC_ADPT_ARG(adapter), size); + } + } + + return is_adj; +} + +/** + * rtw_rx_ampdu_set_accept - Set the permission if RX AMPDU should be set up for the specific @adapter and specific @reason + * @adapter: the adapter to set if RX AMPDU should be set up + * @accept: if RX AMPDU should be set up + * @reason: reason for the permission if RX AMPDU should be set up + * + * Returns: whether the permission if RX AMPDU should be set up is changed + */ +bool rtw_rx_ampdu_set_accept(_adapter *adapter, u8 accept, u8 reason) +{ + bool is_adj = _FALSE; + struct mlme_ext_priv *mlmeext; + struct mlme_ext_info *mlmeinfo; + + mlmeext = &adapter->mlmeextpriv; + mlmeinfo = &mlmeext->mlmext_info; + + if (reason == RX_AMPDU_DRV_FIXED) { + if (adapter->fix_rx_ampdu_accept != accept) { + adapter->fix_rx_ampdu_accept = accept; + is_adj = _TRUE; + RTW_INFO(FUNC_ADPT_FMT" fix_rx_ampdu_accept:%u\n", FUNC_ADPT_ARG(adapter), accept); + } + } else if (reason == RX_AMPDU_DRV_SCAN) { + if (adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept != accept) { + adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept = accept; + is_adj = _TRUE; + RTW_INFO(FUNC_ADPT_FMT" ss.rx_ampdu_accept:%u\n", FUNC_ADPT_ARG(adapter), accept); + } + } + + return is_adj; +} + +/** + * rx_ampdu_apply_sta_tid - Apply RX AMPDU setting to the specific @sta and @tid + * @adapter: the adapter to which @sta belongs + * @sta: the sta to be checked + * @tid: the tid to be checked + * @accept: the target permission if RX AMPDU should be set up + * @size: the target RX AMPDU buffer size + * + * Returns: + * 0: no canceled + * 1: canceled by no permission + * 2: canceled by different buffer size + * 3: canceled by potential mismatched status + * + * Blocking function, may sleep + */ +u8 rx_ampdu_apply_sta_tid(_adapter *adapter, struct sta_info *sta, u8 tid, u8 accept, u8 size) +{ + u8 ret = 0; + struct recv_reorder_ctrl *reorder_ctl = &sta->recvreorder_ctrl[tid]; + + if (reorder_ctl->enable == _FALSE) { + if (reorder_ctl->ampdu_size != RX_AMPDU_SIZE_INVALID) { + send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 1); + ret = 3; + } + goto exit; + } + + if (accept == _FALSE) { + send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 0); + ret = 1; + } else if (reorder_ctl->ampdu_size != size) { + send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 0); + ret = 2; + } + +exit: + return ret; +} + +u8 rx_ampdu_size_sta_limit(_adapter *adapter, struct sta_info *sta) +{ + u8 sz_limit = 0xFF; + +#ifdef CONFIG_80211N_HT + struct registry_priv *regsty = adapter_to_regsty(adapter); + struct mlme_priv *mlme = &adapter->mlmepriv; + struct mlme_ext_info *mlmeinfo = &adapter->mlmeextpriv.mlmext_info; + s8 nss = -1; + u8 bw = rtw_min(sta->cmn.bw_mode, adapter->mlmeextpriv.cur_bwmode); + + #ifdef CONFIG_80211AC_VHT + if (is_supported_vht(sta->wireless_mode)) { + nss = rtw_min(rtw_vht_mcsmap_to_nss(mlme->vhtpriv.vht_mcs_map) + , rtw_vht_mcsmap_to_nss(sta->vhtpriv.vht_mcs_map)); + } else + #endif + if (is_supported_ht(sta->wireless_mode)) { + nss = rtw_min(rtw_ht_mcsset_to_nss(mlmeinfo->HT_caps.u.HT_cap_element.MCS_rate) + , rtw_ht_mcsset_to_nss(sta->htpriv.ht_cap.supp_mcs_set)); + } + + if (nss >= 1) + sz_limit = regsty->rx_ampdu_sz_limit_by_nss_bw[nss - 1][bw]; +#endif /* CONFIG_80211N_HT */ + + return sz_limit; +} + +/** + * rx_ampdu_apply_sta - Apply RX AMPDU setting to the specific @sta + * @adapter: the adapter to which @sta belongs + * @sta: the sta to be checked + * @accept: the target permission if RX AMPDU should be set up + * @size: the target RX AMPDU buffer size + * + * Returns: number of the RX AMPDU assciation canceled for applying current target setting + * + * Blocking function, may sleep + */ +u8 rx_ampdu_apply_sta(_adapter *adapter, struct sta_info *sta, u8 accept, u8 size) +{ + u8 change_cnt = 0; + int i; + + for (i = 0; i < TID_NUM; i++) { + if (rx_ampdu_apply_sta_tid(adapter, sta, i, accept, size) != 0) + change_cnt++; + } + + return change_cnt; +} + +/** + * rtw_rx_ampdu_apply - Apply the current target RX AMPDU setting for the specific @adapter + * @adapter: the adapter to be applied + * + * Returns: number of the RX AMPDU assciation canceled for applying current target setting + */ +u16 rtw_rx_ampdu_apply(_adapter *adapter) +{ + u16 adj_cnt = 0; + struct sta_info *sta; + u8 accept = rtw_rx_ampdu_is_accept(adapter); + u8 size; + + if (adapter->fix_rx_ampdu_size != RX_AMPDU_SIZE_INVALID) + size = adapter->fix_rx_ampdu_size; + else + size = rtw_rx_ampdu_size(adapter); + + if (MLME_IS_STA(adapter)) { + sta = rtw_get_stainfo(&adapter->stapriv, get_bssid(&adapter->mlmepriv)); + if (sta) { + u8 sta_size = size; + + if (adapter->fix_rx_ampdu_size == RX_AMPDU_SIZE_INVALID) + sta_size = rtw_min(size, rx_ampdu_size_sta_limit(adapter, sta)); + adj_cnt += rx_ampdu_apply_sta(adapter, sta, accept, sta_size); + } + /* TODO: TDLS peer */ +#ifdef CONFIG_AP_MODE + } else if (MLME_IS_AP(adapter) || MLME_IS_MESH(adapter)) { + _irqL irqL; + _list *phead, *plist; + u8 peer_num = 0; + char peers[NUM_STA]; + struct sta_priv *pstapriv = &adapter->stapriv; + int i; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + int stainfo_offset; + + sta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + plist = get_next(plist); + + stainfo_offset = rtw_stainfo_offset(pstapriv, sta); + if (stainfo_offset_valid(stainfo_offset)) + peers[peer_num++] = stainfo_offset; + } + + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + for (i = 0; i < peer_num; i++) { + sta = rtw_get_stainfo_by_offset(pstapriv, peers[i]); + if (sta) { + u8 sta_size = size; + + if (adapter->fix_rx_ampdu_size == RX_AMPDU_SIZE_INVALID) + sta_size = rtw_min(size, rx_ampdu_size_sta_limit(adapter, sta)); + adj_cnt += rx_ampdu_apply_sta(adapter, sta, accept, sta_size); + } + } +#endif /* CONFIG_AP_MODE */ + } + + /* TODO: ADHOC */ + + return adj_cnt; +} + +unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame) +{ + u8 *addr; + struct sta_info *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + unsigned char *frame_body; + unsigned char category, action; + unsigned short tid, status, reason_code = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_priv *pstapriv = &padapter->stapriv; + struct registry_priv *pregpriv = &padapter->registrypriv; + +#ifdef CONFIG_80211N_HT + + RTW_INFO("%s\n", __FUNCTION__); + + /* check RA matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN)) + return _SUCCESS; + +#if 0 + /* check A1 matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN)) + return _SUCCESS; +#endif + + if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE) + if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) + return _SUCCESS; + + addr = get_addr2_ptr(pframe); + psta = rtw_get_stainfo(pstapriv, addr); + + if (psta == NULL) + return _SUCCESS; + + frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + + category = frame_body[0]; + if (category == RTW_WLAN_CATEGORY_BACK) { /* representing Block Ack */ +#ifdef CONFIG_TDLS + if ((psta->tdls_sta_state & TDLS_LINKED_STATE) && + (psta->htpriv.ht_option == _TRUE) && + (psta->htpriv.ampdu_enable == _TRUE)) + RTW_INFO("Recv [%s] from direc link\n", __FUNCTION__); + else +#endif /* CONFIG_TDLS */ + if (!pmlmeinfo->HT_enable) + return _SUCCESS; + + action = frame_body[1]; + RTW_INFO("%s, action=%d\n", __FUNCTION__, action); + switch (action) { + case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */ + + _rtw_memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request)); + /* process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe)); */ + process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr); + + break; + + case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */ + + /* status = frame_body[3] | (frame_body[4] << 8); */ /* endian issue */ + status = RTW_GET_LE16(&frame_body[3]); + tid = ((frame_body[5] >> 2) & 0x7); + if (status == 0) { + /* successful */ + RTW_INFO("agg_enable for TID=%d\n", tid); + psta->htpriv.agg_enable_bitmap |= 1 << tid; + psta->htpriv.candidate_tid_bitmap &= ~BIT(tid); + /* amsdu in ampdu */ + if (pregpriv->tx_ampdu_amsdu == 0) + psta->htpriv.tx_amsdu_enable = _FALSE; + else if (pregpriv->tx_ampdu_amsdu == 1) + psta->htpriv.tx_amsdu_enable = _TRUE; + else { + if (frame_body[5] & 1) + psta->htpriv.tx_amsdu_enable = _TRUE; + } + } else + psta->htpriv.agg_enable_bitmap &= ~BIT(tid); + + #ifdef CONFIG_AP_MODE + if (psta->state & WIFI_STA_ALIVE_CHK_STATE) { + RTW_INFO("%s alive check - rx ADDBA response\n", __func__); + psta->htpriv.agg_enable_bitmap &= ~BIT(tid); + psta->expire_to = pstapriv->expire_to; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + } + #endif + + /* RTW_INFO("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap); */ + break; + + case RTW_WLAN_ACTION_DELBA: /* DELBA */ + if ((frame_body[3] & BIT(3)) == 0) { + psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf)); + psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf)); + + /* reason_code = frame_body[4] | (frame_body[5] << 8); */ + reason_code = RTW_GET_LE16(&frame_body[4]); + } else if ((frame_body[3] & BIT(3)) == BIT(3)) { + tid = (frame_body[3] >> 4) & 0x0F; + + preorder_ctrl = &psta->recvreorder_ctrl[tid]; + preorder_ctrl->enable = _FALSE; + preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID; + } + + RTW_INFO("%s(): DELBA: %x(%x)\n", __FUNCTION__, pmlmeinfo->agg_enable_bitmap, reason_code); + /* todo: how to notify the host while receiving DELETE BA */ + break; + + default: + break; + } + } +#endif /* CONFIG_80211N_HT */ + return _SUCCESS; +} + +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE +u32 rtw_build_vendor_ie(_adapter *padapter , unsigned char **pframe , u8 mgmt_frame_tyte) +{ + int vendor_ie_num = 0; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u32 len = 0; + + for (vendor_ie_num = 0 ; vendor_ie_num < WLAN_MAX_VENDOR_IE_NUM ; vendor_ie_num++) { + if (pmlmepriv->vendor_ielen[vendor_ie_num] > 0 && pmlmepriv->vendor_ie_mask[vendor_ie_num] & mgmt_frame_tyte) { + _rtw_memcpy(*pframe , pmlmepriv->vendor_ie[vendor_ie_num] , pmlmepriv->vendor_ielen[vendor_ie_num]); + *pframe += pmlmepriv->vendor_ielen[vendor_ie_num]; + len += pmlmepriv->vendor_ielen[vendor_ie_num]; + } + } + + return len; +} +#endif + +#ifdef CONFIG_P2P +int get_reg_classes_full_count(struct p2p_channels *channel_list) +{ + int cnt = 0; + int i; + + for (i = 0; i < channel_list->reg_classes; i++) + cnt += channel_list->reg_class[i].channels; + + return cnt; +} + +void issue_p2p_GO_request(_adapter *padapter, u8 *raddr) +{ + struct p2p_channels *ch_list = &(adapter_to_rfctl(padapter)->channel_list); + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_GO_NEGO_REQ; + u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 }; + u8 wpsielen = 0, p2pielen = 0; + u16 len_channellist_attr = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + RTW_INFO("[%s] In\n", __FUNCTION__); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pwdinfo->negotiation_dialog_token = 1; /* Initialize the dialog value */ + pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen)); + + + + /* WPS Section */ + wpsielen = 0; + /* WPS OUI */ + *(u32 *)(wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* Device Password ID */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + + if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC); + else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN) + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); + else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC); + + wpsielen += 2; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + + + /* P2P IE Section. */ + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20110306 */ + /* According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */ + /* 1. P2P Capability */ + /* 2. Group Owner Intent */ + /* 3. Configuration Timeout */ + /* 4. Listen Channel */ + /* 5. Extended Listen Timing */ + /* 6. Intended P2P Interface Address */ + /* 7. Channel List */ + /* 8. P2P Device Info */ + /* 9. Operating Channel */ + + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP; + else + p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN; + + + /* Group Owner Intent */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GO_INTENT; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + /* Todo the tie breaker bit. */ + p2pie[p2pielen++] = ((pwdinfo->intent << 1) & 0xFE); + + /* Configuration Timeout */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */ + + + /* Listen Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + p2pie[p2pielen++] = 0x51; /* Copy from SD7 */ + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->listen_channel; /* listening channel number */ + + + /* Extended Listen Timing ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + + /* Intended P2P Interface Address */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INTENDED_IF_ADDR; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + + /* Channel List */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CH_LIST; + + /* Length: */ + /* Country String(3) */ + /* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */ + /* + number of channels in all classes */ + len_channellist_attr = 3 + + (1 + 1) * (u16)(ch_list->reg_classes) + + get_reg_classes_full_count(ch_list); + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1); + else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); +#else + + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); + +#endif + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Channel Entry List */ + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + u8 union_ch = rtw_mi_get_union_chan(padapter); + + /* Operating Class */ + if (union_ch > 14) { + if (union_ch >= 149) + p2pie[p2pielen++] = 0x7c; + else + p2pie[p2pielen++] = 0x73; + } else + p2pie[p2pielen++] = 0x51; + + + /* Number of Channels */ + /* Just support 1 channel and this channel is AP's channel */ + p2pie[p2pielen++] = 1; + + /* Channel List */ + p2pie[p2pielen++] = union_ch; + } else +#endif /* CONFIG_CONCURRENT_MODE */ + { + int i, j; + for (j = 0; j < ch_list->reg_classes; j++) { + /* Operating Class */ + p2pie[p2pielen++] = ch_list->reg_class[j].reg_class; + + /* Number of Channels */ + p2pie[p2pielen++] = ch_list->reg_class[j].channels; + + /* Channel List */ + for (i = 0; i < ch_list->reg_class[j].channels; i++) + p2pie[p2pielen++] = ch_list->reg_class[j].channel[i]; + } + } + + /* Device Info */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + + /* Operating Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + if (pwdinfo->operating_channel <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */ + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_WFD + wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + + +void issue_p2p_GO_response(_adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result) +{ + struct p2p_channels *ch_list = &(adapter_to_rfctl(padapter)->channel_list); + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_GO_NEGO_RESP; + u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 }; + u8 p2pielen = 0; + uint wpsielen = 0; + u16 wps_devicepassword_id = 0x0000; + uint wps_devicepassword_id_len = 0; + u16 len_channellist_attr = 0; + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + RTW_INFO("[%s] In, result = %d\n", __FUNCTION__, result); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pwdinfo->negotiation_dialog_token = frame_body[7]; /* The Dialog Token of provisioning discovery request frame. */ + pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen)); + + /* Commented by Albert 20110328 */ + /* Try to get the device password ID from the WPS IE of group negotiation request frame */ + /* WiFi Direct test plan 5.1.15 */ + rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen); + rtw_get_wps_attr_content(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8 *) &wps_devicepassword_id, &wps_devicepassword_id_len); + wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id); + + _rtw_memset(wpsie, 0x00, 255); + wpsielen = 0; + + /* WPS Section */ + wpsielen = 0; + /* WPS OUI */ + *(u32 *)(wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* Device Password ID */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + if (wps_devicepassword_id == WPS_DPID_USER_SPEC) + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); + else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC); + else + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC); + wpsielen += 2; + + /* Commented by Kurt 20120113 */ + /* If some device wants to do p2p handshake without sending prov_disc_req */ + /* We have to get peer_req_cm from here. */ + if (_rtw_memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) { + if (wps_devicepassword_id == WPS_DPID_USER_SPEC) + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3); + else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3); + else + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3); + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + + + /* P2P IE Section. */ + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20100908 */ + /* According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes */ + /* 1. Status */ + /* 2. P2P Capability */ + /* 3. Group Owner Intent */ + /* 4. Configuration Timeout */ + /* 5. Operating Channel */ + /* 6. Intended P2P Interface Address */ + /* 7. Channel List */ + /* 8. Device Info */ + /* 9. Group ID ( Only GO ) */ + + + /* ToDo: */ + + /* P2P Status */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_STATUS; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = result; + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) { + /* Commented by Albert 2011/03/08 */ + /* According to the P2P specification */ + /* if the sending device will be client, the P2P Capability should be reserved of group negotation response frame */ + p2pie[p2pielen++] = 0; + } else { + /* Be group owner or meet the error case */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + } + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP; + else + p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN; + + /* Group Owner Intent */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GO_INTENT; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + if (pwdinfo->peer_intent & 0x01) { + /* Peer's tie breaker bit is 1, our tie breaker bit should be 0 */ + p2pie[p2pielen++] = (pwdinfo->intent << 1); + } else { + /* Peer's tie breaker bit is 0, our tie breaker bit should be 1 */ + p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0)); + } + + + /* Configuration Timeout */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */ + + /* Operating Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + if (pwdinfo->operating_channel <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */ + + /* Intended P2P Interface Address */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INTENDED_IF_ADDR; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Channel List */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CH_LIST; + + /* Country String(3) */ + /* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */ + /* + number of channels in all classes */ + len_channellist_attr = 3 + + (1 + 1) * (u16)ch_list->reg_classes + + get_reg_classes_full_count(ch_list); + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1); + else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); +#else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); + +#endif + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Channel Entry List */ + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + + u8 union_chan = rtw_mi_get_union_chan(padapter); + + /*Operating Class*/ + if (union_chan > 14) { + if (union_chan >= 149) + p2pie[p2pielen++] = 0x7c; + else + p2pie[p2pielen++] = 0x73; + + } else + p2pie[p2pielen++] = 0x51; + + /* Number of Channels + Just support 1 channel and this channel is AP's channel*/ + p2pie[p2pielen++] = 1; + + /*Channel List*/ + p2pie[p2pielen++] = union_chan; + } else +#endif /* CONFIG_CONCURRENT_MODE */ + { + int i, j; + for (j = 0; j < ch_list->reg_classes; j++) { + /* Operating Class */ + p2pie[p2pielen++] = ch_list->reg_class[j].reg_class; + + /* Number of Channels */ + p2pie[p2pielen++] = ch_list->reg_class[j].channels; + + /* Channel List */ + for (i = 0; i < ch_list->reg_class[j].channels; i++) + p2pie[p2pielen++] = ch_list->reg_class[j].channel[i]; + } + } + + /* Device Info */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* Group ID Attribute */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_ID; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen); + p2pielen += 2; + + /* Value: */ + /* p2P Device Address */ + _rtw_memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* SSID */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen); + p2pielen += pwdinfo->nego_ssidlen; + + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_WFD + wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +void issue_p2p_GO_confirm(_adapter *padapter, u8 *raddr, u8 result) +{ + + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_GO_NEGO_CONF; + u8 p2pie[255] = { 0x00 }; + u8 p2pielen = 0; + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + RTW_INFO("[%s] In\n", __FUNCTION__); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen)); + + + + /* P2P IE Section. */ + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20110306 */ + /* According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes */ + /* 1. Status */ + /* 2. P2P Capability */ + /* 3. Operating Channel */ + /* 4. Channel List */ + /* 5. Group ID ( if this WiFi is GO ) */ + + /* P2P Status */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_STATUS; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = result; + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP; + else + p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN; + + + /* Operating Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) { + if (pwdinfo->peer_operating_ch <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->peer_operating_ch >= 36) && (pwdinfo->peer_operating_ch <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + p2pie[p2pielen++] = pwdinfo->peer_operating_ch; + } else { + if (pwdinfo->operating_channel <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; /* Use the listen channel as the operating channel */ + } + + + /* Channel List */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CH_LIST; + + *(u16 *)(p2pie + p2pielen) = 6; + p2pielen += 2; + + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Value: */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) { + if (pwdinfo->peer_operating_ch <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->peer_operating_ch >= 36) && (pwdinfo->peer_operating_ch <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + p2pie[p2pielen++] = 1; + p2pie[p2pielen++] = pwdinfo->peer_operating_ch; + } else { + if (pwdinfo->operating_channel <= 14) { + /* Operating Class */ + p2pie[p2pielen++] = 0x51; + } else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) { + /* Operating Class */ + p2pie[p2pielen++] = 0x73; + } else { + /* Operating Class */ + p2pie[p2pielen++] = 0x7c; + } + + /* Channel Number */ + p2pie[p2pielen++] = 1; + p2pie[p2pielen++] = pwdinfo->operating_channel; /* Use the listen channel as the operating channel */ + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* Group ID Attribute */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_ID; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen); + p2pielen += 2; + + /* Value: */ + /* p2P Device Address */ + _rtw_memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* SSID */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen); + p2pielen += pwdinfo->nego_ssidlen; + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_WFD + wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +void issue_p2p_invitation_request(_adapter *padapter, u8 *raddr) +{ + struct p2p_channels *ch_list = &(adapter_to_rfctl(padapter)->channel_list); + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_INVIT_REQ; + u8 p2pie[255] = { 0x00 }; + u8 p2pielen = 0; + u8 dialogToken = 3; + u16 len_channellist_attr = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, raddr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + /* P2P IE Section. */ + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20101011 */ + /* According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */ + /* 1. Configuration Timeout */ + /* 2. Invitation Flags */ + /* 3. Operating Channel ( Only GO ) */ + /* 4. P2P Group BSSID ( Should be included if I am the GO ) */ + /* 5. Channel List */ + /* 6. P2P Group ID */ + /* 7. P2P Device Info */ + + /* Configuration Timeout */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */ + + /* Invitation Flags */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT; + + + /* Operating Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + if (pwdinfo->invitereq_info.operating_ch <= 14) + p2pie[p2pielen++] = 0x51; + else if ((pwdinfo->invitereq_info.operating_ch >= 36) && (pwdinfo->invitereq_info.operating_ch <= 48)) + p2pie[p2pielen++] = 0x73; + else + p2pie[p2pielen++] = 0x7c; + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch; /* operating channel number */ + + if (_rtw_memcmp(adapter_mac_addr(padapter), pwdinfo->invitereq_info.go_bssid, ETH_ALEN)) { + /* P2P Group BSSID */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address for GO */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN); + p2pielen += ETH_ALEN; + } + + /* Channel List */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CH_LIST; + + + /* Length: */ + /* Country String(3) */ + /* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */ + /* + number of channels in all classes */ + len_channellist_attr = 3 + + (1 + 1) * (u16)ch_list->reg_classes + + get_reg_classes_full_count(ch_list); + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1); + else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); +#else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); +#endif + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Channel Entry List */ +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + u8 union_ch = rtw_mi_get_union_chan(padapter); + + /* Operating Class */ + if (union_ch > 14) { + if (union_ch >= 149) + p2pie[p2pielen++] = 0x7c; + else + p2pie[p2pielen++] = 0x73; + } else + p2pie[p2pielen++] = 0x51; + + + /* Number of Channels */ + /* Just support 1 channel and this channel is AP's channel */ + p2pie[p2pielen++] = 1; + + /* Channel List */ + p2pie[p2pielen++] = union_ch; + } else +#endif /* CONFIG_CONCURRENT_MODE */ + { + int i, j; + for (j = 0; j < ch_list->reg_classes; j++) { + /* Operating Class */ + p2pie[p2pielen++] = ch_list->reg_class[j].reg_class; + + /* Number of Channels */ + p2pie[p2pielen++] = ch_list->reg_class[j].channels; + + /* Channel List */ + for (i = 0; i < ch_list->reg_class[j].channels; i++) + p2pie[p2pielen++] = ch_list->reg_class[j].channel[i]; + } + } + + + /* P2P Group ID */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_ID; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address for GO */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* SSID */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen); + p2pielen += pwdinfo->invitereq_info.ssidlen; + + + /* Device Info */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_WFD + wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +void issue_p2p_invitation_response(_adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code) +{ + struct p2p_channels *ch_list = &(adapter_to_rfctl(padapter)->channel_list); + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_INVIT_RESP; + u8 p2pie[255] = { 0x00 }; + u8 p2pielen = 0; + u16 len_channellist_attr = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, raddr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + /* P2P IE Section. */ + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20101005 */ + /* According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes */ + /* 1. Status */ + /* 2. Configuration Timeout */ + /* 3. Operating Channel ( Only GO ) */ + /* 4. P2P Group BSSID ( Only GO ) */ + /* 5. Channel List */ + + /* P2P Status */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_STATUS; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001); + p2pielen += 2; + + /* Value: */ + /* When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */ + /* Sent the event receiving the P2P Invitation Req frame to DMP UI. */ + /* DMP had to compare the MAC address to find out the profile. */ + /* So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */ + /* If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req */ + /* to NB to rebuild the persistent group. */ + p2pie[p2pielen++] = status_code; + + /* Configuration Timeout */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P GO */ + p2pie[p2pielen++] = 200; /* 2 seconds needed to be the P2P Client */ + + if (status_code == P2P_STATUS_SUCCESS) { + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO */ + /* In this case, the P2P Invitation response frame should carry the two more P2P attributes. */ + /* First one is operating channel attribute. */ + /* Second one is P2P Group BSSID attribute. */ + + /* Operating Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + p2pie[p2pielen++] = 0x51; /* Copy from SD7 */ + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */ + + + /* P2P Group BSSID */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address for GO */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + } + + /* Channel List */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CH_LIST; + + /* Length: */ + /* Country String(3) */ + /* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */ + /* + number of channels in all classes */ + len_channellist_attr = 3 + + (1 + 1) * (u16)ch_list->reg_classes + + get_reg_classes_full_count(ch_list); + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1); + else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); +#else + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr); +#endif + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Channel Entry List */ +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + u8 union_ch = rtw_mi_get_union_chan(padapter); + + /* Operating Class */ + if (union_ch > 14) { + if (union_ch >= 149) + p2pie[p2pielen++] = 0x7c; + else + p2pie[p2pielen++] = 0x73; + } else + p2pie[p2pielen++] = 0x51; + + + /* Number of Channels */ + /* Just support 1 channel and this channel is AP's channel */ + p2pie[p2pielen++] = 1; + + /* Channel List */ + p2pie[p2pielen++] = union_ch; + } else +#endif /* CONFIG_CONCURRENT_MODE */ + { + int i, j; + for (j = 0; j < ch_list->reg_classes; j++) { + /* Operating Class */ + p2pie[p2pielen++] = ch_list->reg_class[j].reg_class; + + /* Number of Channels */ + p2pie[p2pielen++] = ch_list->reg_class[j].channels; + + /* Channel List */ + for (i = 0; i < ch_list->reg_class[j].channels; i++) + p2pie[p2pielen++] = ch_list->reg_class[j].channel[i]; + } + } + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + +#ifdef CONFIG_WFD + wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +void issue_p2p_provision_request(_adapter *padapter, u8 *pssid, u8 ussidlen, u8 *pdev_raddr) +{ + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u8 dialogToken = 1; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PROVISION_DISC_REQ; + u8 wpsie[100] = { 0x00 }; + u8 wpsielen = 0; + u32 p2pielen = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + RTW_INFO("[%s] In\n", __FUNCTION__); + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + p2pielen = build_prov_disc_request_p2p_ie(pwdinfo, pframe, pssid, ussidlen, pdev_raddr); + + pframe += p2pielen; + pattrib->pktlen += p2pielen; + + wpsielen = 0; + /* WPS OUI */ + *(u32 *)(wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* Config Method */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request); + wpsielen += 2; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + + +#ifdef CONFIG_WFD + wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + + +u8 is_matched_in_profilelist(u8 *peermacaddr, struct profile_info *profileinfo) +{ + u8 i, match_result = 0; + + RTW_INFO("[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__, + peermacaddr[0], peermacaddr[1], peermacaddr[2], peermacaddr[3], peermacaddr[4], peermacaddr[5]); + + for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) { + RTW_INFO("[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__, + profileinfo->peermac[0], profileinfo->peermac[1], profileinfo->peermac[2], profileinfo->peermac[3], profileinfo->peermac[4], profileinfo->peermac[5]); + if (_rtw_memcmp(peermacaddr, profileinfo->peermac, ETH_ALEN)) { + match_result = 1; + RTW_INFO("[%s] Match!\n", __FUNCTION__); + break; + } + } + + return match_result ; +} + +void issue_probersp_p2p(_adapter *padapter, unsigned char *da) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned char *mac; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + /* WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); */ + u16 beacon_interval = 100; + u16 capInfo = 0; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u8 wpsie[255] = { 0x00 }; + u32 wpsielen = 0, p2pielen = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + /* RTW_INFO("%s\n", __FUNCTION__); */ + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + if (IS_CCK_RATE(pattrib->rate)) { + /* force OFDM 6M rate */ + pattrib->rate = MGN_6M; + pattrib->raid = rtw_get_mgntframe_raid(padapter, WIRELESS_11G); + } + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + mac = adapter_mac_addr(padapter); + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN); + + /* Use the device address for BSSID field. */ + _rtw_memcpy(pwlanhdr->addr3, mac, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(fctrl, WIFI_PROBERSP); + + pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = pattrib->hdrlen; + pframe += pattrib->hdrlen; + + /* timestamp will be inserted by hardware */ + pframe += 8; + pattrib->pktlen += 8; + + /* beacon interval: 2 bytes */ + _rtw_memcpy(pframe, (unsigned char *) &beacon_interval, 2); + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + /* ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec) */ + capInfo |= cap_ShortPremble; + capInfo |= cap_ShortSlot; + + _rtw_memcpy(pframe, (unsigned char *) &capInfo, 2); + pframe += 2; + pattrib->pktlen += 2; + + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen); + + /* supported rates... */ + /* Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 ) */ + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen); + +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) { + if (pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL) { + /* WPS IE */ + _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len); + pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len; + pframe += pmlmepriv->wps_probe_resp_ie_len; + + /* P2P IE */ + _rtw_memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len); + pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len; + pframe += pmlmepriv->p2p_probe_resp_ie_len; + } + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + + /* Todo: WPS IE */ + /* Noted by Albert 20100907 */ + /* According to the WPS specification, all the WPS attribute is presented by Big Endian. */ + + wpsielen = 0; + /* WPS OUI */ + *(u32 *)(wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + /* WiFi Simple Config State */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG; /* Not Configured. */ + + /* Response Type */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X; + + /* UUID-E */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010); + wpsielen += 2; + + /* Value: */ + if (pwdinfo->external_uuid == 0) { + _rtw_memset(wpsie + wpsielen, 0x0, 16); + _rtw_memcpy(wpsie + wpsielen, mac, ETH_ALEN); + } else + _rtw_memcpy(wpsie + wpsielen, pwdinfo->uuid, 0x10); + wpsielen += 0x10; + + /* Manufacturer */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0007); + wpsielen += 2; + + /* Value: */ + _rtw_memcpy(wpsie + wpsielen, "Realtek", 7); + wpsielen += 7; + + /* Model Name */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0006); + wpsielen += 2; + + /* Value: */ + _rtw_memcpy(wpsie + wpsielen, "8192CU", 6); + wpsielen += 6; + + /* Model Number */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = 0x31; /* character 1 */ + + /* Serial Number */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SERIAL_NUMBER); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(ETH_ALEN); + wpsielen += 2; + + /* Value: */ + _rtw_memcpy(wpsie + wpsielen, "123456" , ETH_ALEN); + wpsielen += ETH_ALEN; + + /* Primary Device Type */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008); + wpsielen += 2; + + /* Value: */ + /* Category ID */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + wpsielen += 2; + + /* OUI */ + *(u32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* Sub Category ID */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + wpsielen += 2; + + /* Device Name */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len); + wpsielen += 2; + + /* Value: */ + _rtw_memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len); + wpsielen += pwdinfo->device_name_len; + + /* Config Method */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm); + wpsielen += 2; + + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + + + p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe); + pframe += p2pielen; + pattrib->pktlen += p2pielen; + } + +#ifdef CONFIG_WFD + wfdielen = rtw_append_probe_resp_wfd_ie(padapter, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + +/* Vendor Specific IE */ +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_P2P_PROBERESP_VENDOR_IE_BIT); +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned char *mac; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 }; + u16 wpsielen = 0, p2pielen = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + if (IS_CCK_RATE(pattrib->rate)) { + /* force OFDM 6M rate */ + pattrib->rate = MGN_6M; + pattrib->raid = rtw_get_mgntframe_raid(padapter, WIRELESS_11G); + } + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + mac = adapter_mac_addr(padapter); + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + if (da) { + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN); + } else { + if ((pwdinfo->p2p_info.scan_op_ch_only) || (pwdinfo->rx_invitereq_info.scan_op_ch_only)) { + /* This two flags will be set when this is only the P2P client mode. */ + _rtw_memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN); + } else { + /* broadcast probe request frame */ + _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN); + } + } + _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_PROBEREQ); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) + pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen)); + else + pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen)); + /* Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 ) */ + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen); + +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) { + if (pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL) { + /* WPS IE */ + _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len); + pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len; + pframe += pmlmepriv->wps_probe_req_ie_len; + + /* P2P IE */ + _rtw_memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len); + pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len; + pframe += pmlmepriv->p2p_probe_req_ie_len; + } + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + + /* WPS IE */ + /* Noted by Albert 20110221 */ + /* According to the WPS specification, all the WPS attribute is presented by Big Endian. */ + + wpsielen = 0; + /* WPS OUI */ + *(u32 *)(wpsie) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* WPS version */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ + + if (pmlmepriv->wps_probe_req_ie == NULL) { + /* UUID-E */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010); + wpsielen += 2; + + /* Value: */ + if (pwdinfo->external_uuid == 0) { + _rtw_memset(wpsie + wpsielen, 0x0, 16); + _rtw_memcpy(wpsie + wpsielen, mac, ETH_ALEN); + } else + _rtw_memcpy(wpsie + wpsielen, pwdinfo->uuid, 0x10); + wpsielen += 0x10; + + /* Config Method */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm); + wpsielen += 2; + } + + /* Device Name */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len); + wpsielen += 2; + + /* Value: */ + _rtw_memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len); + wpsielen += pwdinfo->device_name_len; + + /* Primary Device Type */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008); + wpsielen += 2; + + /* Value: */ + /* Category ID */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI); + wpsielen += 2; + + /* OUI */ + *(u32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI); + wpsielen += 4; + + /* Sub Category ID */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP); + wpsielen += 2; + + /* Device Password ID */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002); + wpsielen += 2; + + /* Value: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC); /* Registrar-specified */ + wpsielen += 2; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20110221 */ + /* According to the P2P Specification, the probe request frame should contain 5 P2P attributes */ + /* 1. P2P Capability */ + /* 2. P2P Device ID if this probe request wants to find the specific P2P device */ + /* 3. Listen Channel */ + /* 4. Extended Listen Timing */ + /* 5. Operating Channel if this WiFi is working as the group owner now */ + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT; + else + p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT; + + /* Listen Channel */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + p2pie[p2pielen++] = 0x51; /* Copy from SD7 */ + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->listen_channel; /* listen channel */ + + + /* Extended Listen Timing */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* Operating Channel (if this WiFi is working as the group owner now) */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005); + p2pielen += 2; + + /* Value: */ + /* Country String */ + p2pie[p2pielen++] = 'X'; + p2pie[p2pielen++] = 'X'; + + /* The third byte should be set to 0x04. */ + /* Described in the "Operating Channel Attribute" section. */ + p2pie[p2pielen++] = 0x04; + + /* Operating Class */ + p2pie[p2pielen++] = 0x51; /* Copy from SD7 */ + + /* Channel Number */ + p2pie[p2pielen++] = pwdinfo->operating_channel; /* operating channel number */ + + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + + } + +#ifdef CONFIG_WFD + wfdielen = rtw_append_probe_req_wfd_ie(padapter, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + +/* Vendor Specific IE */ +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_P2P_PROBEREQ_VENDOR_IE_BIT); +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +inline void issue_probereq_p2p(_adapter *adapter, u8 *da) +{ + _issue_probereq_p2p(adapter, da, _FALSE); +} + +/* + * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + * try_cnt means the maximal TX count to try + */ +int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + systime start = rtw_get_current_time(); + + do { + ret = _issue_probereq_p2p(adapter, da, wait_ms > 0 ? _TRUE : _FALSE); + + i++; + + if (RTW_CANNOT_RUN(adapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (da) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +#endif /* CONFIG_P2P */ + +s32 rtw_action_public_decache(union recv_frame *rframe, u8 token_offset) +{ + _adapter *adapter = rframe->u.hdr.adapter; + struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv); + u8 *frame = rframe->u.hdr.rx_data; + u16 seq_ctrl = ((rframe->u.hdr.attrib.seq_num & 0xffff) << 4) | (rframe->u.hdr.attrib.frag_num & 0xf); + u8 token = *(rframe->u.hdr.rx_data + sizeof(struct rtw_ieee80211_hdr_3addr) + token_offset); + + if (GetRetry(frame)) { + if ((seq_ctrl == mlmeext->action_public_rxseq) + && (token == mlmeext->action_public_dialog_token) + ) { + RTW_INFO(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n", + FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token); + return _FAIL; + } + } + + /* TODO: per sta seq & token */ + mlmeext->action_public_rxseq = seq_ctrl; + mlmeext->action_public_dialog_token = token; + + return _SUCCESS; +} + +unsigned int on_action_public_p2p(union recv_frame *precv_frame) +{ + _adapter *padapter = precv_frame->u.hdr.adapter; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + u8 *frame_body; +#ifdef CONFIG_P2P + u8 *p2p_ie; + u32 p2p_ielen; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u8 result = P2P_STATUS_SUCCESS; + u8 empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + u8 *merged_p2pie = NULL; + u32 merged_p2p_ielen = 0; +#ifdef CONFIG_CONCURRENT_MODE + struct roch_info *prochinfo = &padapter->rochinfo; +#endif +#endif /* CONFIG_P2P */ + + frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + +#ifdef CONFIG_P2P + _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey); +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) + rtw_cfg80211_rx_p2p_action_public(padapter, precv_frame); + else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + /* Do nothing if the driver doesn't enable the P2P function. */ + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) + return _SUCCESS; + + len -= sizeof(struct rtw_ieee80211_hdr_3addr); + + switch (frame_body[6]) { /* OUI Subtype */ + case P2P_GO_NEGO_REQ: { + RTW_INFO("[%s] Got GO Nego Req Frame\n", __FUNCTION__); + _rtw_memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info)); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ)) + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) { + /* Commented by Albert 20110526 */ + /* In this case, this means the previous nego fail doesn't be reset yet. */ + _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer); + /* Restore the previous p2p state */ + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + RTW_INFO("[%s] Restore the previous p2p state to %d\n", __FUNCTION__, rtw_p2p_state(pwdinfo)); + } +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(padapter, WIFI_ASOC_STATE)) + _cancel_timer_ex(&prochinfo->ap_roch_ch_switch_timer); +#endif /* CONFIG_CONCURRENT_MODE */ + + /* Commented by Kurt 20110902 */ + /* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */ + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) + rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo)); + + /* Commented by Kurt 20120113 */ + /* Get peer_dev_addr here if peer doesn't issue prov_disc frame. */ + if (_rtw_memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN)) + _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, get_addr2_ptr(pframe), ETH_ALEN); + + result = process_p2p_group_negotation_req(pwdinfo, frame_body, len); + issue_p2p_GO_response(padapter, get_addr2_ptr(pframe), frame_body, len, result); + + /* Commented by Albert 20110718 */ + /* No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */ +#ifdef CONFIG_CONCURRENT_MODE + /* Commented by Albert 20120107 */ + _set_timer(&pwdinfo->restore_p2p_state_timer, 3000); +#else /* CONFIG_CONCURRENT_MODE */ + _set_timer(&pwdinfo->restore_p2p_state_timer, 5000); +#endif /* CONFIG_CONCURRENT_MODE */ + break; + } + case P2P_GO_NEGO_RESP: { + RTW_INFO("[%s] Got GO Nego Resp Frame\n", __FUNCTION__); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) { + /* Commented by Albert 20110425 */ + /* The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */ + _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer); + pwdinfo->nego_req_info.benable = _FALSE; + result = process_p2p_group_negotation_resp(pwdinfo, frame_body, len); + issue_p2p_GO_confirm(pwdinfo->padapter, get_addr2_ptr(pframe), result); + if (P2P_STATUS_SUCCESS == result) { + if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) { + pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch; +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->p2p_info.operation_ch[1] = 1; /* Check whether GO is operating in channel 1; */ + pwdinfo->p2p_info.operation_ch[2] = 6; /* Check whether GO is operating in channel 6; */ + pwdinfo->p2p_info.operation_ch[3] = 11; /* Check whether GO is operating in channel 11; */ +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->p2p_info.scan_op_ch_only = 1; + _set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH); + } + } + + /* Reset the dialog token for group negotiation frames. */ + pwdinfo->negotiation_dialog_token = 1; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) + _set_timer(&pwdinfo->restore_p2p_state_timer, 5000); + } else + RTW_INFO("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __FUNCTION__); + + break; + } + case P2P_GO_NEGO_CONF: { + RTW_INFO("[%s] Got GO Nego Confirm Frame\n", __FUNCTION__); + result = process_p2p_group_negotation_confirm(pwdinfo, frame_body, len); + if (P2P_STATUS_SUCCESS == result) { + if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) { + pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch; +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->p2p_info.operation_ch[1] = 1; /* Check whether GO is operating in channel 1; */ + pwdinfo->p2p_info.operation_ch[2] = 6; /* Check whether GO is operating in channel 6; */ + pwdinfo->p2p_info.operation_ch[3] = 11; /* Check whether GO is operating in channel 11; */ +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->p2p_info.scan_op_ch_only = 1; + _set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH); + } + } + break; + } + case P2P_INVIT_REQ: { + /* Added by Albert 2010/10/05 */ + /* Received the P2P Invite Request frame. */ + + RTW_INFO("[%s] Got invite request frame!\n", __FUNCTION__); + p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen); + if (p2p_ie) { + /* Parse the necessary information from the P2P Invitation Request frame. */ + /* For example: The MAC address of sending this P2P Invitation Request frame. */ + u32 attr_contentlen = 0; + u8 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + struct group_id_info group_id; + u8 invitation_flag = 0; + + merged_p2p_ielen = rtw_get_p2p_merged_ies_len(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_); + + merged_p2pie = rtw_zmalloc(merged_p2p_ielen + 2); /* 2 is for EID and Length */ + if (merged_p2pie == NULL) { + RTW_INFO("[%s] Malloc p2p ie fail\n", __FUNCTION__); + goto exit; + } + _rtw_memset(merged_p2pie, 0x00, merged_p2p_ielen); + + merged_p2p_ielen = rtw_p2p_merge_ies(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, merged_p2pie); + + rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen); + if (attr_contentlen) { + + rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen); + /* Commented by Albert 20120510 */ + /* Copy to the pwdinfo->p2p_peer_interface_addr. */ + /* So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command. */ + /* #> iwpriv wlan0 p2p_get peer_ifa */ + /* After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */ + + if (attr_contentlen) { + RTW_INFO("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__, + pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1], + pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3], + pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]); + } + + if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT) { + /* Re-invoke the persistent group. */ + + _rtw_memset(&group_id, 0x00, sizeof(struct group_id_info)); + rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *) &group_id, &attr_contentlen); + if (attr_contentlen) { + if (_rtw_memcmp(group_id.go_device_addr, adapter_mac_addr(padapter), ETH_ALEN)) { + /* The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */ + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + status_code = P2P_STATUS_SUCCESS; + } else { + /* The p2p device sending this p2p invitation request wants to be the persistent GO. */ + if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[0])) { + u8 operatingch_info[5] = { 0x00 }; + if (rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, + &attr_contentlen)) { + if (rtw_chset_search_ch(adapter_to_chset(padapter), (u32)operatingch_info[4]) >= 0) { + /* The operating channel is acceptable for this device. */ + pwdinfo->rx_invitereq_info.operation_ch[0] = operatingch_info[4]; +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->rx_invitereq_info.operation_ch[1] = 1; /* Check whether GO is operating in channel 1; */ + pwdinfo->rx_invitereq_info.operation_ch[2] = 6; /* Check whether GO is operating in channel 6; */ + pwdinfo->rx_invitereq_info.operation_ch[3] = 11; /* Check whether GO is operating in channel 11; */ +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->rx_invitereq_info.scan_op_ch_only = 1; + _set_timer(&pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + status_code = P2P_STATUS_SUCCESS; + } else { + /* The operating channel isn't supported by this device. */ + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + status_code = P2P_STATUS_FAIL_NO_COMMON_CH; + _set_timer(&pwdinfo->restore_p2p_state_timer, 3000); + } + } else { + /* Commented by Albert 20121130 */ + /* Intel will use the different P2P IE to store the operating channel information */ + /* Workaround for Intel WiDi 3.5 */ + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + status_code = P2P_STATUS_SUCCESS; + } + } else { + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH); + status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP; + } + } + } else { + RTW_INFO("[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__); + status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } + } else { + /* Received the invitation to join a P2P group. */ + + _rtw_memset(&group_id, 0x00, sizeof(struct group_id_info)); + rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *) &group_id, &attr_contentlen); + if (attr_contentlen) { + if (_rtw_memcmp(group_id.go_device_addr, adapter_mac_addr(padapter), ETH_ALEN)) { + /* In this case, the GO can't be myself. */ + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH); + status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } else { + /* The p2p device sending this p2p invitation request wants to join an existing P2P group */ + /* Commented by Albert 2012/06/28 */ + /* In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */ + /* The peer device address should be the destination address for the provisioning discovery request. */ + /* Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */ + /* The peer interface address should be the address for WPS mac address */ + _rtw_memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN); + status_code = P2P_STATUS_SUCCESS; + } + } else { + RTW_INFO("[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__); + status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } + } + } else { + RTW_INFO("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __FUNCTION__); + status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } + + RTW_INFO("[%s] status_code = %d\n", __FUNCTION__, status_code); + + pwdinfo->inviteresp_info.token = frame_body[7]; + issue_p2p_invitation_response(padapter, get_addr2_ptr(pframe), pwdinfo->inviteresp_info.token, status_code); + _set_timer(&pwdinfo->restore_p2p_state_timer, 3000); + } + break; + } + case P2P_INVIT_RESP: { + u8 attr_content = 0x00; + u32 attr_contentlen = 0; + + RTW_INFO("[%s] Got invite response frame!\n", __FUNCTION__); + _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer); + p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen); + if (p2p_ie) { + rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + + if (attr_contentlen == 1) { + RTW_INFO("[%s] Status = %d\n", __FUNCTION__, attr_content); + pwdinfo->invitereq_info.benable = _FALSE; + + if (attr_content == P2P_STATUS_SUCCESS) { + if (_rtw_memcmp(pwdinfo->invitereq_info.go_bssid, adapter_mac_addr(padapter), ETH_ALEN)) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + else + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK); + } else { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL); + } + } else { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL); + } + } else { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL); + } + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL)) + _set_timer(&pwdinfo->restore_p2p_state_timer, 5000); + break; + } + case P2P_DEVDISC_REQ: + + process_p2p_devdisc_req(pwdinfo, pframe, len); + + break; + + case P2P_DEVDISC_RESP: + + process_p2p_devdisc_resp(pwdinfo, pframe, len); + + break; + + case P2P_PROVISION_DISC_REQ: + RTW_INFO("[%s] Got Provisioning Discovery Request Frame\n", __FUNCTION__); + process_p2p_provdisc_req(pwdinfo, pframe, len); + _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, get_addr2_ptr(pframe), ETH_ALEN); + + /* 20110902 Kurt */ + /* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */ + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ)) + rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo)); + + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ); + _set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT); + break; + + case P2P_PROVISION_DISC_RESP: + /* Commented by Albert 20110707 */ + /* Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */ + RTW_INFO("[%s] Got Provisioning Discovery Response Frame\n", __FUNCTION__); + /* Commented by Albert 20110426 */ + /* The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */ + _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer); + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP); + process_p2p_provdisc_resp(pwdinfo, pframe); + _set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT); + break; + + } + } + + +exit: + + if (merged_p2pie) + rtw_mfree(merged_p2pie, merged_p2p_ielen + 2); +#endif /* CONFIG_P2P */ + return _SUCCESS; +} + +unsigned int on_action_public_vendor(union recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + _adapter *adapter = precv_frame->u.hdr.adapter; + int cnt = 0; + char msg[64]; + + if (_rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE) { + if (rtw_action_public_decache(precv_frame, 7) == _FAIL) + goto exit; + + if (!hal_chk_wl_func(precv_frame->u.hdr.adapter, WL_FUNC_MIRACAST)) + rtw_rframe_del_wfd_ie(precv_frame, 8); + + ret = on_action_public_p2p(precv_frame); + } else if (_rtw_memcmp(frame_body + 2, DPP_OUI, 4) == _TRUE) { + u8 dpp_type = frame_body[7]; + +#ifdef CONFIG_IOCTL_CFG80211 + cnt += sprintf((msg + cnt), "DPP(type:%u)", dpp_type); + rtw_cfg80211_rx_action(adapter, precv_frame, msg); +#endif + } + +exit: + return ret; +} + +unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action) +{ + unsigned int ret = _FAIL; + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + u8 token; + _adapter *adapter = precv_frame->u.hdr.adapter; + int cnt = 0; + char msg[64]; + + token = frame_body[2]; + + if (rtw_action_public_decache(precv_frame, 2) == _FAIL) + goto exit; + +#ifdef CONFIG_IOCTL_CFG80211 + cnt += sprintf((msg + cnt), "%s(token:%u)", action_public_str(action), token); + rtw_cfg80211_rx_action(adapter, precv_frame, msg); +#endif + + ret = _SUCCESS; + +exit: + return ret; +} + +unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame) +{ + unsigned int ret = _FAIL; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint frame_len = precv_frame->u.hdr.len; + u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + u8 category, action; + + /* check RA matches or broadcast */ + if (!(_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN) || + is_broadcast_mac_addr(GetAddr1Ptr(pframe)))) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_PUBLIC) + goto exit; + + action = frame_body[1]; + switch (action) { + case ACT_PUBLIC_BSSCOEXIST: +#ifdef CONFIG_80211N_HT +#ifdef CONFIG_AP_MODE + /*20/40 BSS Coexistence Management frame is a Public Action frame*/ + if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == _TRUE) + rtw_process_public_act_bsscoex(padapter, pframe, frame_len); +#endif /*CONFIG_AP_MODE*/ +#endif /*CONFIG_80211N_HT*/ + break; + case ACT_PUBLIC_VENDOR: + ret = on_action_public_vendor(precv_frame); + break; + default: + ret = on_action_public_default(precv_frame, action); + break; + } + +exit: + return ret; +} + +unsigned int OnAction_ft(_adapter *padapter, union recv_frame *precv_frame) +{ +#ifdef CONFIG_RTW_80211R + u32 ret = _FAIL; + u32 frame_len = 0; + u8 action_code = 0; + u8 category = 0; + u8 *pframe = NULL; + u8 *pframe_body = NULL; + u8 tgt_addr[ETH_ALEN]; + u8 *pie = NULL; + u32 ft_ie_len = 0; + u32 status_code = 0; + struct mlme_ext_priv *pmlmeext = NULL; + struct mlme_ext_info *pmlmeinfo = NULL; + struct mlme_priv *pmlmepriv = NULL; + struct wlan_network *proam_target = NULL; + struct ft_roam_info *pft_roam = NULL; + _irqL irqL; + + pmlmeext = &(padapter->mlmeextpriv); + pmlmeinfo = &(pmlmeext->mlmext_info); + pmlmepriv = &(padapter->mlmepriv); + pft_roam = &(pmlmepriv->ft_roam); + pframe = precv_frame->u.hdr.rx_data; + frame_len = precv_frame->u.hdr.len; + pframe_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + category = pframe_body[0]; + + if (category != RTW_WLAN_CATEGORY_FT) + goto exit; + + action_code = pframe_body[1]; + switch (action_code) { + case RTW_WLAN_ACTION_FT_RSP: + RTW_INFO("FT: RTW_WLAN_ACTION_FT_RSP recv.\n"); + if (!_rtw_memcmp(adapter_mac_addr(padapter), &pframe_body[2], ETH_ALEN)) { + RTW_ERR("FT: Unmatched STA MAC Address "MAC_FMT"\n", MAC_ARG(&pframe_body[2])); + goto exit; + } + + status_code = le16_to_cpu(*(u16 *)((SIZE_PTR)pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + 14)); + if (status_code != 0) { + RTW_ERR("FT: WLAN ACTION FT RESPONSE fail, status: %d\n", status_code); + goto exit; + } + + _rtw_memcpy(tgt_addr, &pframe_body[8], ETH_ALEN); + if (is_zero_mac_addr(tgt_addr) || is_broadcast_mac_addr(tgt_addr)) { + RTW_ERR("FT: Invalid Target MAC Address "MAC_FMT"\n", MAC_ARG(tgt_addr)); + goto exit; + } + + pie = rtw_get_ie(pframe_body, _MDIE_, &ft_ie_len, frame_len); + if (pie) { + if (!_rtw_memcmp(&pft_roam->mdid, pie+2, 2)) { + RTW_ERR("FT: Invalid MDID\n"); + goto exit; + } + } + + rtw_ft_set_status(padapter, RTW_FT_REQUESTED_STA); + _cancel_timer_ex(&pmlmeext->ft_link_timer); + + /*Disconnect current AP*/ + receive_disconnect(padapter, pmlmepriv->cur_network.network.MacAddress, WLAN_REASON_ACTIVE_ROAM, _FALSE); + + pft_roam->ft_action_len = frame_len; + _rtw_memcpy(pft_roam->ft_action, pframe, rtw_min(frame_len, RTW_FT_MAX_IE_SZ)); + ret = _SUCCESS; + break; + case RTW_WLAN_ACTION_FT_REQ: + case RTW_WLAN_ACTION_FT_CONF: + case RTW_WLAN_ACTION_FT_ACK: + default: + RTW_ERR("FT: Unsupported FT Action!\n"); + break; + } + +exit: + return ret; +#else + return _SUCCESS; +#endif +} + +unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame) +{ + u8 *pframe = precv_frame->u.hdr.rx_data; + u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + u8 category, action; + + /* check RA matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN)) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_HT) + goto exit; + + action = frame_body[1]; + switch (action) { + case RTW_WLAN_ACTION_HT_SM_PS: +#ifdef CONFIG_80211N_HT +#ifdef CONFIG_AP_MODE + if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == _TRUE) + rtw_process_ht_action_smps(padapter, get_addr2_ptr(pframe), frame_body[2]); +#endif /*CONFIG_AP_MODE*/ +#endif /*CONFIG_80211N_HT*/ + break; + case RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING: +#ifdef CONFIG_BEAMFORMING + /*RTW_INFO("RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING\n");*/ + rtw_beamforming_get_report_frame(padapter, precv_frame); +#endif /*CONFIG_BEAMFORMING*/ + break; + default: + break; + } + +exit: + + return _SUCCESS; +} + +#ifdef CONFIG_IEEE80211W +unsigned int OnAction_sa_query(_adapter *padapter, union recv_frame *precv_frame) +{ + u8 *pframe = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u16 tid; + /* Baron */ + + RTW_INFO("OnAction_sa_query\n"); + + switch (pframe[WLAN_HDR_A3_LEN + 1]) { + case 0: /* SA Query req */ + _rtw_memcpy(&tid, &pframe[WLAN_HDR_A3_LEN + 2], sizeof(u16)); + RTW_INFO("OnAction_sa_query request,action=%d, tid=%04x, pframe=%02x-%02x\n" + , pframe[WLAN_HDR_A3_LEN + 1], tid, pframe[WLAN_HDR_A3_LEN + 2], pframe[WLAN_HDR_A3_LEN + 3]); + issue_action_SA_Query(padapter, get_addr2_ptr(pframe), 1, tid, IEEE80211W_RIGHT_KEY); + break; + + case 1: /* SA Query rsp */ + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta != NULL) + _cancel_timer_ex(&psta->dot11w_expire_timer); + + _rtw_memcpy(&tid, &pframe[WLAN_HDR_A3_LEN + 2], sizeof(u16)); + RTW_INFO("OnAction_sa_query response,action=%d, tid=%04x, cancel timer\n", pframe[WLAN_HDR_A3_LEN + 1], tid); + break; + default: + break; + } + if (0) { + int pp; + printk("pattrib->pktlen = %d =>", pattrib->pkt_len); + for (pp = 0; pp < pattrib->pkt_len; pp++) + printk(" %02x ", pframe[pp]); + printk("\n"); + } + + return _SUCCESS; +} +#endif /* CONFIG_IEEE80211W */ + +unsigned int on_action_rm(_adapter *padapter, union recv_frame *precv_frame) +{ +#ifdef CONFIG_RTW_80211K + return rm_on_action(padapter, precv_frame); +#else + return _SUCCESS; +#endif /* CONFIG_RTW_80211K */ +} + +unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame) +{ + return _SUCCESS; +} + +unsigned int OnAction_vht(_adapter *padapter, union recv_frame *precv_frame) +{ +#ifdef CONFIG_80211AC_VHT + u8 *pframe = precv_frame->u.hdr.rx_data; + struct rtw_ieee80211_hdr_3addr *whdr = (struct rtw_ieee80211_hdr_3addr *)pframe; + u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + u8 category, action; + struct sta_info *psta = NULL; + + /* check RA matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN)) + goto exit; + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_VHT) + goto exit; + + action = frame_body[1]; + switch (action) { + case RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING: +#ifdef CONFIG_BEAMFORMING + /*RTW_INFO("RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING\n");*/ + rtw_beamforming_get_report_frame(padapter, precv_frame); +#endif /*CONFIG_BEAMFORMING*/ + break; + case RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION: + /* CategoryCode(1) + ActionCode(1) + OpModeNotification(1) */ + /* RTW_INFO("RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION\n"); */ + psta = rtw_get_stainfo(&padapter->stapriv, whdr->addr2); + if (psta) + rtw_process_vht_op_mode_notify(padapter, &frame_body[2], psta); + break; + case RTW_WLAN_ACTION_VHT_GROUPID_MANAGEMENT: +#ifdef CONFIG_BEAMFORMING +#ifdef RTW_BEAMFORMING_VERSION_2 + rtw_beamforming_get_vht_gid_mgnt_frame(padapter, precv_frame); +#endif /* RTW_BEAMFORMING_VERSION_2 */ +#endif /* CONFIG_BEAMFORMING */ + break; + default: + break; + } + +exit: +#endif /* CONFIG_80211AC_VHT */ + + return _SUCCESS; +} + +unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame) +{ +#ifdef CONFIG_P2P + u8 *frame_body; + u8 category, OUI_Subtype, dialogToken = 0; + u8 *pframe = precv_frame->u.hdr.rx_data; + uint len = precv_frame->u.hdr.len; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + + /* check RA matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN)) + return _SUCCESS; + + frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + + category = frame_body[0]; + if (category != RTW_WLAN_CATEGORY_P2P) + return _SUCCESS; + + if (cpu_to_be32(*((u32 *)(frame_body + 1))) != P2POUI) + return _SUCCESS; + +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled + && pwdinfo->driver_interface == DRIVER_CFG80211 + ) { + rtw_cfg80211_rx_action_p2p(padapter, precv_frame); + return _SUCCESS; + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + len -= sizeof(struct rtw_ieee80211_hdr_3addr); + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + + switch (OUI_Subtype) { + case P2P_NOTICE_OF_ABSENCE: + + break; + + case P2P_PRESENCE_REQUEST: + + process_p2p_presence_req(pwdinfo, pframe, len); + + break; + + case P2P_PRESENCE_RESPONSE: + + break; + + case P2P_GO_DISC_REQUEST: + + break; + + default: + break; + + } + } +#endif /* CONFIG_P2P */ + + return _SUCCESS; + +} + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +unsigned int OnAction_tbtx_token(_adapter *padapter, union recv_frame *precv_frame) +{ +#define TOKEN_REQ 0x00 +#define TOKEN_REL 0x01 + + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + u32 xmit_time; + u8 *src=NULL, *pframe = precv_frame->u.hdr.rx_data; + u8 tbtx_action_code; + u8 i, nr_send; + uint tx_duration = 0; + + if (padapter->tbtx_capability == _FALSE) + goto exit; + + tbtx_action_code = *(pframe + WLAN_HDR_A3_LEN + 1); + + + switch (tbtx_action_code) + { + case TOKEN_REQ: + // parse duration + tx_duration = le32_to_cpu(*(uint *)(pframe + WLAN_HDR_A3_LEN + 2)); + padapter->tbtx_duration = tx_duration/1000; // Mirocsecond to Millisecond + ATOMIC_SET(&padapter->tbtx_tx_pause, _FALSE); + rtw_tx_control_cmd(padapter); + _set_timer(&pmlmeext->tbtx_xmit_timer, padapter->tbtx_duration); + ATOMIC_SET(&padapter->tbtx_remove_tx_pause, _FALSE); +#if defined(CONFIG_SDIO_HCI) && !defined(CONFIG_SDIO_TX_TASKLET) + _rtw_up_sema(&pxmitpriv->SdioXmitSema); +#else + tasklet_hi_schedule(&pxmitpriv->xmit_tasklet); +#endif + break; +#ifdef CONFIG_AP_MODE + case TOKEN_REL: + src = get_addr2_ptr(pframe); + if (!src) + goto exit; + psta = rtw_get_stainfo(&padapter->stapriv, src); + if (!psta) + goto exit; + + if (ATOMIC_READ(&pstapriv->nr_token_keeper) < 1) + goto exit; + + for (i=0; i< NR_MAXSTA_INSLOT; i++) { + if (pstapriv->token_holder[i] == psta) { + pstapriv->token_holder[i] = NULL; + //RTW_INFO("macaddr1:" MAC_FMT "\n", MAC_ARG(psta->cmn.mac_addr)); + ATOMIC_DEC(&pstapriv->nr_token_keeper); + break; + } + } + + if (ATOMIC_READ(&pstapriv->nr_token_keeper) == 0) + _set_timer(&pmlmeext->tbtx_token_dispatch_timer, 1); + + break; +#endif + default: + RTW_INFO("Undefined Action Code\n"); + goto exit; + break; + } + +exit: + return _SUCCESS; +} + +void rtw_issue_action_token_rel(_adapter *padapter) +{ + + // Todo: + // gen token + /* Token Release Format + Category code : 1 Byte + Action code : 1 Byte */ + int ret = _FAIL; + //u16 *fctrl; + u8 val = 0x01; + u8 category = RTW_WLAN_CATEGORY_TBTX; + u8 *pframe; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct rtw_ieee80211_hdr *pwlanhdr; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + //struct sta_info *psta; + //struct sta_priv *pstapriv = &padapter->stapriv; + //struct registry_priv *pregpriv = &padapter->registrypriv; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /*update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->rate = MGN_24M; /* issue action release using OFDM rate? 20190716 Bruce add */ + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + //fctrl = &(pwlanhdr->frame_ctl); + //*(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + // SetSeqNum?? + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(val), &(pattrib->pktlen)); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + //RTW_INFO("%s\n", __func__); + +} +#endif + +unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame) +{ + int i; + unsigned char category; + struct action_handler *ptable; + unsigned char *frame_body; + u8 *pframe = precv_frame->u.hdr.rx_data; + + frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + + category = frame_body[0]; + + for (i = 0; i < sizeof(OnAction_tbl) / sizeof(struct action_handler); i++) { + ptable = &OnAction_tbl[i]; + + if (category == ptable->num) + ptable->func(padapter, precv_frame); + + } + + return _SUCCESS; + +} + +unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame) +{ + + /* RTW_INFO("rcvd mgt frame(%x, %x)\n", (get_frame_sub_type(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe)); */ + return _SUCCESS; +} + +struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once) +{ + struct xmit_frame *pmgntframe; + struct xmit_buf *pxmitbuf; + + if (once) + pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv); + else + pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv); + + if (pmgntframe == NULL) { + RTW_INFO(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once); + goto exit; + } + + pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv); + if (pxmitbuf == NULL) { + RTW_INFO(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter)); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + pmgntframe = NULL; + goto exit; + } + + pmgntframe->frame_tag = MGNT_FRAMETAG; + pmgntframe->pxmitbuf = pxmitbuf; + pmgntframe->buf_addr = pxmitbuf->pbuf; + pxmitbuf->priv_data = pmgntframe; + +exit: + return pmgntframe; + +} + +inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv) +{ + return _alloc_mgtxmitframe(pxmitpriv, _FALSE); +} + +inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv) +{ + return _alloc_mgtxmitframe(pxmitpriv, _TRUE); +} + + +/**************************************************************************** + +Following are some TX fuctions for WiFi MLME + +*****************************************************************************/ + +void update_mgnt_tx_rate(_adapter *padapter, u8 rate) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + pmlmeext->tx_rate = rate; + /* RTW_INFO("%s(): rate = %x\n",__FUNCTION__, rate); */ +} + + +void update_monitor_frame_attrib(_adapter *padapter, struct pkt_attrib *pattrib) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + u8 wireless_mode; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + + psta = rtw_get_stainfo(pstapriv, pattrib->ra); + + pattrib->hdrlen = 24; + pattrib->nr_frags = 1; + pattrib->priority = 7; + pattrib->mac_id = RTW_DEFAULT_MGMT_MACID; + pattrib->qsel = QSLT_MGNT; + + pattrib->pktlen = 0; + + if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB) + wireless_mode = WIRELESS_11B; + else + wireless_mode = WIRELESS_11G; + + pattrib->raid = rtw_get_mgntframe_raid(padapter, wireless_mode); +#ifdef CONFIG_80211AC_VHT + if (pHalData->rf_type == RF_1T1R) + pattrib->raid = RATEID_IDX_VHT_1SS; + else if (pHalData->rf_type == RF_2T2R || pHalData->rf_type == RF_2T4R) + pattrib->raid = RATEID_IDX_VHT_2SS; + else if (pHalData->rf_type == RF_3T3R) + pattrib->raid = RATEID_IDX_VHT_3SS; + else + pattrib->raid = RATEID_IDX_BGN_40M_1SS; +#endif + +#ifdef CONFIG_80211AC_VHT + pattrib->rate = MGN_VHT1SS_MCS9; +#else + pattrib->rate = MGN_MCS7; +#endif + + pattrib->encrypt = _NO_PRIVACY_; + pattrib->bswenc = _FALSE; + + pattrib->qos_en = _FALSE; + pattrib->ht_en = 1; + pattrib->bwmode = CHANNEL_WIDTH_20; + pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pattrib->sgi = _FALSE; + + pattrib->seqnum = pmlmeext->mgnt_seq; + + pattrib->retry_ctrl = _TRUE; + + pattrib->mbssid = 0; + pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no; + +} + +#ifdef CONFIG_RTW_MGMT_QUEUE +void update_mgntframe_subtype(_adapter *padapter, struct xmit_frame *pmgntframe) +{ + struct pkt_attrib *pattrib = &pmgntframe->attrib; + u8 *pframe; + u8 subtype, category ,action; + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + subtype = get_frame_sub_type(pframe); /* bit(7)~bit(2) */ + pattrib->subtype = subtype; + + rtw_action_frame_parse(pframe, pattrib->pktlen, &category, &action); + + if ((subtype == WIFI_ACTION && !(action == ACT_PUBLIC_FTM_REQ || action == ACT_PUBLIC_FTM)) || + subtype == WIFI_DISASSOC || subtype == WIFI_DEAUTH || + (subtype == WIFI_PROBERSP && MLME_IS_ADHOC(padapter))) + pattrib->ps_dontq = 0; + else + pattrib->ps_dontq = 1; +} +#endif + +void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib) +{ + u8 wireless_mode; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + +#ifdef CONFIG_P2P_PS_NOA_USE_MACID_SLEEP + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P_PS_NOA_USE_MACID_SLEEP */ + + /* _rtw_memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib)); */ + + pattrib->hdrlen = 24; + pattrib->nr_frags = 1; + pattrib->priority = 7; + pattrib->mac_id = RTW_DEFAULT_MGMT_MACID; + pattrib->qsel = QSLT_MGNT; + +#ifdef CONFIG_MCC_MODE + update_mcc_mgntframe_attrib(padapter, pattrib); +#endif + + +#ifdef CONFIG_P2P_PS_NOA_USE_MACID_SLEEP +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(padapter, WIFI_ASOC_STATE)) +#endif /* CONFIG_CONCURRENT_MODE */ + if (MLME_IS_GC(padapter)) { + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) { + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + + psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress); + if (psta) { + /* use macid sleep during NoA, mgmt frame use ac queue & ap macid */ + pattrib->mac_id = psta->cmn.mac_id; + pattrib->qsel = QSLT_VO; + } else { + if (pwdinfo->p2p_ps_state != P2P_PS_DISABLE) + RTW_ERR("%s , psta was NULL\n", __func__); + } + } + } +#endif /* CONFIG_P2P_PS_NOA_USE_MACID_SLEEP */ + + pattrib->pktlen = 0; + + if (IS_CCK_RATE(pmlmeext->tx_rate)) + wireless_mode = WIRELESS_11B; + else + wireless_mode = WIRELESS_11G; + pattrib->raid = rtw_get_mgntframe_raid(padapter, wireless_mode); + pattrib->rate = pmlmeext->tx_rate; + + pattrib->encrypt = _NO_PRIVACY_; + pattrib->bswenc = _FALSE; + + pattrib->qos_en = _FALSE; + pattrib->ht_en = _FALSE; + pattrib->bwmode = CHANNEL_WIDTH_20; + pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pattrib->sgi = _FALSE; + + pattrib->seqnum = pmlmeext->mgnt_seq; + + pattrib->retry_ctrl = _TRUE; + + pattrib->mbssid = 0; + pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no; +#ifdef CONFIG_RTW_MGMT_QUEUE + pattrib->ps_dontq = 1; +#endif +} + +void update_mgntframe_attrib_addr(_adapter *padapter, struct xmit_frame *pmgntframe) +{ + u8 *pframe; + struct pkt_attrib *pattrib = &pmgntframe->attrib; +#if defined(CONFIG_BEAMFORMING) || defined(CONFIG_ANTENNA_DIVERSITY) || defined(CONFIG_RTW_MGMT_QUEUE) + struct sta_info *sta = NULL; +#endif + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + + _rtw_memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN); + _rtw_memcpy(pattrib->ta, get_addr2_ptr(pframe), ETH_ALEN); + +#if defined(CONFIG_BEAMFORMING) || defined(CONFIG_ANTENNA_DIVERSITY) || defined(CONFIG_RTW_MGMT_QUEUE) + sta = pattrib->psta; + if (!sta) { + sta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + pattrib->psta = sta; + } + + #ifdef CONFIG_BEAMFORMING + if (sta) + update_attrib_txbf_info(padapter, pattrib, sta); + #endif +#endif /* defined(CONFIG_BEAMFORMING) || defined(CONFIG_ANTENNA_DIVERSITY) || defined(CONFIG_RTW_MGMT_QUEUE) */ +} + +void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe) +{ + if (RTW_CANNOT_RUN(padapter)) { + rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe); + return; + } + + rtw_hal_mgnt_xmit(padapter, pmgntframe); +} + +s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms) +{ + s32 ret = _FAIL; + _irqL irqL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf; + struct submit_ctx sctx; + + if (RTW_CANNOT_RUN(padapter)) { + rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe); + return ret; + } + + rtw_sctx_init(&sctx, timeout_ms); + pxmitbuf->sctx = &sctx; + + ret = rtw_hal_mgnt_xmit(padapter, pmgntframe); + + if (ret == _SUCCESS +#ifdef CONFIG_RTW_MGMT_QUEUE + || ret == RTW_QUEUE_MGMT +#endif + ) + ret = rtw_sctx_wait(&sctx, __func__); + + _enter_critical(&pxmitpriv->lock_sctx, &irqL); + pxmitbuf->sctx = NULL; + _exit_critical(&pxmitpriv->lock_sctx, &irqL); + + return ret; +} + +s32 dump_mgntframe_and_wait_ack_timeout(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms) +{ +#ifdef CONFIG_XMIT_ACK + static u8 seq_no = 0; + s32 ret = _FAIL; + struct xmit_priv *pxmitpriv = &(GET_PRIMARY_ADAPTER(padapter))->xmitpriv; + + if (RTW_CANNOT_RUN(padapter)) { + rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe); + return -1; + } + + _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL); + pxmitpriv->ack_tx = _TRUE; + pxmitpriv->seq_no = seq_no++; + pmgntframe->ack_report = 1; + rtw_sctx_init(&(pxmitpriv->ack_tx_ops), timeout_ms); + + ret = rtw_hal_mgnt_xmit(padapter, pmgntframe); + + if (ret == _SUCCESS +#ifdef CONFIG_RTW_MGMT_QUEUE + || ret == RTW_QUEUE_MGMT +#endif + ) + ret = rtw_sctx_wait(&(pxmitpriv->ack_tx_ops), __func__); + + pxmitpriv->ack_tx = _FALSE; + _exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL); + + return ret; +#else /* !CONFIG_XMIT_ACK */ + dump_mgntframe(padapter, pmgntframe); + rtw_msleep_os(50); + return _SUCCESS; +#endif /* !CONFIG_XMIT_ACK */ +} + +s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe) +{ + /* In this case, use 500 ms as the default wait_ack timeout */ + return dump_mgntframe_and_wait_ack_timeout(padapter, pmgntframe, 500); +} + + +int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode) +{ + u8 *ssid_ie; + sint ssid_len_ori; + int len_diff = 0; + + ssid_ie = rtw_get_ie(ies, WLAN_EID_SSID, &ssid_len_ori, ies_len); + + /* RTW_INFO("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __FUNCTION__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */ + + if (ssid_ie && ssid_len_ori > 0) { + switch (hidden_ssid_mode) { + case 1: { + u8 *next_ie = ssid_ie + 2 + ssid_len_ori; + u32 remain_len = 0; + + remain_len = ies_len - (next_ie - ies); + + ssid_ie[1] = 0; + _rtw_memcpy(ssid_ie + 2, next_ie, remain_len); + len_diff -= ssid_len_ori; + + break; + } + case 2: + _rtw_memset(&ssid_ie[2], 0, ssid_len_ori); + break; + default: + break; + } + } + + return len_diff; +} + +#ifdef CONFIG_AP_MODE +void issue_beacon(_adapter *padapter, int timeout_ms) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned int rate_len; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + _irqL irqL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); +#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ + + + /* RTW_INFO("%s\n", __FUNCTION__); */ + +#ifdef CONFIG_BCN_ICF + pmgntframe = rtw_alloc_bcnxmitframe(pxmitpriv); + if (pmgntframe == NULL) +#else + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) +#endif + { + RTW_INFO("%s, alloc mgnt frame fail\n", __FUNCTION__); + return; + } +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + _enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL); +#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */ + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->qsel = QSLT_BEACON; + +#if defined(CONFIG_CONCURRENT_MODE) && (!defined(CONFIG_SWTIMER_BASED_TXBCN)) + if (padapter->hw_port == HW_PORT1) + pattrib->mbssid = 1; +#endif +#ifdef CONFIG_FW_HANDLE_TXBCN + if (padapter->vap_id != CONFIG_LIMITED_AP_NUM) + pattrib->mbssid = padapter->vap_id; +#endif + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN); + + SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/); + /* pmlmeext->mgnt_seq++; */ + set_frame_sub_type(pframe, WIFI_BEACON); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + if (MLME_IS_AP(padapter) || MLME_IS_MESH(padapter)) { + /* RTW_INFO("ie len=%d\n", cur_network->IELength); */ +#ifdef CONFIG_P2P + /* for P2P : Primary Device Type & Device Name */ + u32 wpsielen = 0, insert_len = 0; + u8 *wpsie = NULL; + wpsie = rtw_get_wps_ie(cur_network->IEs + _FIXED_IE_LENGTH_, cur_network->IELength - _FIXED_IE_LENGTH_, NULL, &wpsielen); + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen > 0) { + uint wps_offset, remainder_ielen; + u8 *premainder_ie, *pframe_wscie; + + wps_offset = (uint)(wpsie - cur_network->IEs); + + premainder_ie = wpsie + wpsielen; + + remainder_ielen = cur_network->IELength - wps_offset - wpsielen; + +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) { + if (pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len > 0) { + _rtw_memcpy(pframe, cur_network->IEs, wps_offset); + pframe += wps_offset; + pattrib->pktlen += wps_offset; + + _rtw_memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len); + pframe += pmlmepriv->wps_beacon_ie_len; + pattrib->pktlen += pmlmepriv->wps_beacon_ie_len; + + /* copy remainder_ie to pframe */ + _rtw_memcpy(pframe, premainder_ie, remainder_ielen); + pframe += remainder_ielen; + pattrib->pktlen += remainder_ielen; + } else { + _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength); + pframe += cur_network->IELength; + pattrib->pktlen += cur_network->IELength; + } + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + pframe_wscie = pframe + wps_offset; + _rtw_memcpy(pframe, cur_network->IEs, wps_offset + wpsielen); + pframe += (wps_offset + wpsielen); + pattrib->pktlen += (wps_offset + wpsielen); + + /* now pframe is end of wsc ie, insert Primary Device Type & Device Name */ + /* Primary Device Type */ + /* Type: */ + *(u16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE); + insert_len += 2; + + /* Length: */ + *(u16 *)(pframe + insert_len) = cpu_to_be16(0x0008); + insert_len += 2; + + /* Value: */ + /* Category ID */ + *(u16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + insert_len += 2; + + /* OUI */ + *(u32 *)(pframe + insert_len) = cpu_to_be32(WPSOUI); + insert_len += 4; + + /* Sub Category ID */ + *(u16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + insert_len += 2; + + + /* Device Name */ + /* Type: */ + *(u16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + insert_len += 2; + + /* Length: */ + *(u16 *)(pframe + insert_len) = cpu_to_be16(pwdinfo->device_name_len); + insert_len += 2; + + /* Value: */ + _rtw_memcpy(pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len); + insert_len += pwdinfo->device_name_len; + + + /* update wsc ie length */ + *(pframe_wscie + 1) = (wpsielen - 2) + insert_len; + + /* pframe move to end */ + pframe += insert_len; + pattrib->pktlen += insert_len; + + /* copy remainder_ie to pframe */ + _rtw_memcpy(pframe, premainder_ie, remainder_ielen); + pframe += remainder_ielen; + pattrib->pktlen += remainder_ielen; + } + } else +#endif /* CONFIG_P2P */ + { + int len_diff; + _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength); + len_diff = update_hidden_ssid( + pframe + _BEACON_IE_OFFSET_ + , cur_network->IELength - _BEACON_IE_OFFSET_ + , pmlmeinfo->hidden_ssid_mode + ); + pframe += (cur_network->IELength + len_diff); + pattrib->pktlen += (cur_network->IELength + len_diff); + } + + { + u8 *wps_ie; + uint wps_ielen; + u8 sr = 0; + wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr + TXDESC_OFFSET + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, + pattrib->pktlen - sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_, NULL, &wps_ielen); + if (wps_ie && wps_ielen > 0) + rtw_get_wps_attr_content(wps_ie, wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL); + if (sr != 0) + set_fwstate(pmlmepriv, WIFI_UNDER_WPS); + else + _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS); + } + +#ifdef CONFIG_P2P + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + u32 len; +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) { + len = pmlmepriv->p2p_beacon_ie_len; + if (pmlmepriv->p2p_beacon_ie && len > 0) + _rtw_memcpy(pframe, pmlmepriv->p2p_beacon_ie, len); + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + len = build_beacon_p2p_ie(pwdinfo, pframe); + } + + pframe += len; + pattrib->pktlen += len; + +#ifdef CONFIG_MCC_MODE + pframe = rtw_hal_mcc_append_go_p2p_ie(padapter, pframe, &pattrib->pktlen); +#endif /* CONFIG_MCC_MODE*/ + +#ifdef CONFIG_WFD + len = rtw_append_beacon_wfd_ie(padapter, pframe); + pframe += len; + pattrib->pktlen += len; +#endif + } +#endif /* CONFIG_P2P */ +#ifdef CONFIG_RTW_REPEATER_SON + rtw_rson_append_ie(padapter, pframe, &pattrib->pktlen); +#endif +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_BEACON_VENDOR_IE_BIT); +#endif + +#ifdef CONFIG_RTL8812A + pframe = rtw_hal_set_8812a_vendor_ie(padapter, pframe, &pattrib->pktlen ); +#endif/*CONFIG_RTL8812A*/ + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (padapter->tbtx_capability == _TRUE) + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 8, REALTEK_TBTX_IE, &pattrib->pktlen); +#endif + + goto _issue_bcn; + + } + + /* below for ad-hoc mode */ + + /* timestamp will be inserted by hardware */ + pframe += 8; + pattrib->pktlen += 8; + + /* beacon interval: 2 bytes */ + + _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + + _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen); + + /* supported rates... */ + rate_len = rtw_get_rateset_len(cur_network->SupportedRates); + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen); + + /* if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */ + { + u8 erpinfo = 0; + u32 ATIMWindow; + /* IBSS Parameter Set... */ + /* ATIMWindow = cur->Configuration.ATIMWindow; */ + ATIMWindow = 0; + pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen); + + /* ERP IE */ + pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen); + } + + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen); + + + /* todo:HT for adhoc */ + +_issue_bcn: + +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + pmlmepriv->update_bcn = _FALSE; + + _exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL); +#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */ + + if ((pattrib->pktlen + TXDESC_SIZE) > MAX_BEACON_LEN) { + RTW_ERR("beacon frame too large ,len(%d,%d)\n", + (pattrib->pktlen + TXDESC_SIZE), MAX_BEACON_LEN); + rtw_warn_on(1); + return; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + /* RTW_INFO("issue bcn_sz=%d\n", pattrib->last_txcmdsz); */ + if (timeout_ms > 0) + dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms); + else + dump_mgntframe(padapter, pmgntframe); + +} +#endif /* CONFIG_AP_MODE */ + +void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned char *mac, *bssid; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + u8 *pwps_ie; + uint wps_ielen; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; +#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + unsigned int rate_len; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ + + /* RTW_INFO("%s\n", __FUNCTION__); */ + + if (da == NULL) + return; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_INFO("%s, alloc mgnt frame fail\n", __FUNCTION__); + return; + } + + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + mac = adapter_mac_addr(padapter); + bssid = cur_network->MacAddress; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, bssid, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(fctrl, WIFI_PROBERSP); + + pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = pattrib->hdrlen; + pframe += pattrib->hdrlen; + + + if (cur_network->IELength > MAX_IE_SZ) + return; + +#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) + if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) { + pwps_ie = rtw_get_wps_ie(cur_network->IEs + _FIXED_IE_LENGTH_, cur_network->IELength - _FIXED_IE_LENGTH_, NULL, &wps_ielen); + + /* inerset & update wps_probe_resp_ie */ + if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie && (wps_ielen > 0)) { + uint wps_offset, remainder_ielen; + u8 *premainder_ie; + + wps_offset = (uint)(pwps_ie - cur_network->IEs); + + premainder_ie = pwps_ie + wps_ielen; + + remainder_ielen = cur_network->IELength - wps_offset - wps_ielen; + + _rtw_memcpy(pframe, cur_network->IEs, wps_offset); + pframe += wps_offset; + pattrib->pktlen += wps_offset; + + wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */ + if ((wps_offset + wps_ielen + 2) <= MAX_IE_SZ) { + _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen + 2); + pframe += wps_ielen + 2; + pattrib->pktlen += wps_ielen + 2; + } + + if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) { + _rtw_memcpy(pframe, premainder_ie, remainder_ielen); + pframe += remainder_ielen; + pattrib->pktlen += remainder_ielen; + } + } else { + _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength); + pframe += cur_network->IELength; + pattrib->pktlen += cur_network->IELength; + } + + /* retrieve SSID IE from cur_network->Ssid */ + { + u8 *ssid_ie; + sint ssid_ielen; + sint ssid_ielen_diff; + u8 buf[MAX_IE_SZ]; + u8 *ies = pmgntframe->buf_addr + TXDESC_OFFSET + sizeof(struct rtw_ieee80211_hdr_3addr); + + ssid_ie = rtw_get_ie(ies + _FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen, + (pframe - ies) - _FIXED_IE_LENGTH_); + + ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen; + + if (ssid_ie && cur_network->Ssid.SsidLength) { + uint remainder_ielen; + u8 *remainder_ie; + remainder_ie = ssid_ie + 2; + remainder_ielen = (pframe - remainder_ie); + + if (remainder_ielen > MAX_IE_SZ) { + RTW_WARN(FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter)); + remainder_ielen = MAX_IE_SZ; + } + + _rtw_memcpy(buf, remainder_ie, remainder_ielen); + _rtw_memcpy(remainder_ie + ssid_ielen_diff, buf, remainder_ielen); + *(ssid_ie + 1) = cur_network->Ssid.SsidLength; + _rtw_memcpy(ssid_ie + 2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength); + + pframe += ssid_ielen_diff; + pattrib->pktlen += ssid_ielen_diff; + } + } +#ifdef CONFIG_RTW_REPEATER_SON + rtw_rson_append_ie(padapter, pframe, &pattrib->pktlen); +#endif +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_PROBERESP_VENDOR_IE_BIT); +#endif + } else +#endif + { + + /* timestamp will be inserted by hardware */ + pframe += 8; + pattrib->pktlen += 8; + + /* beacon interval: 2 bytes */ + + _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* capability info: 2 bytes */ + + _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2); + + pframe += 2; + pattrib->pktlen += 2; + + /* below for ad-hoc mode */ + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen); + + /* supported rates... */ + rate_len = rtw_get_rateset_len(cur_network->SupportedRates); + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen); + + /* DS parameter set */ + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen); + + if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) { + u8 erpinfo = 0; + u32 ATIMWindow; + /* IBSS Parameter Set... */ + /* ATIMWindow = cur->Configuration.ATIMWindow; */ + ATIMWindow = 0; + pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen); + + /* ERP IE */ + pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen); + } + + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen); + + + /* todo:HT for adhoc */ + + } + +#ifdef CONFIG_P2P + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) + /* IOT issue, When wifi_spec is not set, send probe_resp with P2P IE even if probe_req has no P2P IE */ + && (is_valid_p2p_probereq || !padapter->registrypriv.wifi_spec)) { + u32 len; +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) { + /* if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p() */ + len = pmlmepriv->p2p_go_probe_resp_ie_len; + if (pmlmepriv->p2p_go_probe_resp_ie && len > 0) + _rtw_memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len); + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + len = build_probe_resp_p2p_ie(pwdinfo, pframe); + } + + pframe += len; + pattrib->pktlen += len; + +#ifdef CONFIG_MCC_MODE + pframe = rtw_hal_mcc_append_go_p2p_ie(padapter, pframe, &pattrib->pktlen); +#endif /* CONFIG_MCC_MODE*/ + +#ifdef CONFIG_WFD + len = rtw_append_probe_resp_wfd_ie(padapter, pframe); + pframe += len; + pattrib->pktlen += len; +#endif + } +#endif /* CONFIG_P2P */ + + +#ifdef CONFIG_AUTO_AP_MODE + { + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + + RTW_INFO("(%s)\n", __FUNCTION__); + + /* check rc station */ + psta = rtw_get_stainfo(pstapriv, da); + if (psta && psta->isrc && psta->pid > 0) { + u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A}; + u8 RC_INFO[14] = {0}; + /* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */ + u16 cu_ch = (u16)cur_network->Configuration.DSConfig; + + RTW_INFO("%s, reply rc(pid=0x%x) device "MAC_FMT" in ch=%d\n", __FUNCTION__, + psta->pid, MAC_ARG(psta->cmn.mac_addr), cu_ch); + + /* append vendor specific ie */ + _rtw_memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI)); + _rtw_memcpy(&RC_INFO[4], mac, ETH_ALEN); + _rtw_memcpy(&RC_INFO[10], (u8 *)&psta->pid, 2); + _rtw_memcpy(&RC_INFO[12], (u8 *)&cu_ch, 2); + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen); + } + } +#endif /* CONFIG_AUTO_AP_MODE */ + +#ifdef CONFIG_RTL8812A + pframe = rtw_hal_set_8812a_vendor_ie(padapter, pframe, &pattrib->pktlen); +#endif/*CONFIG_RTL8812A*/ + + pattrib->last_txcmdsz = pattrib->pktlen; + + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +int _issue_probereq(_adapter *padapter, const NDIS_802_11_SSID *pssid, const u8 *da, u8 ch, bool append_wps, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned char *mac; + unsigned char bssrate[NumRates]; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + int bssrate_len = 0; + u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); +#endif + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + if ((pwdev_priv->pno_mac_addr[0] != 0xFF) + && (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) == _TRUE) + && (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _FALSE)) + mac = pwdev_priv->pno_mac_addr; + else +#endif + mac = adapter_mac_addr(padapter); + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + if (da) { + /* unicast probe request frame */ + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN); + } else { + /* broadcast probe request frame */ + _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN); + } + + _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN); + +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + if ((pwdev_priv->pno_mac_addr[0] != 0xFF) + && (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) == _TRUE) + && (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _FALSE)) { +#ifdef CONFIG_RTW_DEBUG + RTW_DBG("%s pno_scan_seq_num: %d\n", __func__, + pwdev_priv->pno_scan_seq_num); +#endif + SetSeqNum(pwlanhdr, pwdev_priv->pno_scan_seq_num); + pattrib->seqnum = pwdev_priv->pno_scan_seq_num; + pattrib->qos_en = 1; + pwdev_priv->pno_scan_seq_num++; + } else +#endif + { + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + } + set_frame_sub_type(pframe, WIFI_PROBEREQ); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + if (pssid && !MLME_IS_MESH(padapter)) + pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen)); + else + pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen)); + + get_rate_set(padapter, bssrate, &bssrate_len); + + if (bssrate_len > 8) { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen)); + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen)); + } else + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen)); + + if (ch) + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, &ch, &pattrib->pktlen); + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + if (pssid) + pframe = rtw_set_ie_mesh_id(pframe, &pattrib->pktlen, pssid->Ssid, pssid->SsidLength); + else + pframe = rtw_set_ie_mesh_id(pframe, &pattrib->pktlen, NULL, 0); + } +#endif + + if (append_wps) { + /* add wps_ie for wps2.0 */ + if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) { + _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len); + pframe += pmlmepriv->wps_probe_req_ie_len; + pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len; + /* pmlmepriv->wps_probe_req_ie_len = 0 ; */ /* reset to zero */ + } + } +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_PROBEREQ_VENDOR_IE_BIT); +#endif + +#ifdef CONFIG_RTL8812A + pframe = rtw_hal_set_8812a_vendor_ie(padapter, pframe, &pattrib->pktlen ); +#endif/*CONFIG_RTL8812A*/ + + +#ifdef CONFIG_RTW_MBO + rtw_mbo_build_probe_req_ies( padapter, &pframe, pattrib); +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +inline void issue_probereq(_adapter *padapter, const NDIS_802_11_SSID *pssid, const u8 *da) +{ + _issue_probereq(padapter, pssid, da, 0, 1, _FALSE); +} + +/* + * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + * try_cnt means the maximal TX count to try + */ +int issue_probereq_ex(_adapter *padapter, const NDIS_802_11_SSID *pssid, const u8 *da, u8 ch, bool append_wps, + int try_cnt, int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + do { + ret = _issue_probereq(padapter, pssid, da, ch, append_wps, wait_ms > 0 ? _TRUE : _FALSE); + + i++; + + if (RTW_CANNOT_RUN(padapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (da) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +/* if psta == NULL, indiate we are station(client) now... */ +void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned int val32; + unsigned short val16; + int use_shared_key = 0; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_AUTH); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + + if (psta) { /* for AP mode */ +#ifdef CONFIG_NATIVEAP_MLME + + _rtw_memcpy(pwlanhdr->addr1, psta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN); + + + /* setting auth algo number */ + val16 = (u16)psta->authalg; + + if (status != _STATS_SUCCESSFUL_) + val16 = 0; + + if (val16) { + val16 = cpu_to_le16(val16); + use_shared_key = 1; + } + + pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen)); + + /* setting auth seq number */ + val16 = (u16)psta->auth_seq; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen)); + + /* setting status code... */ + val16 = status; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen)); + + /* added challenging text... */ + if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) + pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen)); +#endif + } else { + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam(padapter)) { + /* 2: 802.11R FTAA */ + val16 = cpu_to_le16(2); + } else +#endif + { + /* setting auth algo number */ + val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0; /* 0:OPEN System, 1:Shared key */ + if (val16) { + val16 = cpu_to_le16(val16); + use_shared_key = 1; + } + } + + /* RTW_INFO("%s auth_algo= %s auth_seq=%d\n",__FUNCTION__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq); */ + + /* setting IV for auth seq #3 */ + if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) { + /* RTW_INFO("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index); */ + val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30)); + val32 = cpu_to_le32(val32); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&val32, &(pattrib->pktlen)); + + pattrib->iv_len = 4; + } + + pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen)); + + /* setting auth seq number */ + val16 = pmlmeinfo->auth_seq; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen)); + + + /* setting status code... */ + val16 = status; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen)); + +#ifdef CONFIG_RTW_80211R + rtw_ft_build_auth_req_ies(padapter, pattrib, &pframe); +#endif + + /* then checking to see if sending challenging text... */ + if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) { + pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen)); + + SetPrivacy(fctrl); + + pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pattrib->encrypt = _WEP40_; + + pattrib->icv_len = 4; + + pattrib->pktlen += pattrib->icv_len; + + } + + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + rtw_wep_encrypt(padapter, (u8 *)pmgntframe); + RTW_INFO("%s\n", __FUNCTION__); + dump_mgntframe(padapter, pmgntframe); + + return; +} + + +void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type) +{ +#ifdef CONFIG_AP_MODE + struct xmit_frame *pmgntframe; + struct rtw_ieee80211_hdr *pwlanhdr; + struct pkt_attrib *pattrib; + unsigned char *pbuf, *pframe; + unsigned short val, ie_status; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network); + u8 *ie = pnetwork->IEs, cap[5], i; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + +#endif /* CONFIG_P2P */ + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + RTW_INFO("%s\n", __FUNCTION__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy((void *)get_addr2_ptr(pwlanhdr), adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP)) + set_frame_sub_type(pwlanhdr, pkt_type); + else + return; + + pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen += pattrib->hdrlen; + pframe += pattrib->hdrlen; + + /* capability */ + val = *(unsigned short *)rtw_get_capability_from_ie(ie); +#ifdef CONFIG_RTW_80211K + val |= cap_RM; +#endif + pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen)); + + ie_status = cpu_to_le16(status); + pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&ie_status, &(pattrib->pktlen)); + + val = cpu_to_le16(pstat->cmn.aid | BIT(14) | BIT(15)); + pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&val, &(pattrib->pktlen)); + + if (pstat->bssratelen <= 8) + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen)); + else { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen)); + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen - 8), pstat->bssrateset + 8, &(pattrib->pktlen)); + } + +#ifdef CONFIG_IEEE80211W + if (status == _STATS_REFUSED_TEMPORARILY_) { + u8 timeout_itvl[5]; + u32 timeout_interval = 3000; + /* Association Comeback time */ + timeout_itvl[0] = 0x03; + timeout_interval = cpu_to_le32(timeout_interval); + _rtw_memcpy(timeout_itvl + 1, &timeout_interval, 4); + pframe = rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl, &(pattrib->pktlen)); + } +#endif /* CONFIG_IEEE80211W */ + +#ifdef CONFIG_80211N_HT + if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) { + uint ie_len = 0; + + /* FILL HT CAP INFO IE */ + /* p = hostapd_eid_ht_capabilities_info(hapd, p); */ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + _rtw_memcpy(pframe, pbuf, ie_len + 2); + pframe += (ie_len + 2); + pattrib->pktlen += (ie_len + 2); + } + + /* FILL HT ADD INFO IE */ + /* p = hostapd_eid_ht_operation(hapd, p); */ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + _rtw_memcpy(pframe, pbuf, ie_len + 2); + pframe += (ie_len + 2); + pattrib->pktlen += (ie_len + 2); + } + + } +#endif + + /*adding EXT_CAPAB_IE */ + if (pmlmepriv->ext_capab_ie_len > 0) { + uint ie_len = 0; + + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_CAP_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + _rtw_memcpy(pframe, pbuf, ie_len + 2); + pframe += (ie_len + 2); + pattrib->pktlen += (ie_len + 2); + } + } + +#ifdef CONFIG_80211AC_VHT + if ((pstat->flags & WLAN_STA_VHT) && (pmlmepriv->vhtpriv.vht_option) + && (pstat->wpa_pairwise_cipher != WPA_CIPHER_TKIP) + && (pstat->wpa2_pairwise_cipher != WPA_CIPHER_TKIP)) { + u32 ie_len = 0; + + /* FILL VHT CAP IE */ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTCapability, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + _rtw_memcpy(pframe, pbuf, ie_len + 2); + pframe += (ie_len + 2); + pattrib->pktlen += (ie_len + 2); + } + + /* FILL VHT OPERATION IE */ + pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTOperation, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (pbuf && ie_len > 0) { + _rtw_memcpy(pframe, pbuf, ie_len + 2); + pframe += (ie_len + 2); + pattrib->pktlen += (ie_len + 2); + } + } +#endif /* CONFIG_80211AC_VHT */ + +#ifdef CONFIG_RTW_80211K + /* FILL RM Enabled Capabilities with joint capabilities */ + for (i = 0; i < 5; i++) { + cap[i] = padapter->rmpriv.rm_en_cap_def[i] + & pstat->rm_en_cap[i]; + } + if (pstat->capability & cap_RM) + pframe = rtw_set_ie(pframe, _EID_RRM_EN_CAP_IE_, 5, + (u8 *)cap, &(pattrib->pktlen)); +#endif /* CONFIG_RTW_80211K */ + + /* FILL WMM IE */ + if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) { + uint ie_len = 0; + unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01}; + + for (pbuf = ie + _BEACON_IE_OFFSET_; ; pbuf += (ie_len + 2)) { + pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2))); + if (pbuf && _rtw_memcmp(pbuf + 2, WMM_PARA_IE, 6)) { + _rtw_memcpy(pframe, pbuf, ie_len + 2); + pframe += (ie_len + 2); + pattrib->pktlen += (ie_len + 2); + + break; + } + + if ((pbuf == NULL) || (ie_len == 0)) + break; + } + + } + + + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen)); + + /* add WPS IE ie for wps 2.0 */ + if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) { + _rtw_memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len); + + pframe += pmlmepriv->wps_assoc_resp_ie_len; + pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len; + } + +#ifdef CONFIG_P2P + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device == _TRUE)) { + u32 len; + + if (padapter->wdinfo.driver_interface == DRIVER_CFG80211) { + len = 0; + if (pmlmepriv->p2p_assoc_resp_ie && pmlmepriv->p2p_assoc_resp_ie_len > 0) { + len = pmlmepriv->p2p_assoc_resp_ie_len; + _rtw_memcpy(pframe, pmlmepriv->p2p_assoc_resp_ie, len); + } + } else + len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code); + pframe += len; + pattrib->pktlen += len; + } + +#ifdef CONFIG_WFD + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + wfdielen = rtw_append_assoc_resp_wfd_ie(padapter, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } +#endif + +#endif /* CONFIG_P2P */ + +#ifdef CONFIG_RTW_MULTI_AP + if (padapter->multi_ap && (pstat->flags & WLAN_STA_MULTI_AP)) + pframe = rtw_set_multi_ap_ie_ext(pframe, &pattrib->pktlen, padapter->multi_ap); +#endif + +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_ASSOCRESP_VENDOR_IE_BIT); +#endif + +#ifdef CONFIG_RTL8812A + pframe = rtw_hal_set_8812a_vendor_ie(padapter, pframe, &pattrib->pktlen ); +#endif/*CONFIG_RTL8812A*/ + + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (padapter->tbtx_capability == _TRUE) + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 8, REALTEK_TBTX_IE, &pattrib->pktlen); +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + +#endif +} + +static u32 rtw_append_assoc_req_owe_ie(_adapter *adapter, u8 *pbuf) +{ + struct security_priv *sec = &adapter->securitypriv; + u32 len = 0; + + if (sec == NULL) + goto exit; + + if (sec->owe_ie_len > 0) { + len = sec->owe_ie_len; + _rtw_memcpy(pbuf, sec->owe_ie, len); + } + +exit: + return len; +} + +void _issue_assocreq(_adapter *padapter, u8 is_reassoc) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned short val16; + unsigned int i, j, index = 0; + unsigned char bssrate[NumRates], sta_bssrate[NumRates]; + PNDIS_802_11_VARIABLE_IEs pIE; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + int bssrate_len = 0, sta_bssrate_len = 0; + u8 vs_ie_length = 0; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u8 p2pie[255] = { 0x00 }; + u16 p2pielen = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif +#endif /* CONFIG_P2P */ + +#if CONFIG_DFS + u16 cap; +#endif + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + if (is_reassoc == _TRUE) + set_frame_sub_type(pframe, WIFI_REASSOCREQ); + else + set_frame_sub_type(pframe, WIFI_ASSOCREQ); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* caps */ + +#if CONFIG_DFS + _rtw_memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2); + cap |= cap_SpecMgmt; +#ifdef CONFIG_RTW_80211K + cap |= cap_RM; +#endif + _rtw_memcpy(pframe, &cap, 2); +#else + _rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2); +#endif + + pframe += 2; + pattrib->pktlen += 2; + + /* listen interval */ + /* todo: listen interval for power saving */ + val16 = cpu_to_le16(3); + _rtw_memcpy(pframe , (unsigned char *)&val16, 2); + pframe += 2; + pattrib->pktlen += 2; + + /*Construct Current AP Field for Reassoc-Req only*/ + if (is_reassoc == _TRUE) { + _rtw_memcpy(pframe, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + pframe += ETH_ALEN; + pattrib->pktlen += ETH_ALEN; + } + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen)); + +#if CONFIG_IEEE80211_BAND_5GHZ && CONFIG_DFS + /* Dot H */ + if (pmlmeext->cur_channel > 14) { + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + u8 pow_cap_ele[2] = { 0x00 }; + u8 sup_ch[30 * 2] = {0x00 }, sup_ch_idx = 0, idx_5g = 2; /* For supported channel */ + + pow_cap_ele[0] = 13; /* Minimum transmit power capability */ + pow_cap_ele[1] = 21; /* Maximum transmit power capability */ + pframe = rtw_set_ie(pframe, EID_PowerCap, 2, pow_cap_ele, &(pattrib->pktlen)); + + /* supported channels */ + while (sup_ch_idx < rfctl->max_chan_nums && rfctl->channel_set[sup_ch_idx].ChannelNum != 0) { + if (rfctl->channel_set[sup_ch_idx].ChannelNum <= 14) { + /* TODO: fix 2.4G supported channel when channel doesn't start from 1 and continuous */ + sup_ch[0] = 1; /* First channel number */ + sup_ch[1] = rfctl->channel_set[sup_ch_idx].ChannelNum; /* Number of channel */ + } else { + sup_ch[idx_5g++] = rfctl->channel_set[sup_ch_idx].ChannelNum; + sup_ch[idx_5g++] = 1; + } + sup_ch_idx++; + } + pframe = rtw_set_ie(pframe, EID_SupportedChannels, idx_5g, sup_ch, &(pattrib->pktlen)); + } +#endif /* CONFIG_IEEE80211_BAND_5GHZ && CONFIG_DFS */ + + /* supported rate & extended supported rate */ + +#if 1 /* Check if the AP's supported rates are also supported by STA. */ + get_rate_set(padapter, sta_bssrate, &sta_bssrate_len); + /* RTW_INFO("sta_bssrate_len=%d\n", sta_bssrate_len); */ + + if (pmlmeext->cur_channel == 14) /* for JAPAN, channel 14 can only uses B Mode(CCK) */ + sta_bssrate_len = 4; + + + /* for (i = 0; i < sta_bssrate_len; i++) { */ + /* RTW_INFO("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */ + /* } */ + + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + if (pmlmeinfo->network.SupportedRates[i] == 0) + break; + RTW_INFO("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]); + } + + + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + if (pmlmeinfo->network.SupportedRates[i] == 0) + break; + + + /* Check if the AP's supported rates are also supported by STA. */ + for (j = 0; j < sta_bssrate_len; j++) { + /* Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */ + if ((pmlmeinfo->network.SupportedRates[i] | IEEE80211_BASIC_RATE_MASK) + == (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) { + /* RTW_INFO("match i = %d, j=%d\n", i, j); */ + break; + } else { + /* RTW_INFO("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)); */ + } + } + + if (j == sta_bssrate_len) { + /* the rate is not supported by STA */ + RTW_INFO("%s(): the rate[%d]=%02X is not supported by STA!\n", __FUNCTION__, i, pmlmeinfo->network.SupportedRates[i]); + } else { + /* the rate is supported by STA */ + bssrate[index++] = pmlmeinfo->network.SupportedRates[i]; + } + } + + bssrate_len = index; + RTW_INFO("bssrate_len = %d\n", bssrate_len); + +#else /* Check if the AP's supported rates are also supported by STA. */ +#if 0 + get_rate_set(padapter, bssrate, &bssrate_len); +#else + for (bssrate_len = 0; bssrate_len < NumRates; bssrate_len++) { + if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0) + break; + + if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0x2C) /* Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */ + break; + + bssrate[bssrate_len] = pmlmeinfo->network.SupportedRates[bssrate_len]; + } +#endif +#endif /* Check if the AP's supported rates are also supported by STA. */ + + if ((bssrate_len == 0) && (pmlmeinfo->network.SupportedRates[0] != 0)) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; /* don't connect to AP if no joint supported rate */ + } + + + if (bssrate_len > 8) { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen)); + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen)); + } else if (bssrate_len > 0) + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen)); + else + RTW_INFO("%s: Connect to AP without 11b and 11g data rate!\n", __FUNCTION__); + +#ifdef CONFIG_RTW_MBO + rtw_mbo_build_assoc_req_ies(padapter, &pframe, pattrib); +#endif +#ifdef CONFIG_RTW_80211R + rtw_ft_build_assoc_req_ies(padapter, is_reassoc, pattrib, &pframe); +#endif +#ifdef CONFIG_RTW_80211K + pframe = rtw_set_ie(pframe, _EID_RRM_EN_CAP_IE_, 5, + (u8 *)padapter->rmpriv.rm_en_cap_def, + &(pattrib->pktlen)); +#endif /* CONFIG_RTW_80211K */ + + /* vendor specific IE, such as WPA, WMM, WPS */ + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) || + (_rtw_memcmp(pIE->data, WMM_OUI, 4)) || + (_rtw_memcmp(pIE->data, WPS_OUI, 4))) { + vs_ie_length = pIE->Length; + if ((!padapter->registrypriv.wifi_spec) && (_rtw_memcmp(pIE->data, WPS_OUI, 4))) { + /* Commented by Kurt 20110629 */ + /* In some older APs, WPS handshake */ + /* would be fail if we append vender extensions informations to AP */ + + vs_ie_length = 14; + } + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, vs_ie_length, pIE->data, &(pattrib->pktlen)); + } + break; + + case EID_WPA2: +#ifdef CONFIG_RTW_80211R + if ((is_reassoc) && (rtw_ft_roam(padapter))) { + rtw_ft_update_rsnie(padapter, _TRUE, pattrib, &pframe); + } else +#endif + { +#ifdef CONFIG_IOCTL_CFG80211 + if (rtw_sec_chk_auth_alg(padapter, WLAN_AUTH_OPEN) && + rtw_sec_chk_auth_type(padapter, MLME_AUTHTYPE_SAE)) { + s32 entry = rtw_cached_pmkid(padapter, pmlmepriv->assoc_bssid); + + rtw_rsn_sync_pmkid(padapter, (u8 *)pIE, (pIE->Length + 2), entry); + } +#endif /* CONFIG_IOCTL_CFG80211 */ + + pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen)); + /* tmp: update rsn's spp related opt. */ + rtw_set_spp_amsdu_mode(padapter->registrypriv.amsdu_mode, pframe - (pIE->Length + 2), pIE->Length +2); + } + break; +#ifdef CONFIG_80211N_HT + case EID_HTCapability: + if (padapter->mlmepriv.htpriv.ht_option == _TRUE) { + if (!(is_ap_in_tkip(padapter))) { + _rtw_memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element)); + + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info); + + pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen)); + } + } + break; + + case EID_EXTCapability: + if (padapter->mlmepriv.htpriv.ht_option == _TRUE) + pframe = rtw_set_ie(pframe, EID_EXTCapability, pIE->Length, pIE->data, &(pattrib->pktlen)); + break; +#endif /* CONFIG_80211N_HT */ +#ifdef CONFIG_80211AC_VHT + case EID_VHTCapability: + if (padapter->mlmepriv.vhtpriv.vht_option == _TRUE) + pframe = rtw_set_ie(pframe, EID_VHTCapability, pIE->Length, pIE->data, &(pattrib->pktlen)); + break; + + case EID_OpModeNotification: + if (padapter->mlmepriv.vhtpriv.vht_option == _TRUE) + pframe = rtw_set_ie(pframe, EID_OpModeNotification, pIE->Length, pIE->data, &(pattrib->pktlen)); + break; +#endif /* CONFIG_80211AC_VHT */ + default: + break; + } + + i += (pIE->Length + 2); + } +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (padapter->tbtx_capability == _TRUE) + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 8 , REALTEK_TBTX_IE, &(pattrib->pktlen)); +#endif + + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen)); + + +#ifdef CONFIG_WAPI_SUPPORT + rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib); +#endif + + +#ifdef CONFIG_P2P + +#ifdef CONFIG_IOCTL_CFG80211 + if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) { + if (pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len > 0) { + _rtw_memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len); + pframe += pmlmepriv->p2p_assoc_req_ie_len; + pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len; + } + } else +#endif /* CONFIG_IOCTL_CFG80211 */ + { + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) { + /* Should add the P2P IE in the association request frame. */ + /* P2P OUI */ + + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20101109 */ + /* According to the P2P Specification, the association request frame should contain 3 P2P attributes */ + /* 1. P2P Capability */ + /* 2. Extended Listen Timing */ + /* 3. Device Info */ + /* Commented by Albert 20110516 */ + /* 4. P2P Interface */ + + /* P2P Capability */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT; + else + p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT; + + /* Extended Listen Timing */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF); + p2pielen += 2; + + /* Device Info */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + if ((pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) || + (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)) + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); + else + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + *(u32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + /* P2P Interface */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_INTERFACE; + + /* Length: */ + *(u16 *)(p2pie + p2pielen) = cpu_to_le16(0x000D); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); /* P2P Device Address */ + p2pielen += ETH_ALEN; + + p2pie[p2pielen++] = 1; /* P2P Interface Address Count */ + + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); /* P2P Interface Address List */ + p2pielen += ETH_ALEN; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen); + } + } + +#ifdef CONFIG_WFD + wfdielen = rtw_append_assoc_req_wfd_ie(padapter, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif +#endif /* CONFIG_P2P */ + +#ifdef CONFIG_RTW_MULTI_AP + if (padapter->multi_ap) + pframe = rtw_set_multi_ap_ie_ext(pframe, &pattrib->pktlen, padapter->multi_ap); +#endif + + /* OWE */ + { + u32 owe_ie_len; + + owe_ie_len = rtw_append_assoc_req_owe_ie(padapter, pframe); + pframe += owe_ie_len; + pattrib->pktlen += owe_ie_len; + } + +#ifdef CONFIG_RTW_REPEATER_SON + rtw_rson_append_ie(padapter, pframe, &pattrib->pktlen); +#endif +#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE + pattrib->pktlen += rtw_build_vendor_ie(padapter , &pframe , WIFI_ASSOCREQ_VENDOR_IE_BIT); +#endif + +#ifdef CONFIG_RTL8812A + pframe = rtw_hal_set_8812a_vendor_ie(padapter, pframe, &pattrib->pktlen ); +#endif/*CONFIG_RTL8812A*/ + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(padapter, pmgntframe); + + ret = _SUCCESS; + +exit: + if (ret == _SUCCESS) { + rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen); + #ifdef CONFIG_RTW_WNM + if (is_reassoc == _TRUE) + rtw_wnm_update_reassoc_req_ie(padapter); + #endif + } else + rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len); + + return; +} + +void issue_assocreq(_adapter *padapter) +{ + _issue_assocreq(padapter, _FALSE); +} + +void issue_reassocreq(_adapter *padapter) +{ + _issue_assocreq(padapter, _TRUE); +} + +/* when wait_ack is ture, this function shoule be called at process context */ +static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv; + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + u8 a4_shift; + + /* RTW_INFO("%s:%d\n", __FUNCTION__, power_mode); */ + + if (!padapter) + goto exit; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + pxmitpriv = &(padapter->xmitpriv); + pmlmeext = &(padapter->mlmeextpriv); + pmlmeinfo = &(pmlmeext->mlmext_info); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->retry_ctrl = _FALSE; + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + if (MLME_IS_AP(padapter)) + SetFrDs(fctrl); + else if (MLME_IS_STA(padapter)) + SetToDs(fctrl); + else if (MLME_IS_MESH(padapter)) { + SetToDs(fctrl); + SetFrDs(fctrl); + } + + if (power_mode) + SetPwrMgt(fctrl); + + if (get_tofr_ds(fctrl) == 3) { + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr4, adapter_mac_addr(padapter), ETH_ALEN); + a4_shift = ETH_ALEN; + pattrib->hdrlen += ETH_ALEN; + } else { + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + a4_shift = 0; + } + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_DATA_NULL); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr) + a4_shift; + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr) + a4_shift; + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +/* + * When wait_ms > 0, this function should be called at process context + * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + * try_cnt means the maximal TX count to try + * da == NULL for station mode + */ +int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + /* da == NULL, assum it's null data for sta to ap */ + if (da == NULL) + da = get_my_bssid(&(pmlmeinfo->network)); + + do { + ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0 ? _TRUE : _FALSE); + + i++; + + if (RTW_CANNOT_RUN(padapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (da) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +/* when wait_ack is ture, this function shoule be called at process context */ +static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, u8 ps, int wait_ack) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl, *qc; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 a4_shift; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + /* RTW_INFO("%s\n", __FUNCTION__); */ + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + pattrib->hdrlen += 2; + pattrib->qos_en = _TRUE; + pattrib->eosp = 1; + pattrib->ack_policy = 0; + pattrib->mdata = 0; + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + if (MLME_IS_AP(padapter)) + SetFrDs(fctrl); + else if (MLME_IS_STA(padapter)) + SetToDs(fctrl); + else if (MLME_IS_MESH(padapter)) { + SetToDs(fctrl); + SetFrDs(fctrl); + } + + if (ps) + SetPwrMgt(fctrl); + + if (pattrib->mdata) + SetMData(fctrl); + + if (get_tofr_ds(fctrl) == 3) { + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr4, adapter_mac_addr(padapter), ETH_ALEN); + a4_shift = ETH_ALEN; + pattrib->hdrlen += ETH_ALEN; + } else { + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + a4_shift = 0; + } + + qc = (unsigned short *)(pframe + pattrib->hdrlen - 2); + + SetPriority(qc, tid); + + SetEOSP(qc, pattrib->eosp); + + SetAckpolicy(qc, pattrib->ack_policy); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_QOS_DATA_NULL); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos) + a4_shift; + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos) + a4_shift; + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +/* + * when wait_ms >0 , this function should be called at process context + * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + * try_cnt means the maximal TX count to try + * da == NULL for station mode + */ +int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, u8 ps, int try_cnt, int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + /* da == NULL, assum it's null data for sta to ap*/ + if (da == NULL) + da = get_my_bssid(&(pmlmeinfo->network)); + + do { + ret = _issue_qos_nulldata(padapter, da, tid, ps, wait_ms > 0 ? _TRUE : _FALSE); + + i++; + + if (RTW_CANNOT_RUN(padapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (da) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack, u8 key_type) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + int ret = _FAIL; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ + + /* RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */ + +#ifdef CONFIG_P2P + if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) && (pwdinfo->rx_invitereq_info.scan_op_ch_only)) { + _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey); + _set_timer(&pwdinfo->reset_ch_sitesurvey, 10); + } +#endif /* CONFIG_P2P */ + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->retry_ctrl = _FALSE; + pattrib->key_type = key_type; + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_DEAUTH); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + reason = cpu_to_le16(reason); + pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&reason, &(pattrib->pktlen)); + + pattrib->last_txcmdsz = pattrib->pktlen; + + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; +} + +int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason) +{ + RTW_INFO("%s reason(%u) to "MAC_FMT"\n", __func__, reason, MAC_ARG(da)); + return _issue_deauth(padapter, da, reason, _FALSE, IEEE80211W_RIGHT_KEY); +} + +#ifdef CONFIG_IEEE80211W +int issue_deauth_11w(_adapter *padapter, unsigned char *da, unsigned short reason, u8 key_type) +{ + RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); + return _issue_deauth(padapter, da, reason, _FALSE, key_type); +} +#endif /* CONFIG_IEEE80211W */ + +/* + * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + * try_cnt means the maximal TX count to try + */ +int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt, + int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + do { + ret = _issue_deauth(padapter, da, reason, wait_ms > 0 ? _TRUE : _FALSE, IEEE80211W_RIGHT_KEY); + + i++; + + if (RTW_CANNOT_RUN(padapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (da) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + RTW_INFO(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n", + FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */ + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); /* TA */ + _rtw_memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */ + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* category, action */ + { + u8 category, action; + category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT; + action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH; + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + } + + pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0); + pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen), + hal_ch_offset_to_secondary_ch_offset(ch_offset)); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + +} + +#ifdef CONFIG_IEEE80211W +void issue_action_SA_Query(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid, u8 key_type) +{ + u8 category = RTW_WLAN_CATEGORY_SA_QUERY; + u16 reason_code; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + u8 *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + u16 *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + RTW_INFO("%s, %04x\n", __FUNCTION__, tid); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_INFO("%s: alloc_mgtxmitframe fail\n", __FUNCTION__); + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + pattrib->key_type = key_type; + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + if (raddr) + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + else + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen); + + switch (action) { + case 0: /* SA Query req */ + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen); + pmlmeext->sa_query_seq++; + /* send sa query request to AP, AP should reply sa query response in 1 second */ + if (pattrib->key_type == IEEE80211W_RIGHT_KEY) { + psta = rtw_get_stainfo(pstapriv, pwlanhdr->addr1); + if (psta != NULL) { + /* RTW_INFO("%s, %d, set dot11w_expire_timer\n", __func__, __LINE__); */ + _set_timer(&psta->dot11w_expire_timer, 1000); + } + } + break; + + case 1: /* SA Query rsp */ + tid = cpu_to_le16(tid); + /* RTW_INFO("rtw_set_fixed_ie, %04x\n", tid); */ + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&tid, &pattrib->pktlen); + break; + default: + break; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); +} +#endif /* CONFIG_IEEE80211W */ + +/** + * issue_action_ba - internal function to TX Block Ack action frame + * @padapter: the adapter to TX + * @raddr: receiver address + * @action: Block Ack Action + * @tid: tid + * @size: the announced AMPDU buffer size. used by ADDBA_RESP + * @status: status/reason code. used by ADDBA_RESP, DELBA + * @initiator: if we are the initiator of AMPDU association. used by DELBA + * @wait_ack: used xmit ack + * + * Returns: + * _SUCCESS: No xmit ack is used or acked + * _FAIL: not acked when using xmit ack + */ +static int issue_action_ba(_adapter *padapter, unsigned char *raddr, unsigned char action + , u8 tid, u8 size, u16 status, u8 initiator, int wait_ack) +{ + int ret = _FAIL; + u8 category = RTW_WLAN_CATEGORY_BACK; + u16 start_seq; + u16 BA_para_set; + u16 BA_timeout_value; + u16 BA_starting_seqctrl; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + u8 *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + u16 *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + struct registry_priv *pregpriv = &padapter->registrypriv; + +#ifdef CONFIG_80211N_HT + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + /* _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */ + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + if (category == 3) { + switch (action) { + case RTW_WLAN_ACTION_ADDBA_REQ: + do { + pmlmeinfo->dialogToken++; + } while (pmlmeinfo->dialogToken == 0); + pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen)); + +#if defined(CONFIG_RTL8188E) && defined(CONFIG_SDIO_HCI) + BA_para_set = (0x0802 | ((tid & 0xf) << 2)); /* immediate ack & 16 buffer size */ +#else + BA_para_set = (0x1002 | ((tid & 0xf) << 2)); /* immediate ack & 64 buffer size */ +#endif + +#ifdef CONFIG_TX_AMSDU + if (padapter->tx_amsdu >= 1) /* TX AMSDU enabled */ + BA_para_set |= BIT(0); + else /* TX AMSDU disabled */ + BA_para_set &= ~BIT(0); +#endif + + psta = rtw_get_stainfo(pstapriv, raddr); + if (psta != NULL) { + if (psta->flags & WLAN_STA_AMSDU_DISABLE) + BA_para_set &= ~BIT(0); + } + BA_para_set = cpu_to_le16(BA_para_set); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen)); + + /* BA_timeout_value = 0xffff; */ /* max: 65535 TUs(~ 65 ms) */ + BA_timeout_value = 5000;/* ~ 5ms */ + BA_timeout_value = cpu_to_le16(BA_timeout_value); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_timeout_value)), &(pattrib->pktlen)); + + /* if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL) */ + psta = rtw_get_stainfo(pstapriv, raddr); + if (psta != NULL) { + start_seq = (psta->sta_xmitpriv.txseq_tid[tid & 0x07] & 0xfff) + 1; + + RTW_INFO("BA_starting_seqctrl = %d for TID=%d\n", start_seq, tid & 0x07); + + psta->BA_starting_seqctrl[tid & 0x07] = start_seq; + + BA_starting_seqctrl = start_seq << 4; + } + + BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_starting_seqctrl)), &(pattrib->pktlen)); + break; + + case RTW_WLAN_ACTION_ADDBA_RESP: + pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen)); + status = cpu_to_le16(status); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen)); + + BA_para_set = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set); + + BA_para_set &= ~IEEE80211_ADDBA_PARAM_TID_MASK; + BA_para_set |= (tid << 2) & IEEE80211_ADDBA_PARAM_TID_MASK; + + BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + BA_para_set |= (size << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK; + + if (!padapter->registrypriv.wifi_spec) { + if (pregpriv->rx_ampdu_amsdu == 0) /* disabled */ + BA_para_set &= ~BIT(0); + else if (pregpriv->rx_ampdu_amsdu == 1) /* enabled */ + BA_para_set |= BIT(0); + } + + psta = rtw_get_stainfo(pstapriv, raddr); + if (psta != NULL) { + if (psta->flags & WLAN_STA_AMSDU_DISABLE) + BA_para_set &= ~BIT(0); + } + + BA_para_set = cpu_to_le16(BA_para_set); + + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen)); + break; + + case RTW_WLAN_ACTION_DELBA: + BA_para_set = 0; + BA_para_set |= (tid << 12) & IEEE80211_DELBA_PARAM_TID_MASK; + BA_para_set |= (initiator << 11) & IEEE80211_DELBA_PARAM_INITIATOR_MASK; + + BA_para_set = cpu_to_le16(BA_para_set); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen)); + status = cpu_to_le16(status); + pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(status)), &(pattrib->pktlen)); + break; + default: + break; + } + } + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: +#endif /* CONFIG_80211N_HT */ + return ret; +} + +/** + * issue_addba_req - TX ADDBA_REQ + * @adapter: the adapter to TX + * @ra: receiver address + * @tid: tid + */ +inline void issue_addba_req(_adapter *adapter, unsigned char *ra, u8 tid) +{ + issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_REQ + , tid + , 0 /* unused */ + , 0 /* unused */ + , 0 /* unused */ + , _FALSE + ); + RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" tid=%u\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), tid); + +} + +/** + * issue_addba_rsp - TX ADDBA_RESP + * @adapter: the adapter to TX + * @ra: receiver address + * @tid: tid + * @status: status code + * @size: the announced AMPDU buffer size + */ +inline void issue_addba_rsp(_adapter *adapter, unsigned char *ra, u8 tid, u16 status, u8 size) +{ + issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_RESP + , tid + , size + , status + , 0 /* unused */ + , _FALSE + ); + RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" status=%u, tid=%u, size=%u\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), status, tid, size); +} + +/** + * issue_addba_rsp_wait_ack - TX ADDBA_RESP and wait ack + * @adapter: the adapter to TX + * @ra: receiver address + * @tid: tid + * @status: status code + * @size: the announced AMPDU buffer size + * @try_cnt: the maximal TX count to try + * @wait_ms: == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + */ +inline u8 issue_addba_rsp_wait_ack(_adapter *adapter, unsigned char *ra, u8 tid, u16 status, u8 size, int try_cnt, int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(adapter))) + goto exit; + + do { + ret = issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_RESP + , tid + , size + , status + , 0 /* unused */ + , _TRUE + ); + + i++; + + if (RTW_CANNOT_RUN(adapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + /* goto exit; */ +#endif + } + + if (try_cnt && wait_ms) { + RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" status:=%u tid=%u size:%u%s, %d/%d in %u ms\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), status, tid, size + , ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } + +exit: + return ret; +} + +/** + * issue_del_ba - TX DELBA + * @adapter: the adapter to TX + * @ra: receiver address + * @tid: tid + * @reason: reason code + * @initiator: if we are the initiator of AMPDU association. used by DELBA + */ +inline void issue_del_ba(_adapter *adapter, unsigned char *ra, u8 tid, u16 reason, u8 initiator) +{ + issue_action_ba(adapter, ra, RTW_WLAN_ACTION_DELBA + , tid + , 0 /* unused */ + , reason + , initiator + , _FALSE + ); + RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" reason=%u, tid=%u, initiator=%u\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), reason, tid, initiator); +} + +/** + * issue_del_ba_ex - TX DELBA with xmit ack options + * @adapter: the adapter to TX + * @ra: receiver address + * @tid: tid + * @reason: reason code + * @initiator: if we are the initiator of AMPDU association. used by DELBA + * @try_cnt: the maximal TX count to try + * @wait_ms: == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + */ +int issue_del_ba_ex(_adapter *adapter, unsigned char *ra, u8 tid, u16 reason, u8 initiator + , int try_cnt, int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(adapter))) + goto exit; + + do { + ret = issue_action_ba(adapter, ra, RTW_WLAN_ACTION_DELBA + , tid + , 0 /* unused */ + , reason + , initiator + , wait_ms > 0 ? _TRUE : _FALSE + ); + + i++; + + if (RTW_CANNOT_RUN(adapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + /* goto exit; */ +#endif + } + + if (try_cnt && wait_ms) { + RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" reason=%u, tid=%u, initiator=%u%s, %d/%d in %u ms\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), reason, tid, initiator + , ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +void issue_action_BSSCoexistPacket(_adapter *padapter) +{ + _irqL irqL; + _list *plist, *phead; + unsigned char category, action; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct wlan_network *pnetwork = NULL; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + _queue *queue = &(pmlmepriv->scanned_queue); + u8 InfoContent[16] = {0}; + u8 ICS[8][15]; +#ifdef CONFIG_80211N_HT + if ((pmlmepriv->num_FortyMHzIntolerant == 0) && (pmlmepriv->num_sta_no_ht == 0)) + return; + + if (_TRUE == pmlmeinfo->bwmode_updated) + return; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return; + + RTW_INFO("%s\n", __FUNCTION__); + + + category = RTW_WLAN_CATEGORY_PUBLIC; + action = ACT_PUBLIC_BSSCOEXIST; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + /* TODO calculate 40Mhz intolerant via ch and ch offset */ + /* if (pmlmepriv->num_FortyMHzIntolerant > 0) */ + { + u8 iedata = 0; + + iedata |= BIT(2);/* 20 MHz BSS Width Request */ + pframe = rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen)); + } + + /* */ + _rtw_memset(ICS, 0, sizeof(ICS)); + if (pmlmepriv->num_sta_no_ht > 0) { + int i; + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + phead = get_list_head(queue); + plist = get_next(phead); + + while (1) { + int len; + u8 *p; + WLAN_BSSID_EX *pbss_network; + + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + + plist = get_next(plist); + + pbss_network = (WLAN_BSSID_EX *)&pnetwork->network; + + p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_); + if ((p == NULL) || (len == 0)) { /* non-HT */ + if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14)) + continue; + + ICS[0][pbss_network->Configuration.DSConfig] = 1; + + if (ICS[0][0] == 0) + ICS[0][0] = 1; + } + + } + + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + + for (i = 0; i < 8; i++) { + if (ICS[i][0] == 1) { + int j, k = 0; + + InfoContent[k] = i; + /* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i); */ + k++; + + for (j = 1; j <= 14; j++) { + if (ICS[i][j] == 1) { + if (k < 16) { + InfoContent[k] = j; /* channel number */ + /* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */ + k++; + } + } + } + + pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen)); + + } + + } + + + } + + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); +#endif /* CONFIG_80211N_HT */ +} + +/* Spatial Multiplexing Powersave (SMPS) action frame */ +int _issue_action_SM_PS(_adapter *padapter , unsigned char *raddr , u8 NewMimoPsMode , u8 wait_ack) +{ + + int ret = _FAIL; + unsigned char category = RTW_WLAN_CATEGORY_HT; + u8 action = RTW_WLAN_ACTION_HT_SM_PS; + u8 sm_power_control = 0; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + + if (NewMimoPsMode == WLAN_HT_CAP_SM_PS_DISABLED) { + sm_power_control = sm_power_control & ~(BIT(0)); /* SM Power Save Enable = 0 SM Power Save Disable */ + } else if (NewMimoPsMode == WLAN_HT_CAP_SM_PS_STATIC) { + sm_power_control = sm_power_control | BIT(0); /* SM Power Save Enable = 1 SM Power Save Enable */ + sm_power_control = sm_power_control & ~(BIT(1)); /* SM Mode = 0 Static Mode */ + } else if (NewMimoPsMode == WLAN_HT_CAP_SM_PS_DYNAMIC) { + sm_power_control = sm_power_control | BIT(0); /* SM Power Save Enable = 1 SM Power Save Enable */ + sm_power_control = sm_power_control | BIT(1); /* SM Mode = 1 Dynamic Mode */ + } else + return ret; + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + return ret; + + RTW_INFO("%s, sm_power_control=%u, NewMimoPsMode=%u\n", __FUNCTION__ , sm_power_control , NewMimoPsMode); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return ret; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); /* RA */ + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); /* TA */ + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); /* DA = RA */ + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* category, action */ + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + pframe = rtw_set_fixed_ie(pframe, 1, &(sm_power_control), &(pattrib->pktlen)); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + + if (ret != _SUCCESS) + RTW_INFO("%s, ack to\n", __func__); + + return ret; +} + +/* + * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + * try_cnt means the maximal TX count to try + */ +int issue_action_SM_PS_wait_ack(_adapter *padapter, unsigned char *raddr, u8 NewMimoPsMode, int try_cnt, int wait_ms) +{ + int ret = _FAIL; + int i = 0; + systime start = rtw_get_current_time(); + + if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter))) + goto exit; + + do { + ret = _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , wait_ms > 0 ? _TRUE : _FALSE); + + i++; + + if (RTW_CANNOT_RUN(padapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0))); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (raddr) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", %s , %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(raddr), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", %s , %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + + return ret; +} + +int issue_action_SM_PS(_adapter *padapter , unsigned char *raddr , u8 NewMimoPsMode) +{ + RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(raddr)); + return _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , _FALSE); +} + +/** + * _send_delba_sta_tid - Cancel the AMPDU association for the specific @sta, @tid + * @adapter: the adapter to which @sta belongs + * @initiator: if we are the initiator of AMPDU association + * @sta: the sta to be checked + * @tid: the tid to be checked + * @force: cancel and send DELBA even when no AMPDU association is setup + * @wait_ack: send delba with xmit ack (valid when initiator == 0) + * + * Returns: + * _FAIL if sta is NULL + * when initiator is 1, always _SUCCESS + * when initiator is 0, _SUCCESS if DELBA is acked + */ +static unsigned int _send_delba_sta_tid(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid + , u8 force, int wait_ack) +{ + int ret = _SUCCESS; + + if (sta == NULL) { + ret = _FAIL; + goto exit; + } + + if (initiator == 0) { + /* recipient */ + if (force || sta->recvreorder_ctrl[tid].enable == _TRUE) { + u8 ampdu_size_bak = sta->recvreorder_ctrl[tid].ampdu_size; + + sta->recvreorder_ctrl[tid].enable = _FALSE; + sta->recvreorder_ctrl[tid].ampdu_size = RX_AMPDU_SIZE_INVALID; + + if (rtw_del_rx_ampdu_test_trigger_no_tx_fail()) + ret = _FAIL; + else if (wait_ack) + ret = issue_del_ba_ex(adapter, sta->cmn.mac_addr, tid, 37, initiator, 3, 1); + else + issue_del_ba(adapter, sta->cmn.mac_addr, tid, 37, initiator); + + if (ret == _FAIL && sta->recvreorder_ctrl[tid].enable == _FALSE) + sta->recvreorder_ctrl[tid].ampdu_size = ampdu_size_bak; + } + } else if (initiator == 1) { + /* originator */ +#ifdef CONFIG_80211N_HT + if (force || sta->htpriv.agg_enable_bitmap & BIT(tid)) { + sta->htpriv.agg_enable_bitmap &= ~BIT(tid); + sta->htpriv.candidate_tid_bitmap &= ~BIT(tid); + issue_del_ba(adapter, sta->cmn.mac_addr, tid, 37, initiator); + } +#endif + } + +exit: + return ret; +} + +inline unsigned int send_delba_sta_tid(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid + , u8 force) +{ + return _send_delba_sta_tid(adapter, initiator, sta, tid, force, 0); +} + +inline unsigned int send_delba_sta_tid_wait_ack(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid + , u8 force) +{ + return _send_delba_sta_tid(adapter, initiator, sta, tid, force, 1); +} + +unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u16 tid; + + if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE) + if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) + return _SUCCESS; + + psta = rtw_get_stainfo(pstapriv, addr); + if (psta == NULL) + return _SUCCESS; + +#if 0 + RTW_INFO("%s:%s\n", __func__, (initiator == 0) ? "RX_DIR" : "TX_DIR"); + if (initiator == 1) /* originator */ + RTW_INFO("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap); +#endif + + for (tid = 0; tid < TID_NUM; tid++) + send_delba_sta_tid(padapter, initiator, psta, tid, 0); + + return _SUCCESS; +} + +#ifdef CONFIG_AP_MODE +unsigned int send_beacon(_adapter *padapter) +{ +#if defined(CONFIG_PCI_HCI) && !defined(CONFIG_PCI_BCN_POLLING) + #ifdef CONFIG_FW_HANDLE_TXBCN + u8 vap_id = padapter->vap_id; + + /* bypass TX BCN because vap_id is invalid*/ + if (vap_id == CONFIG_LIMITED_AP_NUM) + return _SUCCESS; + #endif + + /* bypass TX BCN queue because op ch is switching/waiting */ + if (check_fwstate(&padapter->mlmepriv, WIFI_OP_CH_SWITCHING) + || IS_CH_WAITING(adapter_to_rfctl(padapter)) + ) + return _SUCCESS; + + /* RTW_INFO("%s\n", __FUNCTION__); */ + + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL); + + /* 8192EE Port select for Beacon DL */ + rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL); + #ifdef CONFIG_FW_HANDLE_TXBCN + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_HEAD_SEL, &vap_id); + #endif + + issue_beacon(padapter, 0); + + #ifdef CONFIG_FW_HANDLE_TXBCN + vap_id = 0xFF; + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_HEAD_SEL, &vap_id); + #endif + + #ifdef RTL8814AE_SW_BCN + if (GET_HAL_DATA(padapter)->bCorrectBCN != 0) + RTW_INFO("%s, line%d, Warnning, pHalData->bCorrectBCN != 0\n", __func__, __LINE__); + GET_HAL_DATA(padapter)->bCorrectBCN = 1; + #endif + + return _SUCCESS; +#endif + +/* CONFIG_PCI_BCN_POLLING is for pci interface beacon polling mode */ +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)|| defined(CONFIG_PCI_BCN_POLLING) + u8 bxmitok = _FALSE; + int issue = 0; + int poll = 0; + systime start = rtw_get_current_time(); + #ifdef CONFIG_FW_HANDLE_TXBCN + u8 vap_id = padapter->vap_id; + + /* bypass TX BCN because vap_id is invalid*/ + if (vap_id == CONFIG_LIMITED_AP_NUM) + return _SUCCESS; + #endif + + /* bypass TX BCN queue because op ch is switching/waiting */ + if (check_fwstate(&padapter->mlmepriv, WIFI_OP_CH_SWITCHING) + || IS_CH_WAITING(adapter_to_rfctl(padapter)) + ) + return _SUCCESS; + + #if defined(CONFIG_USB_HCI) + #if defined(CONFIG_RTL8812A) + if (IS_FULL_SPEED_USB(padapter)) { + issue_beacon(padapter, 300); + bxmitok = _TRUE; + } else + #endif + #endif + { + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL); + rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL); + #ifdef CONFIG_FW_HANDLE_TXBCN + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_HEAD_SEL, &vap_id); + #endif + do { + #if defined(CONFIG_PCI_BCN_POLLING) + issue_beacon(padapter, 0); + #else + issue_beacon(padapter, 100); + #endif + issue++; + do { + #if defined(CONFIG_PCI_BCN_POLLING) + rtw_msleep_os(1); + #else + rtw_yield_os(); + #endif + rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok)); + poll++; + } while ((poll % 10) != 0 && _FALSE == bxmitok && !RTW_CANNOT_RUN(padapter)); + #if defined(CONFIG_PCI_BCN_POLLING) + rtw_hal_unmap_beacon_icf(padapter); + #endif + } while (bxmitok == _FALSE && (issue < 100) && !RTW_CANNOT_RUN(padapter)); + #ifdef CONFIG_FW_HANDLE_TXBCN + vap_id = 0xFF; + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_HEAD_SEL, &vap_id); + #endif + } + if (RTW_CANNOT_RUN(padapter)) + return _FAIL; + + + if (_FALSE == bxmitok) { + RTW_INFO("%s fail! %u ms\n", __FUNCTION__, rtw_get_passing_time_ms(start)); + #ifdef CONFIG_BCN_RECOVERY + GET_HAL_DATA(padapter)->issue_bcn_fail++; + #endif /*CONFIG_BCN_RECOVERY*/ + return _FAIL; + } else { + u32 passing_time = rtw_get_passing_time_ms(start); + + if (passing_time > 100 || issue > 3) + RTW_INFO("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start)); + else if (0) + RTW_INFO("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start)); + + #ifdef CONFIG_FW_CORRECT_BCN + rtw_hal_fw_correct_bcn(padapter); + #endif + return _SUCCESS; + } + +#endif /*defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)*/ + +} +#endif /* CONFIG_AP_MODE */ + +/**************************************************************************** + +Following are some utitity fuctions for WiFi MLME + +*****************************************************************************/ + +BOOLEAN IsLegal5GChannel( + PADAPTER Adapter, + u8 channel) +{ + + int i = 0; + u8 Channel_5G[45] = {36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, + 60, 62, 64, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122, + 124, 126, 128, 130, 132, 134, 136, 138, 140, 149, 151, 153, 155, 157, 159, + 161, 163, 165 + }; + for (i = 0; i < sizeof(Channel_5G); i++) + if (channel == Channel_5G[i]) + return _TRUE; + return _FALSE; +} + +/* collect bss info from Beacon and Probe request/response frames. */ +u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid) +{ + int i; + sint len; + u8 *p; + u8 rf_path; + u16 val16, subtype; + u8 *pframe = precv_frame->u.hdr.rx_data; + u32 packet_len = precv_frame->u.hdr.len; + u8 ie_offset; + struct hal_spec_t *hal_spec = GET_HAL_SPEC(padapter); + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); +#ifdef CONFIG_LAYER2_ROAMING + u32 *pbuf; +#endif + + len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr); + + if (len > MAX_IE_SZ) { + /* RTW_INFO("IE too long for survey event\n"); */ + return _FAIL; + } + + _rtw_memset(bssid, 0, sizeof(WLAN_BSSID_EX)); + + subtype = get_frame_sub_type(pframe); + + if (subtype == WIFI_BEACON) { + bssid->Reserved[0] = BSS_TYPE_BCN; + ie_offset = _BEACON_IE_OFFSET_; + } else { + /* FIXME : more type */ + if (subtype == WIFI_PROBERSP) { + ie_offset = _PROBERSP_IE_OFFSET_; + bssid->Reserved[0] = BSS_TYPE_PROB_RSP; + } else if (subtype == WIFI_PROBEREQ) { + ie_offset = _PROBEREQ_IE_OFFSET_; + bssid->Reserved[0] = BSS_TYPE_PROB_REQ; + } else { + bssid->Reserved[0] = BSS_TYPE_UNDEF; + ie_offset = _FIXED_IE_LENGTH_; + } + } + + bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len; + + /* below is to copy the information element */ + bssid->IELength = len; + _rtw_memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength); + + /* get the signal strength */ + /* bssid->Rssi = precv_frame->u.hdr.attrib.SignalStrength; */ /* 0-100 index. */ + bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.recv_signal_power; /* in dBM.raw data */ + bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.signal_quality;/* in percentage */ + bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.signal_strength;/* in percentage */ + + /* get rx_snr */ + if (precv_frame->u.hdr.attrib.data_rate >= DESC_RATE11M) { + bssid->PhyInfo.is_cck_rate = 0; + for (rf_path = 0; rf_path < hal_spec->rf_reg_path_num; rf_path++) + bssid->PhyInfo.rx_snr[rf_path] = + precv_frame->u.hdr.attrib.phy_info.rx_snr[rf_path]; + } else + bssid->PhyInfo.is_cck_rate = 1; + +#ifdef CONFIG_ANTENNA_DIVERSITY + rtw_hal_get_odm_var(padapter, HAL_ODM_ANTDIV_SELECT, &(bssid->PhyInfo.Optimum_antenna), NULL); +#endif + + /* checking SSID */ + p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset); + if (p == NULL) { + RTW_INFO("marc: cannot find SSID for survey event\n"); + return _FAIL; + } + + if (*(p + 1)) { + if (len > NDIS_802_11_LENGTH_SSID) { + RTW_INFO("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len); + return _FAIL; + } + _rtw_memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1)); + bssid->Ssid.SsidLength = *(p + 1); + } else + bssid->Ssid.SsidLength = 0; + + _rtw_memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX); + + /* checking rate info... */ + i = 0; + p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset); + if (p != NULL) { + if (len > NDIS_802_11_LENGTH_RATES_EX) { + RTW_INFO("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len); + return _FAIL; + } +#ifdef CONFIG_CHECK_SPECIFIC_IE_CONTENT + if (rtw_validate_value(_SUPPORTEDRATES_IE_, p+2, len) == _FALSE) { + rtw_absorb_ssid_ifneed(padapter, bssid, pframe); + RTW_DBG_DUMP("Invalidated Support Rate IE --", p, len+2); + return _FAIL; + } +#endif /* #ifdef CONFIG_CHECK_SPECIFIC_IE_CONTENT */ + _rtw_memcpy(bssid->SupportedRates, (p + 2), len); + i = len; + } + + p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset); + if (p != NULL) { + if (len > (NDIS_802_11_LENGTH_RATES_EX - i)) { + RTW_INFO("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len); + return _FAIL; + } +#ifdef CONFIG_CHECK_SPECIFIC_IE_CONTENT + if (rtw_validate_value(_EXT_SUPPORTEDRATES_IE_, p+2, len) == _FALSE) { + rtw_absorb_ssid_ifneed(padapter, bssid, pframe); + RTW_DBG_DUMP("Invalidated EXT Support Rate IE --", p, len+2); + return _FAIL; + } +#endif /* #ifdef CONFIG_CHECK_SPECIFIC_IE_CONTENT */ + _rtw_memcpy(bssid->SupportedRates + i, (p + 2), len); + } + +#ifdef CONFIG_P2P + if (subtype == WIFI_PROBEREQ) { + u8 *p2p_ie; + u32 p2p_ielen; + /* Set Listion Channel */ + p2p_ie = rtw_get_p2p_ie(bssid->IEs, bssid->IELength, NULL, &p2p_ielen); + if (p2p_ie) { + u32 attr_contentlen = 0; + u8 listen_ch[5] = { 0x00 }; + + rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, listen_ch, &attr_contentlen); + bssid->Configuration.DSConfig = listen_ch[4]; + } else { + /* use current channel */ + bssid->Configuration.DSConfig = padapter->mlmeextpriv.cur_channel; + RTW_INFO("%s()-%d: Cannot get p2p_ie. set DSconfig to op_ch(%d)\n", __FUNCTION__, __LINE__, bssid->Configuration.DSConfig); + } + + /* FIXME */ + bssid->InfrastructureMode = Ndis802_11Infrastructure; + _rtw_memcpy(bssid->MacAddress, get_addr2_ptr(pframe), ETH_ALEN); + bssid->Privacy = 1; + return _SUCCESS; + } +#endif /* CONFIG_P2P */ + + if (bssid->IELength < 12) + return _FAIL; + + /* Checking for DSConfig */ + p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset); + + bssid->Configuration.DSConfig = 0; + bssid->Configuration.Length = 0; + + if (p) + bssid->Configuration.DSConfig = *(p + 2); + else { + /* In 5G, some ap do not have DSSET IE */ + /* checking HT info for channel */ + p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset); + if (p) { + struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2); + bssid->Configuration.DSConfig = HT_info->primary_channel; + } else { + /* use current channel */ + bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter); + } + } + + _rtw_memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2); + bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod); + + val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid); + + if ((val16 & 0x03) == cap_ESS) { + bssid->InfrastructureMode = Ndis802_11Infrastructure; + _rtw_memcpy(bssid->MacAddress, get_addr2_ptr(pframe), ETH_ALEN); + } else if ((val16 & 0x03) == cap_IBSS){ + bssid->InfrastructureMode = Ndis802_11IBSS; + _rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN); + } else if ((val16 & 0x03) == 0x00){ + u8 *mesh_id_ie, *mesh_conf_ie; + sint mesh_id_ie_len, mesh_conf_ie_len; + + mesh_id_ie = rtw_get_ie(bssid->IEs + ie_offset, WLAN_EID_MESH_ID, &mesh_id_ie_len, bssid->IELength - ie_offset); + mesh_conf_ie = rtw_get_ie(bssid->IEs + ie_offset, WLAN_EID_MESH_CONFIG, &mesh_conf_ie_len, bssid->IELength - ie_offset); + if (mesh_id_ie || mesh_conf_ie) { + if (!mesh_id_ie) { + RTW_INFO("cannot find Mesh ID for survey event\n"); + return _FAIL; + } + if (mesh_id_ie_len) { + if (mesh_id_ie_len > NDIS_802_11_LENGTH_SSID) { + RTW_INFO("Mesh ID too long (%d) for survey event\n", mesh_id_ie_len); + return _FAIL; + } + _rtw_memcpy(bssid->mesh_id.Ssid, (mesh_id_ie + 2), mesh_id_ie_len); + bssid->mesh_id.SsidLength = mesh_id_ie_len; + } else + bssid->mesh_id.SsidLength = 0; + + if (!mesh_conf_ie) { + RTW_INFO("cannot find Mesh config for survey event\n"); + return _FAIL; + } + if (mesh_conf_ie_len != 7) { + RTW_INFO("invalid Mesh conf IE len (%d) for survey event\n", mesh_conf_ie_len); + return _FAIL; + } + + bssid->InfrastructureMode = Ndis802_11_mesh; + _rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN); + } else { + /* default cases */ + bssid->InfrastructureMode = Ndis802_11IBSS; + _rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN); + } + } + + if (val16 & BIT(4)) + bssid->Privacy = 1; + else + bssid->Privacy = 0; + + bssid->Configuration.ATIMWindow = 0; + + /* 20/40 BSS Coexistence check */ + if ((pregistrypriv->wifi_spec == 1) && (_FALSE == pmlmeinfo->bwmode_updated)) { + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; +#ifdef CONFIG_80211N_HT + p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset); + if (p && len > 0) { + struct HT_caps_element *pHT_caps; + pHT_caps = (struct HT_caps_element *)(p + 2); + + if (pHT_caps->u.HT_cap_element.HT_caps_info & BIT(14)) + pmlmepriv->num_FortyMHzIntolerant++; + } else + pmlmepriv->num_sta_no_ht++; +#endif /* CONFIG_80211N_HT */ + + } + +#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1 + if (strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) { + RTW_INFO("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n" + , bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig + , rtw_get_oper_ch(padapter) + , bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi + ); + } +#endif + + /* mark bss info receving from nearby channel as SignalQuality 101 */ + if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter)) + bssid->PhyInfo.SignalQuality = 101; + +#ifdef CONFIG_RTW_80211K + p = rtw_get_ie(bssid->IEs + ie_offset, _EID_RRM_EN_CAP_IE_, &len, bssid->IELength - ie_offset); + if (p) + _rtw_memcpy(bssid->PhyInfo.rm_en_cap, (p + 2), *(p + 1)); + + /* save freerun counter */ + bssid->PhyInfo.free_cnt = precv_frame->u.hdr.attrib.free_cnt; +#endif +#ifdef CONFIG_LAYER2_ROAMING + pbuf = (u32 *)((u8 *)pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + bssid->tsf = le32_to_cpu(*(pbuf + 1)); + bssid->tsf = bssid->tsf << 32; + bssid->tsf |= le32_to_cpu(*pbuf); +#endif + return _SUCCESS; +} + +#ifdef CONFIG_AP_MODE +void start_create_ibss(_adapter *padapter) +{ + unsigned short caps; + u8 val8; + u8 join_type; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); + u8 doiqk = _FALSE; + pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig; + pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork); + + /* update wireless mode */ + update_wireless_mode(padapter); + + /* udpate capability */ + caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork); + update_capinfo(padapter, caps); + if (caps & cap_IBSS) { /* adhoc master */ + /* set_opmode_cmd(padapter, adhoc); */ /* removed */ + + val8 = 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + doiqk = _TRUE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + + /* switch channel */ + set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + + doiqk = _FALSE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + + beacon_timing_control(padapter); + + /* set msr to WIFI_FW_ADHOC_STATE */ + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + Set_MSR(padapter, (pmlmeinfo->state & 0x3)); + + /* issue beacon */ + if (send_beacon(padapter) == _FAIL) { + + report_join_res(padapter, -1, WLAN_STATUS_UNSPECIFIED_FAILURE); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + } else { + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress); + rtw_hal_rcr_set_chk_bssid(padapter, MLME_ADHOC_STARTED); + join_type = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + rtw_btcoex_connect_notify(padapter, join_type); + + report_join_res(padapter, 1, WLAN_STATUS_SUCCESS); + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + rtw_indicate_connect(padapter); + } + } else { + RTW_INFO("start_create_ibss, invalid cap:%x\n", caps); + return; + } + /* update bc/mc sta_info */ + update_bmc_sta(padapter); + +} +#endif /* CONFIG_AP_MODE */ + +void start_clnt_join(_adapter *padapter) +{ + unsigned short caps; + u8 val8; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); + int beacon_timeout; + u8 ASIX_ID[] = {0x00, 0x0E, 0xC6}; + + /* update wireless mode */ + update_wireless_mode(padapter); + + /* udpate capability */ + caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork); + update_capinfo(padapter, caps); + + /* check if sta is ASIX peer and fix IOT issue if it is. */ + if (_rtw_memcmp(get_my_bssid(&pmlmeinfo->network) , ASIX_ID , 3)) { + u8 iot_flag = _TRUE; + rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag)); + } + + if (caps & cap_ESS) { + Set_MSR(padapter, WIFI_FW_STATION_STATE); + + val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf; + +#ifdef CONFIG_WAPI_SUPPORT + if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI) { + /* Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey. */ + val8 = 0x4c; + } +#endif + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + +#ifdef CONFIG_DEAUTH_BEFORE_CONNECT + /* Because of AP's not receiving deauth before */ + /* AP may: 1)not response auth or 2)deauth us after link is complete */ + /* issue deauth before issuing auth to deal with the situation */ + + /* Commented by Albert 2012/07/21 */ + /* For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */ + { +#ifdef CONFIG_P2P + _queue *queue = &(padapter->mlmepriv.scanned_queue); + _list *head = get_list_head(queue); + _list *pos = get_next(head); + struct wlan_network *scanned = NULL; + u8 ie_offset = 0; + _irqL irqL; + bool has_p2p_ie = _FALSE; + + _enter_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL); + + for (pos = get_next(head); !rtw_end_of_queue_search(head, pos); pos = get_next(pos)) { + + scanned = LIST_CONTAINOR(pos, struct wlan_network, list); + + if (_rtw_memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(NDIS_802_11_SSID)) == _TRUE + && _rtw_memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS)) == _TRUE + ) { + ie_offset = (scanned->network.Reserved[0] == BSS_TYPE_PROB_REQ ? 0 : 12); + if (rtw_get_p2p_ie(scanned->network.IEs + ie_offset, scanned->network.IELength - ie_offset, NULL, NULL)) + has_p2p_ie = _TRUE; + break; + } + } + + _exit_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL); + + if (scanned == NULL || rtw_end_of_queue_search(head, pos) || has_p2p_ie == _FALSE) +#endif /* CONFIG_P2P */ + /* To avoid connecting to AP fail during resume process, change retry count from 5 to 1 */ + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100); + } +#endif /* CONFIG_DEAUTH_BEFORE_CONNECT */ + + /* here wait for receiving the beacon to start auth */ + /* and enable a timer */ + beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval); + set_link_timer(pmlmeext, beacon_timeout); + _set_timer(&padapter->mlmepriv.assoc_timer, + (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO * REASSOC_LIMIT) + beacon_timeout); + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam(padapter)) { + rtw_ft_start_clnt_join(padapter); + } else +#endif + { + rtw_sta_linking_test_set_start(); + pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE; + } + } else if (caps & cap_IBSS) { /* adhoc client */ + Set_MSR(padapter, WIFI_FW_ADHOC_STATE); + + val8 = 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + + beacon_timing_control(padapter); + + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + + report_join_res(padapter, 1, WLAN_STATUS_SUCCESS); + } else { + /* RTW_INFO("marc: invalid cap:%x\n", caps); */ + return; + } + +} + +void start_clnt_auth(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + _cancel_timer_ex(&pmlmeext->link_timer); + + pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL); + pmlmeinfo->state |= WIFI_FW_AUTH_STATE; + + pmlmeinfo->auth_seq = 1; + pmlmeinfo->reauth_count = 0; + pmlmeinfo->reassoc_count = 0; + pmlmeinfo->link_count = 0; + pmlmeext->retry = 0; + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam(padapter)) { + rtw_ft_set_status(padapter, RTW_FT_AUTHENTICATING_STA); + RTW_PRINT("start ft auth\n"); + } else +#endif + RTW_PRINT("start auth\n"); + +#ifdef CONFIG_IOCTL_CFG80211 + if (rtw_sec_chk_auth_type(padapter, MLME_AUTHTYPE_SAE)) { + if (rtw_cached_pmkid(padapter, get_my_bssid(&pmlmeinfo->network)) != -1) { + RTW_INFO("SAE: PMKSA cache entry found\n"); + padapter->securitypriv.auth_alg = WLAN_AUTH_OPEN; + goto no_external_auth; + } + + RTW_PRINT("SAE: start external auth\n"); + rtw_cfg80211_external_auth_request(padapter, NULL); + return; + } +no_external_auth: +#endif /* CONFIG_IOCTL_CFG80211 */ + + issue_auth(padapter, NULL, 0); + + set_link_timer(pmlmeext, REAUTH_TO); + +} + + +void start_clnt_assoc(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + _cancel_timer_ex(&pmlmeext->link_timer); + + pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE)); + pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE); + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam(padapter) + #ifdef CONFIG_RTW_WNM + || rtw_wnm_btm_reassoc_req(padapter) + #endif + ) + issue_reassocreq(padapter); + else +#endif + issue_assocreq(padapter); + + set_link_timer(pmlmeext, REASSOC_TO); +} + +unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason, u8 locally_generated) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (!(_rtw_memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN))) + return _SUCCESS; + + RTW_INFO("%s\n", __FUNCTION__); + +#ifdef CONFIG_RTW_REPEATER_SON + rtw_rson_do_disconnect(padapter); +#endif + if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) { + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { + if (report_del_sta_event(padapter, MacAddr, reason, _TRUE, locally_generated) != _FAIL) + pmlmeinfo->state = WIFI_FW_NULL_STATE; + } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) { + if (report_join_res(padapter, -2, reason) != _FAIL) + pmlmeinfo->state = WIFI_FW_NULL_STATE; + } else + RTW_INFO(FUNC_ADPT_FMT" - End to Disconnect\n", FUNC_ADPT_ARG(padapter)); +#ifdef CONFIG_RTW_80211R + rtw_ft_roam_status_reset(padapter); +#endif +#ifdef CONFIG_RTW_WNM + rtw_wnm_reset_btm_state(padapter); +#endif + } + + return _SUCCESS; +} + +static void rtw_hidden_ssid_bss_count(_adapter *adapter, WLAN_BSSID_EX *bss) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + int chset_idx; + + if (bss->InfrastructureMode != Ndis802_11Infrastructure) + return; + + if (!hidden_ssid_ap(bss)) + return; + + chset_idx = rtw_chset_search_ch(chset, bss->Configuration.DSConfig); + if (chset_idx < 0) + return; + + chset[chset_idx].hidden_bss_cnt++; +} + +/**************************************************************************** + +Following are the functions to report events + +*****************************************************************************/ + +void report_survey_event(_adapter *padapter, union recv_frame *precv_frame) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct survey_event *psurvey_evt; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext; + struct cmd_priv *pcmdpriv; + /* u8 *pframe = precv_frame->u.hdr.rx_data; */ + /* uint len = precv_frame->u.hdr.len; */ + + if (!padapter) + return; + + pmlmeext = &padapter->mlmeextpriv; + pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct survey_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + return; + } + + _rtw_init_listhead(&pcmd_obj->list); + + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct survey_event); + evt_hdr->id = EVT_SURVEY; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + + if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + rtw_mfree((u8 *)pevtcmd, cmdsz); + return; + } + + rtw_hidden_ssid_bss_count(padapter, &psurvey_evt->bss); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + pmlmeext->sitesurvey_res.bss_cnt++; + + return; + +} + +/* +* @acs: aim to trigger channel selection +*/ +void report_surveydone_event(_adapter *padapter, bool acs) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct surveydone_event *psurveydone_evt; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct surveydone_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + return; + } + + _rtw_init_listhead(&pcmd_obj->list); + + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct surveydone_event); + evt_hdr->id = EVT_SURVEY_DONE; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + psurveydone_evt = (struct surveydone_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt; + psurveydone_evt->activate_ch_cnt = pmlmeext->sitesurvey_res.activate_ch_cnt; + psurveydone_evt->acs = acs; + + RTW_INFO("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter)); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; + +} + +u32 report_join_res(_adapter *padapter, int aid_res, u16 status) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct joinbss_event *pjoinbss_evt; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u32 ret = _FAIL; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + goto exit; + + cmdsz = (sizeof(struct joinbss_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + goto exit; + } + + _rtw_init_listhead(&pcmd_obj->list); + + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct joinbss_event); + evt_hdr->id = EVT_JOINBSS; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + _rtw_memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX)); + pjoinbss_evt->network.join_res = pjoinbss_evt->network.aid = aid_res; + + RTW_INFO("report_join_res(%d, %u)\n", aid_res, status); + + + rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network, status); + + + ret = rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + +exit: + return ret; +} + +void report_wmm_edca_update(_adapter *padapter) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct wmm_event *pwmm_event; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct wmm_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + return; + } + + _rtw_init_listhead(&pcmd_obj->list); + + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct wmm_event); + evt_hdr->id = EVT_WMM_UPDATE; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + pwmm_event = (struct wmm_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + pwmm_event->wmm = 0; + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; + +} + +u32 report_del_sta_event(_adapter *padapter, unsigned char *MacAddr, unsigned short reason, bool enqueue, u8 locally_generated) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct sta_info *psta; + int mac_id = -1; + struct stadel_event *pdel_sta_evt; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + /* prepare cmd parameter */ + cmdsz = (sizeof(struct stadel_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + res = _FAIL; + goto exit; + } + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct stadel_event); + evt_hdr->id = EVT_DEL_STA; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + _rtw_memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN); + _rtw_memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2); + psta = rtw_get_stainfo(&padapter->stapriv, MacAddr); + if (psta) + mac_id = (int)psta->cmn.mac_id; + else + mac_id = (-1); + pdel_sta_evt->mac_id = mac_id; + pdel_sta_evt->locally_generated = locally_generated; + + if (!enqueue) { + /* do directly */ + rtw_stadel_event_callback(padapter, (u8 *)pdel_sta_evt); + rtw_mfree(pevtcmd, cmdsz); + } else { + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) { + rtw_mfree(pevtcmd, cmdsz); + res = _FAIL; + goto exit; + } + + _rtw_init_listhead(&pcmd_obj->list); + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + res = rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + } + +exit: + + RTW_INFO(FUNC_ADPT_FMT" "MAC_FMT" mac_id=%d, enqueue:%d, res:%u\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(MacAddr), mac_id, enqueue, res); + + return res; +} + +void report_add_sta_event(_adapter *padapter, unsigned char *MacAddr) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct stassoc_event *padd_sta_evt; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct stassoc_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + return; + } + + _rtw_init_listhead(&pcmd_obj->list); + + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct stassoc_event); + evt_hdr->id = EVT_ADD_STA; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + _rtw_memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN); + + RTW_INFO("report_add_sta_event: add STA\n"); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; +} + + +bool rtw_port_switch_chk(_adapter *adapter) +{ + bool switch_needed = _FALSE; +#ifdef CONFIG_CONCURRENT_MODE +#ifdef CONFIG_RUNTIME_PORT_SWITCH + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrctl = dvobj_to_pwrctl(dvobj); + _adapter *if_port0 = NULL; + _adapter *if_port1 = NULL; + struct mlme_ext_info *if_port0_mlmeinfo = NULL; + struct mlme_ext_info *if_port1_mlmeinfo = NULL; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (get_hw_port(dvobj->padapters[i]) == HW_PORT0) { + if_port0 = dvobj->padapters[i]; + if_port0_mlmeinfo = &(if_port0->mlmeextpriv.mlmext_info); + } else if (get_hw_port(dvobj->padapters[i]) == HW_PORT1) { + if_port1 = dvobj->padapters[i]; + if_port1_mlmeinfo = &(if_port1->mlmeextpriv.mlmext_info); + } + } + + if (if_port0 == NULL) { + rtw_warn_on(1); + goto exit; + } + + if (if_port1 == NULL) { + rtw_warn_on(1); + goto exit; + } + +#ifdef DBG_RUNTIME_PORT_SWITCH + RTW_INFO(FUNC_ADPT_FMT" wowlan_mode:%u\n" + ADPT_FMT", port0, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n" + ADPT_FMT", port1, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n", + FUNC_ADPT_ARG(adapter), pwrctl->wowlan_mode, + ADPT_ARG(if_port0), if_port0_mlmeinfo->state, rtw_p2p_state(&if_port0->wdinfo), rtw_p2p_chk_state(&if_port0->wdinfo, P2P_STATE_NONE), + ADPT_ARG(if_port1), if_port1_mlmeinfo->state, rtw_p2p_state(&if_port1->wdinfo), rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE)); +#endif /* DBG_RUNTIME_PORT_SWITCH */ + +#ifdef CONFIG_WOWLAN + /* WOWLAN interface(primary, for now) should be port0 */ + if (pwrctl->wowlan_mode == _TRUE) { + if (!is_primary_adapter(if_port0)) { + RTW_INFO("%s "ADPT_FMT" enable WOWLAN\n", __func__, ADPT_ARG(if_port1)); + switch_needed = _TRUE; + } + goto exit; + } +#endif /* CONFIG_WOWLAN */ + + /* AP/Mesh should use port0 for ctl frame's ack */ + if ((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) { + RTW_INFO("%s "ADPT_FMT" is AP/GO/Mesh\n", __func__, ADPT_ARG(if_port1)); + switch_needed = _TRUE; + goto exit; + } + + /* GC should use port0 for p2p ps */ + if (((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) + && (if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) +#ifdef CONFIG_P2P + && !rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE) +#endif + && !check_fwstate(&if_port1->mlmepriv, WIFI_UNDER_WPS) + ) { + RTW_INFO("%s "ADPT_FMT" is GC\n", __func__, ADPT_ARG(if_port1)); + switch_needed = _TRUE; + goto exit; + } + + /* port1 linked, but port0 not linked */ + if ((if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + && !(if_port0_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) + && ((if_port0_mlmeinfo->state & 0x03) != WIFI_FW_AP_STATE) + ) { + RTW_INFO("%s "ADPT_FMT" is SINGLE_LINK\n", __func__, ADPT_ARG(if_port1)); + switch_needed = _TRUE; + goto exit; + } + +exit: +#ifdef DBG_RUNTIME_PORT_SWITCH + RTW_INFO(FUNC_ADPT_FMT" ret:%d\n", FUNC_ADPT_ARG(adapter), switch_needed); +#endif /* DBG_RUNTIME_PORT_SWITCH */ +#endif /* CONFIG_RUNTIME_PORT_SWITCH */ +#endif /* CONFIG_CONCURRENT_MODE */ + return switch_needed; +} + +/**************************************************************************** + +Following are the event callback functions + +*****************************************************************************/ + +/* for sta/adhoc mode */ +void update_sta_info(_adapter *padapter, struct sta_info *psta) +{ + _irqL irqL; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* ERP */ + VCS_update(padapter, psta); + +#ifdef CONFIG_80211N_HT + /* HT */ + if (pmlmepriv->htpriv.ht_option) { + psta->htpriv.ht_option = _TRUE; + + psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable; + + psta->htpriv.rx_ampdu_min_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & IEEE80211_HT_CAP_AMPDU_DENSITY) >> 2; + + if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20)) + psta->htpriv.sgi_20m = _TRUE; + + if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40)) + psta->htpriv.sgi_40m = _TRUE; + + psta->qos_option = _TRUE; + + psta->htpriv.ldpc_cap = pmlmepriv->htpriv.ldpc_cap; + psta->htpriv.stbc_cap = pmlmepriv->htpriv.stbc_cap; + psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap; + + _rtw_memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct rtw_ieee80211_ht_cap)); + #ifdef CONFIG_BEAMFORMING + psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap; + psta->cmn.bf_info.ht_beamform_cap = pmlmepriv->htpriv.beamform_cap; + #endif + } else +#endif /* CONFIG_80211N_HT */ + { +#ifdef CONFIG_80211N_HT + psta->htpriv.ht_option = _FALSE; + psta->htpriv.ampdu_enable = _FALSE; + psta->htpriv.tx_amsdu_enable = _FALSE; + psta->htpriv.sgi_20m = _FALSE; + psta->htpriv.sgi_40m = _FALSE; +#endif /* CONFIG_80211N_HT */ + psta->qos_option = _FALSE; + + } + +#ifdef CONFIG_80211N_HT + psta->htpriv.ch_offset = pmlmeext->cur_ch_offset; + + psta->htpriv.agg_enable_bitmap = 0x0;/* reset */ + psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */ +#endif /* CONFIG_80211N_HT */ + + psta->cmn.bw_mode = pmlmeext->cur_bwmode; + + /* QoS */ + if (pmlmepriv->qospriv.qos_option) + psta->qos_option = _TRUE; + +#ifdef CONFIG_80211AC_VHT + _rtw_memcpy(&psta->vhtpriv, &pmlmepriv->vhtpriv, sizeof(struct vht_priv)); + if (psta->vhtpriv.vht_option) { + psta->cmn.ra_info.is_vht_enable = _TRUE; + #ifdef CONFIG_BEAMFORMING + psta->vhtpriv.beamform_cap = pmlmepriv->vhtpriv.beamform_cap; + psta->cmn.bf_info.vht_beamform_cap = pmlmepriv->vhtpriv.beamform_cap; + #endif /*CONFIG_BEAMFORMING*/ + } +#endif /* CONFIG_80211AC_VHT */ + psta->cmn.ra_info.is_support_sgi = query_ra_short_GI(psta, rtw_get_tx_bw_mode(padapter, psta)); + update_ldpc_stbc_cap(psta); + + _enter_critical_bh(&psta->lock, &irqL); + psta->state = WIFI_ASOC_STATE; + _exit_critical_bh(&psta->lock, &irqL); + +} + +static void rtw_mlmeext_disconnect(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 self_action = MLME_ACTION_UNKNOWN; + u8 state_backup = (pmlmeinfo->state & 0x03); + u8 ASIX_ID[] = {0x00, 0x0E, 0xC6}; + + if (MLME_IS_AP(padapter)) + self_action = MLME_AP_STOPPED; + else if (MLME_IS_MESH(padapter)) + self_action = MLME_MESH_STOPPED; + else if (MLME_IS_STA(padapter)) + self_action = MLME_STA_DISCONNECTED; + else if (MLME_IS_ADHOC(padapter) || MLME_IS_ADHOC_MASTER(padapter)) + self_action = MLME_ADHOC_STOPPED; + else { + RTW_INFO("state:0x%x\n", MLME_STATE(padapter)); + rtw_warn_on(1); + } + + /* set_opmode_cmd(padapter, infra_client_with_mlme); */ +#ifdef CONFIG_HW_P0_TSF_SYNC + if (self_action == MLME_STA_DISCONNECTED) + correct_TSF(padapter, self_action); +#endif + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0); + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr); + if (self_action == MLME_STA_DISCONNECTED) + rtw_hal_rcr_set_chk_bssid(padapter, self_action); + + /* set MSR to no link state->infra. mode */ + Set_MSR(padapter, _HW_STATE_STATION_); + + /* check if sta is ASIX peer and fix IOT issue if it is. */ + if (_rtw_memcmp(get_my_bssid(&pmlmeinfo->network) , ASIX_ID , 3)) { + u8 iot_flag = _FALSE; + rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag)); + } + pmlmeinfo->state = WIFI_FW_NULL_STATE; + +#ifdef CONFIG_MCC_MODE + /* mcc disconnect setting before download LPS rsvd page */ + rtw_hal_set_mcc_setting_disconnect(padapter); +#endif /* CONFIG_MCC_MODE */ + + if (state_backup == WIFI_FW_STATION_STATE) { + if (rtw_port_switch_chk(padapter) == _TRUE) { + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL); +#ifdef CONFIG_LPS + { + _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter)); + if (port0_iface) + rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, RTW_CMDF_DIRECTLY); + } +#endif + } + } + + /* switch to the 20M Hz mode after disconnect */ + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; +#ifdef CONFIG_CTRL_TXSS_BY_TP + pmlmeext->txss_1ss = _FALSE; +#endif + +#ifdef CONFIG_FCS_MODE + if (EN_FCS(padapter)) + rtw_hal_set_hwreg(padapter, HW_VAR_STOP_FCS_MODE, NULL); +#endif + + if (!(MLME_IS_STA(padapter) && MLME_IS_OPCH_SW(padapter))) { + /* DFS and channel status no need to check here for STA under OPCH_SW */ + u8 ch, bw, offset; + + #ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(padapter, self_action, 0); + #endif + + if (rtw_mi_get_ch_setting_union_no_self(padapter, &ch, &bw, &offset) != 0) + set_channel_bwmode(padapter, ch, offset, bw); + rtw_mi_update_union_chan_inf(padapter, ch, offset, bw); + rtw_rfctl_update_op_mode(adapter_to_rfctl(padapter), BIT(padapter->iface_id), 0); + } + + flush_all_cam_entry(padapter); + + _cancel_timer_ex(&pmlmeext->link_timer); + + /* pmlmepriv->LinkDetectInfo.TrafficBusyState = _FALSE; */ + pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0; + pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0; + +#ifdef CONFIG_TDLS + padapter->tdlsinfo.ap_prohibited = _FALSE; + + /* For TDLS channel switch, currently we only allow it to work in wifi logo test mode */ + if (padapter->registrypriv.wifi_spec == 1) + padapter->tdlsinfo.ch_switch_prohibited = _FALSE; +#endif /* CONFIG_TDLS */ + +#ifdef CONFIG_WMMPS_STA + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + /* reset currently related uapsd setting when the connection has broken */ + pmlmepriv->qospriv.uapsd_max_sp_len = 0; + pmlmepriv->qospriv.uapsd_tid = 0; + pmlmepriv->qospriv.uapsd_tid_delivery_enabled = 0; + pmlmepriv->qospriv.uapsd_tid_trigger_enabled = 0; + pmlmepriv->qospriv.uapsd_ap_supported = 0; + } +#endif /* CONFIG_WMMPS_STA */ +#ifdef CONFIG_RTS_FULL_BW + rtw_set_rts_bw(padapter); +#endif/*CONFIG_RTS_FULL_BW*/ + +} + +void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res) +{ + struct sta_info *psta; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + struct sta_priv *pstapriv = &padapter->stapriv; + u8 join_type; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + +#if (!defined(CONFIG_IOCTL_CFG80211)) || defined(CONFIG_LAYER2_ROAMING) + struct security_priv *psecuritypriv = &padapter->securitypriv; +#endif + + if (pmlmepriv->wpa_phase == _TRUE) + pmlmepriv->wpa_phase = _FALSE; + + if (join_res < 0) { + join_type = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr); + if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) + rtw_hal_rcr_set_chk_bssid(padapter, MLME_STA_DISCONNECTED); + + rtw_btcoex_connect_notify(padapter, join_type); + + goto exit_mlmeext_joinbss_event_callback; + } + +#ifdef CONFIG_ARP_KEEP_ALIVE + pmlmepriv->bGetGateway = 1; + pmlmepriv->GetGatewayTryCnt = 0; +#endif + +#ifdef CONFIG_AP_MODE + if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) { + /* update bc/mc sta_info */ + update_bmc_sta(padapter); + } +#endif + + /* turn on dynamic functions */ + /* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE); */ + + /* update IOT-releated issue */ + update_IOT_info(padapter); + + #ifdef CONFIG_RTS_FULL_BW + rtw_set_rts_bw(padapter); + #endif/*CONFIG_RTS_FULL_BW*/ + + rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates); + + /* BCN interval */ + rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval)); + + /* udpate capability */ + update_capinfo(padapter, pmlmeinfo->capability); + + /* WMM, Update EDCA param */ + WMMOnAssocRsp(padapter); +#ifdef CONFIG_80211N_HT + /* HT */ + HTOnAssocRsp(padapter); +#endif /* CONFIG_80211N_HT */ +#ifdef CONFIG_80211AC_VHT + /* VHT */ + VHTOnAssocRsp(padapter); +#endif + + psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress); + if (psta) { /* only for infra. mode */ + psta->wireless_mode = pmlmeext->cur_wireless_mode; + + /* set per sta rate after updating HT cap. */ + set_sta_rate(padapter, psta); + + rtw_sta_media_status_rpt(padapter, psta, 1); + + /* wakeup macid after join bss successfully to ensure + the subsequent data frames can be sent out normally */ + rtw_hal_macid_wakeup(padapter, psta->cmn.mac_id); + + rtw_xmit_queue_clear(psta); + } + +#ifndef CONFIG_IOCTL_CFG80211 + if (is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm)) + rtw_sec_restore_wep_key(padapter); +#endif /* CONFIG_IOCTL_CFG80211 */ + + if (rtw_port_switch_chk(padapter) == _TRUE) + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL); + + join_type = 2; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) { + rtw_hal_rcr_set_chk_bssid(padapter, MLME_STA_CONNECTED); + + /* correcting TSF */ + correct_TSF(padapter, MLME_STA_CONNECTED); + + /* set_link_timer(pmlmeext, DISCONNECT_TO); */ + } + +#ifdef CONFIG_LPS + #ifndef CONFIG_FW_MULTI_PORT_SUPPORT + if (get_hw_port(padapter) == HW_PORT0) + #endif + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, RTW_CMDF_DIRECTLY); +#endif + + rtw_btcoex_connect_notify(padapter, join_type); + +#ifdef CONFIG_BEAMFORMING + if (psta) + beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0); +#endif/*CONFIG_BEAMFORMING*/ + +exit_mlmeext_joinbss_event_callback: + + rtw_join_done_chk_ch(padapter, join_res); +#ifdef CONFIG_RTW_REPEATER_SON + rtw_rson_join_done(padapter); +#endif + +#ifdef CONFIG_LAYER2_ROAMING + if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) && + (pmlmepriv->roam_network)) { + struct xmit_frame *rframe; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _list *plist, *phead; + _irqL irqL; + _pkt *pkt; + + padapter->mlmepriv.roam_network = NULL; + _enter_critical_bh(&pxmitpriv->rpkt_queue.lock, &irqL); + phead = get_list_head(&pxmitpriv->rpkt_queue); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + rframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + plist = get_next(plist); + rtw_list_delete(&rframe->list); + pkt = rframe->pkt; + rframe->pkt = NULL; + rtw_xmit_posthandle(padapter, rframe, pkt); + } + _exit_critical_bh(&pxmitpriv->rpkt_queue.lock, &irqL); + } +#endif + RTW_INFO("=>%s - End to Connection without 4-way\n", __FUNCTION__); +} + +/* currently only adhoc mode will go here */ +void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 join_type; + + RTW_INFO("%s\n", __FUNCTION__); + + if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) { + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { /* adhoc master or sta_count>1 */ + /* nothing to do */ + } else { /* adhoc client */ + #ifdef CONFIG_AP_MODE + /* update TSF Value */ + /* update_TSF(pmlmeext, pframe, len); */ + + /* correcting TSF */ + correct_TSF(padapter, MLME_ADHOC_STARTED); + + /* start beacon */ + if (send_beacon(padapter) == _FAIL) + rtw_warn_on(1); + + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + #endif + } + + join_type = 2; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + rtw_btcoex_connect_notify(padapter, join_type); + } + + /* update adhoc sta_info */ + update_sta_info(padapter, psta); + + rtw_hal_update_sta_ra_info(padapter, psta); + + /* ToDo: HT for Ad-hoc */ + psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel); + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE); + + /* rate radaptive */ + Update_RA_Entry(padapter, psta); +} + +void mlmeext_sta_del_event_callback(_adapter *padapter) +{ + if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter)) + rtw_mlmeext_disconnect(padapter); +} + +/**************************************************************************** + +Following are the functions for the timer handlers + +*****************************************************************************/ +void _linked_info_dump(_adapter *padapter) +{ + if (padapter->bLinkInfoDump) { + rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, RTW_DBGDUMP); + rtw_hal_set_odm_var(padapter, HAL_ODM_RX_INFO_DUMP, RTW_DBGDUMP, _FALSE); + } +} +/******************************************************************** + +When station does not receive any packet in MAX_CONTINUAL_NORXPACKET_COUNT*2 seconds, +recipient station will teardown the block ack by issuing DELBA frame. + +*********************************************************************/ +void rtw_delba_check(_adapter *padapter, struct sta_info *psta, u8 from_timer) +{ + int i = 0; + int ret = _SUCCESS; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + /* + IOT issue,occur Broadcom ap(Buffalo WZR-D1800H,Netgear R6300). + AP is originator.AP does not transmit unicast packets when STA response its BAR. + This case probably occur ap issue BAR after AP builds BA. + + Follow 802.11 spec, STA shall maintain an inactivity timer for every negotiated Block Ack setup. + The inactivity timer is not reset when MPDUs corresponding to other TIDs are received. + */ + if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_BROADCOM) { + for (i = 0; i < TID_NUM ; i++) { + if ((psta->recvreorder_ctrl[i].enable) && + (sta_rx_data_qos_pkts(psta, i) == sta_last_rx_data_qos_pkts(psta, i)) ) { + if (_TRUE == rtw_inc_and_chk_continual_no_rx_packet(psta, i)) { + /* send a DELBA frame to the peer STA with the Reason Code field set to TIMEOUT */ + if (!from_timer) + ret = issue_del_ba_ex(padapter, psta->cmn.mac_addr, i, 39, 0, 3, 1); + else + issue_del_ba(padapter, psta->cmn.mac_addr, i, 39, 0); + psta->recvreorder_ctrl[i].enable = _FALSE; + if (ret != _FAIL) + psta->recvreorder_ctrl[i].ampdu_size = RX_AMPDU_SIZE_INVALID; + rtw_reset_continual_no_rx_packet(psta, i); + } + } else { + /* The inactivity timer is reset when MPDUs to the TID is received. */ + rtw_reset_continual_no_rx_packet(psta, i); + } + } + } +} + + +u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta) +{ + u8 ret = _FALSE; +#ifdef DBG_EXPIRATION_CHK + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + RTW_INFO(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu" + /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/ + ", retry:%u\n" + , FUNC_ADPT_ARG(padapter) + , STA_RX_PKTS_DIFF_ARG(psta) + , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts + , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts + /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts + , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts + , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts + , pmlmeinfo->bcn_interval*/ + , pmlmeext->retry + ); + + RTW_INFO(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter) + , sta_tx_pkts(psta) + , pmlmeinfo->link_count + ); +#endif + + if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) + && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) + && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta) + ) + ret = _FALSE; + else + ret = _TRUE; + + sta_update_last_rx_pkts(psta); + + return ret; +} + +u8 chk_adhoc_peer_is_alive(struct sta_info *psta) +{ + u8 ret = _TRUE; + +#ifdef DBG_EXPIRATION_CHK + RTW_INFO("sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu" + /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/ + ", expire_to:%u\n" + , MAC_ARG(psta->cmn.mac_addr) + , psta->cmn.rssi_stat.rssi + , STA_RX_PKTS_DIFF_ARG(psta) + , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts + , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts + /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts + , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts + , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts + , pmlmeinfo->bcn_interval*/ + , psta->expire_to + ); +#endif + + if (sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta) + && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) + && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)) + ret = _FALSE; + + sta_update_last_rx_pkts(psta); + + return ret; +} + +#ifdef CONFIG_TDLS +u8 chk_tdls_peer_sta_is_alive(_adapter *padapter, struct sta_info *psta) +{ + if ((psta->sta_stats.rx_data_pkts == psta->sta_stats.last_rx_data_pkts) + && (psta->sta_stats.rx_tdls_disc_rsp_pkts == psta->sta_stats.last_rx_tdls_disc_rsp_pkts)) + return _FALSE; + + return _TRUE; +} + +void linked_status_chk_tdls(_adapter *padapter) +{ + struct candidate_pool { + struct sta_info *psta; + u8 addr[ETH_ALEN]; + }; + struct sta_priv *pstapriv = &padapter->stapriv; + _irqL irqL; + u8 ack_chk; + struct sta_info *psta; + int i, num_teardown = 0, num_checkalive = 0; + _list *plist, *phead; + struct tdls_txmgmt txmgmt; + struct candidate_pool checkalive[MAX_ALLOWED_TDLS_STA_NUM]; + struct candidate_pool teardown[MAX_ALLOWED_TDLS_STA_NUM]; + u8 tdls_sta_max = _FALSE; + +#define ALIVE_MIN 2 +#define ALIVE_MAX 5 + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + _rtw_memset(checkalive, 0x00, sizeof(checkalive)); + _rtw_memset(teardown, 0x00, sizeof(teardown)); + + if ((padapter->tdlsinfo.link_established == _TRUE)) { + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + + if (psta->tdls_sta_state & TDLS_LINKED_STATE) { + psta->alive_count++; + if (psta->alive_count >= ALIVE_MIN) { + if (chk_tdls_peer_sta_is_alive(padapter, psta) == _FALSE) { + if (psta->alive_count < ALIVE_MAX) { + _rtw_memcpy(checkalive[num_checkalive].addr, psta->cmn.mac_addr, ETH_ALEN); + checkalive[num_checkalive].psta = psta; + num_checkalive++; + } else { + _rtw_memcpy(teardown[num_teardown].addr, psta->cmn.mac_addr, ETH_ALEN); + teardown[num_teardown].psta = psta; + num_teardown++; + } + } else + psta->alive_count = 0; + } + psta->sta_stats.last_rx_data_pkts = psta->sta_stats.rx_data_pkts; + psta->sta_stats.last_rx_tdls_disc_rsp_pkts = psta->sta_stats.rx_tdls_disc_rsp_pkts; + + if ((num_checkalive >= MAX_ALLOWED_TDLS_STA_NUM) || (num_teardown >= MAX_ALLOWED_TDLS_STA_NUM)) { + tdls_sta_max = _TRUE; + break; + } + } + } + + if (tdls_sta_max == _TRUE) + break; + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + if (num_checkalive > 0) { + for (i = 0; i < num_checkalive; i++) { + _rtw_memcpy(txmgmt.peer, checkalive[i].addr, ETH_ALEN); + issue_tdls_dis_req(padapter, &txmgmt); + issue_tdls_dis_req(padapter, &txmgmt); + issue_tdls_dis_req(padapter, &txmgmt); + } + } + + if (num_teardown > 0) { + for (i = 0; i < num_teardown; i++) { + RTW_INFO("[%s %d] Send teardown to "MAC_FMT"\n", __FUNCTION__, __LINE__, MAC_ARG(teardown[i].addr)); + txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_; + _rtw_memcpy(txmgmt.peer, teardown[i].addr, ETH_ALEN); + issue_tdls_teardown(padapter, &txmgmt, _FALSE); + } + } + } + +} +#endif /* CONFIG_TDLS */ + +inline int rtw_get_rx_chk_limit(_adapter *adapter) +{ + return adapter->stapriv.rx_chk_limit; +} + +inline void rtw_set_rx_chk_limit(_adapter *adapter, int limit) +{ + adapter->stapriv.rx_chk_limit = limit; +} + +/* from_timer == 1 means driver is in LPS */ +void linked_status_chk(_adapter *padapter, u8 from_timer) +{ + u32 i; + struct sta_info *psta; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; +#ifdef CONFIG_LAYER2_ROAMING + struct recv_priv *precvpriv = &padapter->recvpriv; +#endif + +#ifdef CONFIG_RTW_WDS + rtw_wds_gptr_expire(padapter); +#endif + + if (padapter->registrypriv.mp_mode == _TRUE) + return; + + if (check_fwstate(pmlmepriv, WIFI_CSA_UPDATE_BEACON)) + return; + + if (is_client_associated_to_ap(padapter)) { + /* linked infrastructure client mode */ + + int tx_chk = _SUCCESS, rx_chk = _SUCCESS; + int rx_chk_limit; + int link_count_limit; + +#if defined(CONFIG_RTW_REPEATER_SON) + rtw_rson_scan_wk_cmd(padapter, RSON_SCAN_PROCESS); +#elif defined(CONFIG_LAYER2_ROAMING) + if (rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE)) { + RTW_INFO("signal_strength_data.avg_val = %d\n", precvpriv->signal_strength_data.avg_val); + if ((precvpriv->signal_strength_data.avg_val < pmlmepriv->roam_rssi_threshold) + && (rtw_get_passing_time_ms(pmlmepriv->last_roaming) >= pmlmepriv->roam_scan_int*2000)) { +#ifdef CONFIG_RTW_80211K + rtw_roam_nb_discover(padapter, _FALSE); +#endif + pmlmepriv->need_to_roam = _TRUE; + rtw_drv_scan_by_self(padapter, RTW_AUTO_SCAN_REASON_ROAM); + pmlmepriv->last_roaming = rtw_get_current_time(); + } else + pmlmepriv->need_to_roam = _FALSE; + } +#endif +#ifdef CONFIG_MCC_MODE + /* + * due to tx ps null date to ao, so ap doest not tx pkt to driver + * we may check chk_ap_is_alive fail, and may issue_probereq to wrong channel under sitesurvey + * don't keep alive check under MCC + */ + if (rtw_hal_mcc_link_status_chk(padapter, __func__) == _FALSE) + return; +#endif + + rx_chk_limit = rtw_get_rx_chk_limit(padapter); + +#ifdef CONFIG_ARP_KEEP_ALIVE + if (!from_timer && pmlmepriv->bGetGateway == 1 && pmlmepriv->GetGatewayTryCnt < 3) { + RTW_INFO("do rtw_gw_addr_query() : %d\n", pmlmepriv->GetGatewayTryCnt); + pmlmepriv->GetGatewayTryCnt++; + if (rtw_gw_addr_query(padapter) == 0) + pmlmepriv->bGetGateway = 0; + else { + _rtw_memset(pmlmepriv->gw_ip, 0, 4); + _rtw_memset(pmlmepriv->gw_mac_addr, 0, ETH_ALEN); + } + } +#endif +#ifdef CONFIG_P2P + if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE)) { + if (!from_timer) + link_count_limit = 3; /* 8 sec */ + else + link_count_limit = 15; /* 32 sec */ + } else +#endif /* CONFIG_P2P */ + { + if (!from_timer) + link_count_limit = 7; /* 16 sec */ + else + link_count_limit = 29; /* 60 sec */ + } + +#ifdef CONFIG_TDLS +#ifdef CONFIG_TDLS_CH_SW + if (ATOMIC_READ(&padapter->tdlsinfo.chsw_info.chsw_on) == _TRUE) + return; +#endif /* CONFIG_TDLS_CH_SW */ + +#ifdef CONFIG_TDLS_AUTOCHECKALIVE + linked_status_chk_tdls(padapter); +#endif /* CONFIG_TDLS_AUTOCHECKALIVE */ +#endif /* CONFIG_TDLS */ + + psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress); + if (psta != NULL) { + bool is_p2p_enable = _FALSE; +#ifdef CONFIG_P2P + is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE); +#endif + +#ifdef CONFIG_ISSUE_DELBA_WHEN_NO_TRAFFIC + /*issue delba when ap does not tx data packet that is Broadcom ap */ + rtw_delba_check(padapter, psta, from_timer); +#endif + if (chk_ap_is_alive(padapter, psta) == _FALSE) + rx_chk = _FAIL; + + if (sta_last_tx_pkts(psta) == sta_tx_pkts(psta)) + tx_chk = _FAIL; + +#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + if (!from_timer && pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL) + ) { + u8 backup_ch = 0, backup_bw = 0, backup_offset = 0; + u8 union_ch = 0, union_bw = 0, union_offset = 0; + u8 switch_channel_by_drv = _TRUE; + + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) { + /* driver doesn't switch channel under MCC */ + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) + switch_channel_by_drv = _FALSE; + } +#endif + if (switch_channel_by_drv) { + if (!rtw_mi_get_ch_setting_union(padapter, &union_ch, &union_bw, &union_offset) + || pmlmeext->cur_channel != union_ch) + goto bypass_active_keep_alive; + + /* switch to correct channel of current network before issue keep-alive frames */ + if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) { + backup_ch = rtw_get_oper_ch(padapter); + backup_bw = rtw_get_oper_bw(padapter); + backup_offset = rtw_get_oper_choffset(padapter); + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + } + } + + if (rx_chk != _SUCCESS) + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->cmn.mac_addr, 0, 0, 3, 1); + + if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) || rx_chk != _SUCCESS) { + if (rtw_mi_check_fwstate(padapter, WIFI_UNDER_SURVEY)) + tx_chk = issue_nulldata(padapter, psta->cmn.mac_addr, 1, 3, 1); + else + tx_chk = issue_nulldata(padapter, psta->cmn.mac_addr, 0, 3, 1); + /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */ + if (tx_chk == _SUCCESS && !is_p2p_enable) + rx_chk = _SUCCESS; + } + + /* back to the original operation channel */ + if (backup_ch > 0 && switch_channel_by_drv) + set_channel_bwmode(padapter, backup_ch, backup_offset, backup_bw); + +bypass_active_keep_alive: + ; + } else +#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */ + { + if (rx_chk != _SUCCESS) { + if (pmlmeext->retry == 0) { +#ifdef DBG_EXPIRATION_CHK + RTW_INFO("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry); +#endif + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, (from_timer ? 0 : 1)); + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, (from_timer ? 0 : 1)); + issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, (from_timer ? 0 : 1)); + } + } + + if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit +#ifdef CONFIG_MCC_MODE + /* FW tx nulldata under MCC mode, we just check ap is alive */ + && (!rtw_hal_check_mcc_status(padapter, MCC_STATUS_NEED_MCC)) +#endif /* CONFIG_MCC_MODE */ + ) { + #ifdef DBG_EXPIRATION_CHK + RTW_INFO("%s issue_nulldata(%d)\n", __FUNCTION__, from_timer ? 1 : 0); + #endif + if (from_timer || rtw_mi_check_fwstate(padapter, WIFI_UNDER_SURVEY)) + tx_chk = issue_nulldata(padapter, NULL, 1, 0, 0); + else + tx_chk = issue_nulldata(padapter, NULL, 0, 1, 1); + } + } + + if (rx_chk == _FAIL) { + pmlmeext->retry++; + if (pmlmeext->retry > rx_chk_limit) { + RTW_PRINT(FUNC_ADPT_FMT" disconnect or roaming\n", + FUNC_ADPT_ARG(padapter)); + receive_disconnect(padapter, pmlmeinfo->network.MacAddress + , WLAN_REASON_EXPIRATION_CHK, _FALSE); + return; + } + } else + pmlmeext->retry = 0; + + if (tx_chk == _FAIL) + pmlmeinfo->link_count %= (link_count_limit + 1); + else { + psta->sta_stats.last_tx_pkts = psta->sta_stats.tx_pkts; + pmlmeinfo->link_count = 0; + } + + } /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */ + + } else if (is_client_associated_to_ibss(padapter)) { + _irqL irqL; + _list *phead, *plist, dlist; + + _rtw_init_listhead(&dlist); + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (i = 0; i < NUM_STA; i++) { + + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + + if (is_broadcast_mac_addr(psta->cmn.mac_addr)) + continue; + + if (chk_adhoc_peer_is_alive(psta) || !psta->expire_to) + psta->expire_to = pstapriv->adhoc_expire_to; + else + psta->expire_to--; + + if (psta->expire_to <= 0) { + rtw_list_delete(&psta->list); + rtw_list_insert_tail(&psta->list, &dlist); + } + } + } + + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + plist = get_next(&dlist); + while (rtw_end_of_queue_search(&dlist, plist) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, list); + plist = get_next(plist); + rtw_list_delete(&psta->list); + RTW_INFO(FUNC_ADPT_FMT" ibss expire "MAC_FMT"\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(psta->cmn.mac_addr)); + report_del_sta_event(padapter, psta->cmn.mac_addr, WLAN_REASON_EXPIRATION_CHK, from_timer ? _TRUE : _FALSE, _FALSE); + } + } + +} + +void survey_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct cmd_obj *cmd; + struct sitesurvey_parm *psurveyPara; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (mlmeext_scan_state(pmlmeext) > SCAN_DISABLE) { + cmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (cmd == NULL) { + rtw_warn_on(1); + goto exit; + } + + psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm)); + if (psurveyPara == NULL) { + rtw_warn_on(1); + rtw_mfree((unsigned char *)cmd, sizeof(struct cmd_obj)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(cmd, psurveyPara, CMD_SITE_SURVEY); + rtw_enqueue_cmd(pcmdpriv, cmd); + } + +exit: + return; +} + +#ifdef CONFIG_RTW_REPEATER_SON +/* 100ms pass, stop rson_scan */ +void rson_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + + rtw_rson_scan_wk_cmd(padapter, RSON_SCAN_DISABLE); +} + +#endif + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +void rtw_tbtx_xmit_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (ATOMIC_READ(&padapter->tbtx_remove_tx_pause) == _TRUE){ + ATOMIC_SET(&padapter->tbtx_tx_pause, _FALSE); + rtw_tx_control_cmd(padapter); + }else { + rtw_issue_action_token_rel(padapter); + ATOMIC_SET(&padapter->tbtx_tx_pause, _TRUE); + rtw_tx_control_cmd(padapter); + _set_timer(&pmlmeext->tbtx_xmit_timer, MAX_TXPAUSE_DURATION); + ATOMIC_SET(&padapter->tbtx_remove_tx_pause, _TRUE); + } +} + +#ifdef CONFIG_AP_MODE +void rtw_tbtx_token_dispatch_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + _irqL irqL; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + _list *phead, *plist; + int i, found = _FALSE; + u8 nr_send, th_idx = 0; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + RTW_DBG("%s:asoc_cnt: %d\n",__func__, pstapriv->tbtx_asoc_list_cnt); + + // check number of TBTX sta + if (padapter->stapriv.tbtx_asoc_list_cnt < 2) + goto exit; + + // dispatch token + + nr_send = RTW_DIV_ROUND_UP(pstapriv->tbtx_asoc_list_cnt, NR_TBTX_SLOT); + + phead = &pstapriv->asoc_list; + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + /* psta is supporting TBTX */ + if ((!psta) || (!psta->tbtx_enable)) + RTW_DBG("sta tbtx_enable is false\n"); + else { + for (i = 0; i < nr_send; i++) { + if (pstapriv->last_token_holder == psta) { + found = _TRUE; + goto outof_loop; + } + } + } + plist = get_next(plist); + } +outof_loop: + + RTW_DBG("rtw_tbtx_token_dispatch_timer_hdl() th_idx=%d, nr_send=%d, phead=%p, plist=%p, found=%d\n ", th_idx , nr_send, phead, plist, found); + if (!found) { + plist = get_next(phead); + while(rtw_end_of_queue_search(phead, plist) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + if ((!psta) || (!psta->tbtx_enable)) + RTW_DBG("sta tbtx_enable is false\n"); + else { + pstapriv->token_holder[th_idx] = psta; + rtw_issue_action_token_req(padapter, pstapriv->token_holder[th_idx++]); + break; + } + plist = get_next(plist); + } + } + + for (i=th_idx; itbtx_enable)) + RTW_DBG("sta tbtx_enable is false\n"); + else { + pstapriv->token_holder[th_idx] = psta; + rtw_issue_action_token_req(padapter, pstapriv->token_holder[th_idx++]); + i++; + } + } + ATOMIC_SET(&pstapriv->nr_token_keeper, nr_send); + + +exit: + // set_timer + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + _set_timer(&pmlmeext->tbtx_token_dispatch_timer, TBTX_TX_DURATION); +} +#endif /* CONFIG_AP_MODE */ +#endif /* CONFIG_RTW_TOKEN_BASED_XMIT */ + +void link_timer_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + /* static unsigned int rx_pkt = 0; */ + /* static u64 tx_cnt = 0; */ + /* struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); */ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + /* struct sta_priv *pstapriv = &padapter->stapriv; */ +#ifdef CONFIG_RTW_80211R + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL; + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); +#endif + + if (rtw_sta_linking_test_force_fail()) + RTW_INFO("rtw_sta_linking_test_force_fail\n"); + + if (pmlmeext->join_abort && pmlmeinfo->state != WIFI_FW_NULL_STATE) { + RTW_INFO(FUNC_ADPT_FMT" join abort\n", FUNC_ADPT_ARG(padapter)); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res(padapter, -4, WLAN_STATUS_UNSPECIFIED_FAILURE); + goto exit; + } + + if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) { + RTW_INFO("link_timer_hdl:no beacon while connecting\n"); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + report_join_res(padapter, -3, WLAN_STATUS_UNSPECIFIED_FAILURE); + } else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) { + +#ifdef CONFIG_IOCTL_CFG80211 + if (rtw_sec_chk_auth_type(padapter, MLME_AUTHTYPE_SAE)) + return; +#endif /* CONFIG_IOCTL_CFG80211 */ + + /* re-auth timer */ + if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) { + /* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */ + /* { */ + pmlmeinfo->state = 0; + if (pmlmeinfo->auth_status) { + report_join_res(padapter, -1, pmlmeinfo->auth_status); + pmlmeinfo->auth_status = 0; /* reset */ + } else + report_join_res(padapter, -1, WLAN_STATUS_UNSPECIFIED_FAILURE); + return; + /* } */ + /* else */ + /* { */ + /* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */ + /* pmlmeinfo->reauth_count = 0; */ + /* } */ + } + + RTW_INFO("link_timer_hdl: auth timeout and try again\n"); + pmlmeinfo->auth_seq = 1; + issue_auth(padapter, NULL, 0); + set_link_timer(pmlmeext, REAUTH_TO); + } else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) { + /* re-assoc timer */ + if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) { + pmlmeinfo->state = WIFI_FW_NULL_STATE; +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam(padapter)) { + psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress); + if (psta) + rtw_free_stainfo(padapter, psta); + } +#endif + report_join_res(padapter, -2, WLAN_STATUS_UNSPECIFIED_FAILURE); + return; + } + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_roam(padapter)) { + RTW_INFO("link_timer_hdl: reassoc timeout and try again\n"); + issue_reassocreq(padapter); + } else +#endif + { + RTW_INFO("link_timer_hdl: assoc timeout and try again\n"); + issue_assocreq(padapter); + } + + set_link_timer(pmlmeext, REASSOC_TO); + } + +exit: + return; +} + +void addba_timer_hdl(void *ctx) +{ + struct sta_info *psta = (struct sta_info *)ctx; + +#ifdef CONFIG_80211N_HT + struct ht_priv *phtpriv; + + if (!psta) + return; + + phtpriv = &psta->htpriv; + + if ((phtpriv->ht_option == _TRUE) && (phtpriv->ampdu_enable == _TRUE)) { + if (phtpriv->candidate_tid_bitmap) + phtpriv->candidate_tid_bitmap = 0x0; + + } +#endif /* CONFIG_80211N_HT */ +} + +#ifdef CONFIG_IEEE80211W +void report_sta_timeout_event(_adapter *padapter, u8 *MacAddr, unsigned short reason) +{ + struct cmd_obj *pcmd_obj; + u8 *pevtcmd; + u32 cmdsz; + struct sta_info *psta; + int mac_id; + struct stadel_event *pdel_sta_evt; + struct rtw_evt_header *evt_hdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + + pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd_obj == NULL) + return; + + cmdsz = (sizeof(struct stadel_event) + sizeof(struct rtw_evt_header)); + pevtcmd = (u8 *)rtw_zmalloc(cmdsz); + if (pevtcmd == NULL) { + rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj)); + return; + } + + _rtw_init_listhead(&pcmd_obj->list); + + pcmd_obj->cmdcode = CMD_SET_MLME_EVT; + pcmd_obj->cmdsz = cmdsz; + pcmd_obj->parmbuf = pevtcmd; + + pcmd_obj->rsp = NULL; + pcmd_obj->rspsz = 0; + + evt_hdr = (struct rtw_evt_header *)(pevtcmd); + evt_hdr->len = sizeof(struct stadel_event); + evt_hdr->id = EVT_TIMEOUT_STA; + evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq); + + pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct rtw_evt_header)); + _rtw_memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN); + _rtw_memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2); + + + psta = rtw_get_stainfo(&padapter->stapriv, MacAddr); + if (psta) + mac_id = (int)psta->cmn.mac_id; + else + mac_id = (-1); + + pdel_sta_evt->mac_id = mac_id; + + RTW_INFO("report_del_sta_event: delete STA, mac_id=%d\n", mac_id); + + rtw_enqueue_cmd(pcmdpriv, pcmd_obj); + + return; +} + +void clnt_sa_query_timeout(_adapter *padapter) +{ + struct mlme_ext_priv *mlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *mlmeinfo = &(mlmeext->mlmext_info); + + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + receive_disconnect(padapter, get_my_bssid(&(mlmeinfo->network)), WLAN_REASON_SA_QUERY_TIMEOUT, _FALSE); +} + +void sa_query_timer_hdl(void *ctx) +{ + struct sta_info *psta = (struct sta_info *)ctx; + _adapter *padapter = psta->padapter; + _irqL irqL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE && + check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + clnt_sa_query_timeout(padapter); + else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE) + report_sta_timeout_event(padapter, psta->cmn.mac_addr, WLAN_REASON_PREV_AUTH_NOT_VALID); +} + +#endif /* CONFIG_IEEE80211W */ + +#ifdef CONFIG_AUTO_AP_MODE +void rtw_auto_ap_rx_msg_dump(_adapter *padapter, union recv_frame *precv_frame, u8 *ehdr_pos) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_info *psta = precv_frame->u.hdr.psta; + struct ethhdr *ehdr = (struct ethhdr *)ehdr_pos; + + RTW_INFO("eth rx: got eth_type=0x%x\n", ntohs(ehdr->h_proto)); + + if (psta && psta->isrc && psta->pid > 0) { + u16 rx_pid; + + rx_pid = *(u16 *)(ehdr_pos + ETH_HLEN); + + RTW_INFO("eth rx(pid=0x%x): sta("MAC_FMT") pid=0x%x\n", + rx_pid, MAC_ARG(psta->cmn.mac_addr), psta->pid); + + if (rx_pid == psta->pid) { + int i; + u16 len = *(u16 *)(ehdr_pos + ETH_HLEN + 2); + /* u16 ctrl_type = *(u16 *)(ehdr_pos + ETH_HLEN + 4); */ + + /* RTW_INFO("eth, RC: len=0x%x, ctrl_type=0x%x\n", len, ctrl_type); */ + RTW_INFO("eth, RC: len=0x%x\n", len); + + for (i = 0; i < len; i++) + RTW_INFO("0x%x\n", *(ehdr_pos + ETH_HLEN + 4 + i)); + /* RTW_INFO("0x%x\n", *(ehdr_pos + ETH_HLEN + 6 + i)); */ + + RTW_INFO("eth, RC-end\n"); + } + } + +} + +void rtw_start_auto_ap(_adapter *adapter) +{ + RTW_INFO("%s\n", __FUNCTION__); + + rtw_set_802_11_infrastructure_mode(adapter, Ndis802_11APMode, 0); + + rtw_setopmode_cmd(adapter, Ndis802_11APMode, RTW_CMDF_WAIT_ACK); +} + +static int rtw_auto_ap_start_beacon(_adapter *adapter) +{ + int ret = 0; + u8 *pbuf = NULL; + uint len; + u8 supportRate[16]; + int sz = 0, rateLen; + u8 *ie; + u8 wireless_mode, oper_channel; + u8 ssid[3] = {0}; /* hidden ssid */ + u32 ssid_len = sizeof(ssid); + struct mlme_priv *pmlmepriv = &(adapter->mlmepriv); + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != _TRUE) + return -EINVAL; + + + len = 128; + pbuf = rtw_zmalloc(len); + if (!pbuf) + return -ENOMEM; + + + /* generate beacon */ + ie = pbuf; + + /* timestamp will be inserted by hardware */ + sz += 8; + ie += sz; + + /* beacon interval : 2bytes */ + *(u16 *)ie = cpu_to_le16((u16)100); /* BCN_INTERVAL=100; */ + sz += 2; + ie += 2; + + /* capability info */ + *(u16 *)ie = 0; + *(u16 *)ie |= cpu_to_le16(cap_ESS); + *(u16 *)ie |= cpu_to_le16(cap_ShortPremble); + /* *(u16*)ie |= cpu_to_le16(cap_Privacy); */ + sz += 2; + ie += 2; + + /* SSID */ + ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz); + + /* supported rates */ + wireless_mode = (WIRELESS_11BG_24N & padapter->registrypriv.wireless_mode); + rtw_set_supported_rate(supportRate, wireless_mode); + rateLen = rtw_get_rateset_len(supportRate); + if (rateLen > 8) + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz); + else + ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz); + + + /* DS parameter set */ + if (rtw_mi_check_status(adapter, MI_LINKED)) + oper_channel = rtw_mi_get_union_chan(adapter); + else + oper_channel = adapter_to_dvobj(adapter)->oper_channel; + + ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz); + + /* ext supported rates */ + if (rateLen > 8) + ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz); + + RTW_INFO("%s, start auto ap beacon sz=%d\n", __FUNCTION__, sz); + + /* lunch ap mode & start to issue beacon */ + if (rtw_check_beacon_data(adapter, pbuf, sz) == _SUCCESS) { + + } else + ret = -EINVAL; + + + rtw_mfree(pbuf, len); + + return ret; + +} +#endif/* CONFIG_AUTO_AP_MODE */ + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +u8 tx_control_hdl(_adapter *adapter) +{ + u8 val; + + if(ATOMIC_READ(&adapter->tbtx_tx_pause)) + val = 0xff; + else + val = 0x00; + + rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &val); + + return H2C_SUCCESS; +} +#endif + +#ifdef CONFIG_AP_MODE +u8 stop_ap_hdl(_adapter *adapter) +{ + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter)); + + rtw_set_802_11_infrastructure_mode(adapter, Ndis802_11Infrastructure, RTW_CMDF_DIRECTLY); + rtw_setopmode_cmd(adapter, Ndis802_11Infrastructure, RTW_CMDF_DIRECTLY); + + return H2C_SUCCESS; +} +#endif + +u8 setopmode_hdl(_adapter *padapter, u8 *pbuf) +{ + u8 type; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf; + + if (psetop->mode == Ndis802_11APMode + || psetop->mode == Ndis802_11_mesh + ) { + pmlmeinfo->state = WIFI_FW_AP_STATE; + type = _HW_STATE_AP_; + } else if (psetop->mode == Ndis802_11Infrastructure) { + pmlmeinfo->state &= ~(BIT(0) | BIT(1)); /* clear state */ + pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to STATION_STATE */ + type = _HW_STATE_STATION_; + } else if (psetop->mode == Ndis802_11IBSS) + type = _HW_STATE_ADHOC_; + else if (psetop->mode == Ndis802_11Monitor) + type = _HW_STATE_MONITOR_; + else + type = _HW_STATE_NOLINK_; + +#ifdef CONFIG_AP_PORT_SWAP + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, (u8 *)(&type)); +#endif + + rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type)); + +#ifdef CONFIG_AUTO_AP_MODE + if (psetop->mode == Ndis802_11APMode) + rtw_auto_ap_start_beacon(padapter); +#endif + + if (rtw_port_switch_chk(padapter) == _TRUE) { + rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL); + + if (psetop->mode == Ndis802_11APMode) + adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; /* ap mode won't dowload rsvd pages */ + else if (psetop->mode == Ndis802_11Infrastructure) { +#ifdef CONFIG_LPS + _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter)); + if (port0_iface) + rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, RTW_CMDF_DIRECTLY); +#endif + } + } + +#ifdef CONFIG_BT_COEXIST + if (psetop->mode == Ndis802_11APMode + || psetop->mode == Ndis802_11_mesh + || psetop->mode == Ndis802_11Monitor + ) { + /* Do this after port switch to */ + /* prevent from downloading rsvd page to wrong port */ + rtw_btcoex_MediaStatusNotify(padapter, 1); /* connect */ + } +#endif /* CONFIG_BT_COEXIST */ + + return H2C_SUCCESS; + +} + +u8 createbss_hdl(_adapter *padapter, u8 *pbuf) +{ +#ifdef CONFIG_AP_MODE + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); + WLAN_BSSID_EX *pdev_network = &padapter->registrypriv.dev_network; + struct createbss_parm *parm = (struct createbss_parm *)pbuf; + u8 ret = H2C_SUCCESS; + /* u8 initialgain; */ + + if ((parm->req_ch == 0 && pmlmeinfo->state == WIFI_FW_AP_STATE) + || parm->req_ch != 0 + ) { + start_bss_network(padapter, parm); + goto exit; + } + + /* below is for ad-hoc master */ + if (parm->adhoc) { + rtw_warn_on(pdev_network->InfrastructureMode != Ndis802_11IBSS); + rtw_joinbss_reset(padapter); + + pmlmeext->cur_bwmode = CHANNEL_WIDTH_20; + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pmlmeinfo->ERP_enable = 0; + pmlmeinfo->WMM_enable = 0; + pmlmeinfo->HT_enable = 0; + pmlmeinfo->HT_caps_enable = 0; + pmlmeinfo->HT_info_enable = 0; + pmlmeinfo->agg_enable_bitmap = 0; + pmlmeinfo->candidate_tid_bitmap = 0; + + /* cancel link timer */ + _cancel_timer_ex(&pmlmeext->link_timer); + + /* clear CAM */ + flush_all_cam_entry(padapter); + + pdev_network->Length = get_WLAN_BSSID_EX_sz(pdev_network); + _rtw_memcpy(pnetwork, pdev_network, FIELD_OFFSET(WLAN_BSSID_EX, IELength)); + pnetwork->IELength = pdev_network->IELength; + + if (pnetwork->IELength > MAX_IE_SZ) { + ret = H2C_PARAMETERS_ERROR; + goto ibss_post_hdl; + } + + _rtw_memcpy(pnetwork->IEs, pdev_network->IEs, pnetwork->IELength); + start_create_ibss(padapter); + } else { + rtw_warn_on(1); + ret = H2C_PARAMETERS_ERROR; + } + +ibss_post_hdl: + rtw_create_ibss_post_hdl(padapter, ret); + +exit: + return ret; +#else + return H2C_SUCCESS; +#endif /* CONFIG_AP_MODE */ +} + +u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf) +{ + u8 join_type; + PNDIS_802_11_VARIABLE_IEs pIE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); +#ifdef CONFIG_ANTENNA_DIVERSITY + struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf; +#endif /* CONFIG_ANTENNA_DIVERSITY */ +#ifdef CONFIG_LAYER2_ROAMING + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct wlan_network *rnetwork = pmlmepriv->roam_network; + struct beacon_keys bcn_keys; + u32 roam_ielen; + _irqL irqL; +#endif + u32 i; + /* u8 initialgain; */ + /* u32 acparm; */ + u8 u_ch, u_bw, u_offset; + u8 doiqk = _FALSE; + + /* check already connecting to AP or not */ + if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { + if (pmlmeinfo->state & WIFI_FW_STATION_STATE) + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100); + pmlmeinfo->state = WIFI_FW_NULL_STATE; + + /* clear CAM */ + flush_all_cam_entry(padapter); + + _cancel_timer_ex(&pmlmeext->link_timer); + + /* set MSR to nolink->infra. mode */ + /* Set_MSR(padapter, _HW_STATE_NOLINK_); */ + Set_MSR(padapter, _HW_STATE_STATION_); + + + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0); + if (pmlmeinfo->state & WIFI_FW_STATION_STATE) + rtw_hal_rcr_set_chk_bssid(padapter, MLME_STA_DISCONNECTED); + } + +#ifdef CONFIG_ANTENNA_DIVERSITY + rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, _FALSE); +#endif + +#ifdef CONFIG_WAPI_SUPPORT + rtw_wapi_clear_all_cam_entry(padapter); +#endif + + rtw_joinbss_reset(padapter); + + pmlmeinfo->ERP_enable = 0; + pmlmeinfo->WMM_enable = 0; + pmlmeinfo->HT_enable = 0; + pmlmeinfo->HT_caps_enable = 0; + pmlmeinfo->HT_info_enable = 0; + pmlmeinfo->agg_enable_bitmap = 0; + pmlmeinfo->candidate_tid_bitmap = 0; + pmlmeinfo->bwmode_updated = _FALSE; + /* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */ + pmlmeinfo->VHT_enable = 0; +#ifdef ROKU_PRIVATE + pmlmeinfo->ht_vht_received = 0; + _rtw_memset(pmlmeinfo->SupportedRates_infra_ap, 0, NDIS_802_11_LENGTH_RATES_EX); +#endif /* ROKU_PRIVATE */ + _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength)); + pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength; + + if (pnetwork->IELength > MAX_IE_SZ) /* Check pbuf->IELength */ + return H2C_PARAMETERS_ERROR; + + if (pnetwork->IELength < 2) { + report_join_res(padapter, (-4), WLAN_STATUS_UNSPECIFIED_FAILURE); + return H2C_SUCCESS; + } + _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength); + + pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork); + + /* Check AP vendor to move rtw_joinbss_cmd() */ + /* pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength); */ + + /* sizeof(NDIS_802_11_FIXED_IEs) */ + for (i = _FIXED_IE_LENGTH_ ; i < pnetwork->IELength - 2 ;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: /* Get WMM IE. */ + if (_rtw_memcmp(pIE->data, WMM_OUI, 4)) + WMM_param_handler(padapter, pIE); + break; + +#ifdef CONFIG_80211N_HT + case _HT_CAPABILITY_IE_: /* Get HT Cap IE. */ + pmlmeinfo->HT_caps_enable = 1; + break; + + case _HT_EXTRA_INFO_IE_: /* Get HT Info IE. */ + pmlmeinfo->HT_info_enable = 1; + break; +#endif /* CONFIG_80211N_HT */ + +#ifdef CONFIG_80211AC_VHT + case EID_VHTCapability: /* Get VHT Cap IE. */ + pmlmeinfo->VHT_enable = 1; + break; + + case EID_VHTOperation: /* Get VHT Operation IE. */ + break; +#endif /* CONFIG_80211AC_VHT */ + default: + break; + } + + i += (pIE->Length + 2); + } + + rtw_bss_get_chbw(pnetwork + , &pmlmeext->cur_channel, &pmlmeext->cur_bwmode, &pmlmeext->cur_ch_offset, 1, 1); + + rtw_adjust_chbw(padapter, pmlmeext->cur_channel, &pmlmeext->cur_bwmode, &pmlmeext->cur_ch_offset); + +#if 0 + if (padapter->registrypriv.wifi_spec) { + /* for WiFi test, follow WMM test plan spec */ + acparm = 0x002F431C; /* VO */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm)); + acparm = 0x005E541C; /* VI */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm)); + acparm = 0x0000A525; /* BE */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm)); + acparm = 0x0000A549; /* BK */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm)); + + /* for WiFi test, mixed mode with intel STA under bg mode throughput issue */ + if (padapter->mlmepriv.htpriv.ht_option == _FALSE) { + acparm = 0x00004320; + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm)); + } + } else { + acparm = 0x002F3217; /* VO */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm)); + acparm = 0x005E4317; /* VI */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm)); + acparm = 0x00105320; /* BE */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm)); + acparm = 0x0000A444; /* BK */ + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm)); + } +#endif + + /* check channel, bandwidth, offset and switch */ + if (rtw_chk_start_clnt_join(padapter, &u_ch, &u_bw, &u_offset) == _FAIL) { + report_join_res(padapter, (-4), WLAN_STATUS_UNSPECIFIED_FAILURE); + return H2C_SUCCESS; + } + + /* disable dynamic functions, such as high power, DIG */ + /*rtw_phydm_func_disable_all(padapter);*/ + + /* config the initial gain under linking, need to write the BB registers */ + /* initialgain = 0x1E; */ + /*rtw_hal_set_odm_var(padapter, HAL_ODM_INITIAL_GAIN, &initialgain, _FALSE);*/ + + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress); + if (MLME_IS_STA(padapter)) + rtw_hal_rcr_set_chk_bssid(padapter, MLME_STA_CONNECTING); + else + rtw_hal_rcr_set_chk_bssid(padapter, MLME_ADHOC_STARTED); + + join_type = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + rtw_btcoex_connect_notify(padapter, join_type); + + doiqk = _TRUE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + + set_channel_bwmode(padapter, u_ch, u_offset, u_bw); + + doiqk = _FALSE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + + /* cancel link timer */ + _cancel_timer_ex(&pmlmeext->link_timer); + + start_clnt_join(padapter); + +#ifdef CONFIG_LAYER2_ROAMING + _enter_critical_bh(&pmlmepriv->clnt_auth_lock, &irqL); + if (rnetwork && (pmlmeinfo->state & (WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE))) { + struct beacon_keys bcn_keys; + u32 roam_ielen; + + roam_ielen = rnetwork->network.IELength; + update_network(&(pmlmepriv->cur_network.network), &(rnetwork->network), padapter, _TRUE); + + /* update bcn keys */ + if (rtw_get_bcn_keys_from_bss(&rnetwork->network, &bcn_keys) == _TRUE) { + _rtw_memcpy(&pmlmepriv->cur_beacon_keys, &bcn_keys, sizeof(bcn_keys)); + if (is_hidden_ssid(bcn_keys.ssid, bcn_keys.ssid_len)) { + _rtw_memcpy(pmlmepriv->cur_beacon_keys.ssid, pmlmeinfo->network.Ssid.Ssid, IW_ESSID_MAX_SIZE); + pmlmepriv->cur_beacon_keys.ssid_len = pmlmeinfo->network.Ssid.SsidLength; + } + } else { + RTW_ERR("%s: get beacon keys failed\n", __func__); + _rtw_memset(&pmlmepriv->cur_beacon_keys, 0, sizeof(bcn_keys)); + } + #ifdef CONFIG_BCN_CNT_CONFIRM_HDL + pmlmepriv->new_beacon_cnts = 0; + #endif + + /* check the vendor of the assoc AP */ + pmlmeinfo->assoc_AP_vendor = check_assoc_AP(rnetwork->network.IEs, rnetwork->network.IELength); + + /* update TSF Value */ + pmlmeext->TSFValue = rnetwork->network.tsf + rtw_get_passing_time_ms(rnetwork->last_scanned)*1000; + pmlmeext->bcn_cnt = 0; + pmlmeext->last_bcn_cnt = 0; + /* start auth */ + start_clnt_auth(padapter); + } + _exit_critical_bh(&pmlmepriv->clnt_auth_lock, &irqL); +#endif + return H2C_SUCCESS; + +} + +u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf) +{ +#if CONFIG_DFS + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); +#endif + struct disconnect_parm *param = (struct disconnect_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); + u8 val8; + + if (is_client_associated_to_ap(padapter) + #if CONFIG_DFS + && !IS_RADAR_DETECTED(rfctl) && !rfctl->csa_ch + #endif + ) { + #ifdef CONFIG_PLATFORM_ROCKCHIPS + /* To avoid connecting to AP fail during resume process, change retry count from 5 to 1 */ + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100); + #else + issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms / 100, 100); + #endif /* CONFIG_PLATFORM_ROCKCHIPS */ + } + +#ifndef CONFIG_SUPPORT_MULTI_BCN + if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) { + /* Stop BCN */ + val8 = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8)); + } +#endif + + rtw_sta_mstatus_report(padapter); + + rtw_mlmeext_disconnect(padapter); + + rtw_free_uc_swdec_pending_queue(padapter); + + return H2C_SUCCESS; +} + +static const char *const _scan_state_str[] = { + "SCAN_DISABLE", + "SCAN_START", + "SCAN_PS_ANNC_WAIT", + "SCAN_ENTER", + "SCAN_PROCESS", + "SCAN_BACKING_OP", + "SCAN_BACK_OP", + "SCAN_LEAVING_OP", + "SCAN_LEAVE_OP", + "SCAN_SW_ANTDIV_BL", + "SCAN_TO_P2P_LISTEN", + "SCAN_P2P_LISTEN", + "SCAN_COMPLETE", + "SCAN_STATE_MAX", +}; + +const char *scan_state_str(u8 state) +{ + state = (state >= SCAN_STATE_MAX) ? SCAN_STATE_MAX : state; + return _scan_state_str[state]; +} + +static bool scan_abort_hdl(_adapter *adapter) +{ + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct ss_res *ss = &pmlmeext->sitesurvey_res; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &adapter->wdinfo; +#endif + bool ret = _FALSE; + + if (pmlmeext->scan_abort == _TRUE) { +#ifdef CONFIG_P2P + if (!rtw_p2p_chk_state(&adapter->wdinfo, P2P_STATE_NONE)) { + rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX); + ss->channel_idx = 3; + RTW_INFO("%s idx:%d, cnt:%u\n", __FUNCTION__ + , ss->channel_idx + , pwdinfo->find_phase_state_exchange_cnt + ); + } else +#endif + { + ss->channel_idx = ss->ch_num; + RTW_INFO("%s idx:%d\n", __FUNCTION__ + , ss->channel_idx + ); + } + ret = _TRUE; + } + + return ret; +} + +u8 rtw_scan_sparse(_adapter *adapter, struct rtw_ieee80211_channel *ch, u8 ch_num) +{ + /* interval larger than this is treated as backgroud scan */ +#ifndef RTW_SCAN_SPARSE_BG_INTERVAL_MS +#define RTW_SCAN_SPARSE_BG_INTERVAL_MS 12000 +#endif + +#ifndef RTW_SCAN_SPARSE_CH_NUM_MIRACAST +#define RTW_SCAN_SPARSE_CH_NUM_MIRACAST 1 +#endif +#ifndef RTW_SCAN_SPARSE_CH_NUM_BG +#define RTW_SCAN_SPARSE_CH_NUM_BG 4 +#endif + +#define SCAN_SPARSE_CH_NUM_INVALID 255 + + static u8 token = 255; + u32 interval; + bool busy_traffic = _FALSE; + bool miracast_enabled = _FALSE; + bool bg_scan = _FALSE; + u8 max_allow_ch = SCAN_SPARSE_CH_NUM_INVALID; + u8 scan_division_num; + u8 ret_num = ch_num; + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + + if (mlmeext->last_scan_time == 0) + mlmeext->last_scan_time = rtw_get_current_time(); + + interval = rtw_get_passing_time_ms(mlmeext->last_scan_time); + + + if (rtw_mi_busy_traffic_check(adapter)) + busy_traffic = _TRUE; + + if (rtw_mi_check_miracast_enabled(adapter)) + miracast_enabled = _TRUE; + + if (interval > RTW_SCAN_SPARSE_BG_INTERVAL_MS) + bg_scan = _TRUE; + + /* max_allow_ch by conditions*/ + +#if RTW_SCAN_SPARSE_MIRACAST + if (miracast_enabled == _TRUE && busy_traffic == _TRUE) + max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_MIRACAST); +#endif + +#if RTW_SCAN_SPARSE_BG + if (bg_scan == _TRUE) + max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_BG); +#endif + + if (max_allow_ch != SCAN_SPARSE_CH_NUM_INVALID) { + int i; + int k = 0; + + scan_division_num = (ch_num / max_allow_ch) + ((ch_num % max_allow_ch) ? 1 : 0); + token = (token + 1) % scan_division_num; + + if (0) + RTW_INFO("scan_division_num:%u, token:%u\n", scan_division_num, token); + + for (i = 0; i < ch_num; i++) { + if (ch[i].hw_value && (i % scan_division_num) == token + ) { + if (i != k) + _rtw_memcpy(&ch[k], &ch[i], sizeof(struct rtw_ieee80211_channel)); + k++; + } + } + + _rtw_memset(&ch[k], 0, sizeof(struct rtw_ieee80211_channel)); + + ret_num = k; + mlmeext->last_scan_time = rtw_get_current_time(); + } + + return ret_num; +} + +#ifdef CONFIG_SCAN_BACKOP +u8 rtw_scan_backop_decision(_adapter *adapter) +{ + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct mi_state mstate; + u8 backop_flags = 0; + + rtw_mi_status(adapter, &mstate); + + if ((MSTATE_STA_LD_NUM(&mstate) && mlmeext_chk_scan_backop_flags_sta(mlmeext, SS_BACKOP_EN)) + || (MSTATE_STA_NUM(&mstate) && mlmeext_chk_scan_backop_flags_sta(mlmeext, SS_BACKOP_EN_NL))) + backop_flags |= mlmeext_scan_backop_flags_sta(mlmeext); + +#ifdef CONFIG_AP_MODE + if ((MSTATE_AP_LD_NUM(&mstate) && mlmeext_chk_scan_backop_flags_ap(mlmeext, SS_BACKOP_EN)) + || (MSTATE_AP_NUM(&mstate) && mlmeext_chk_scan_backop_flags_ap(mlmeext, SS_BACKOP_EN_NL))) + backop_flags |= mlmeext_scan_backop_flags_ap(mlmeext); +#endif + +#ifdef CONFIG_RTW_MESH + if ((MSTATE_MESH_LD_NUM(&mstate) && mlmeext_chk_scan_backop_flags_mesh(mlmeext, SS_BACKOP_EN)) + || (MSTATE_MESH_NUM(&mstate) && mlmeext_chk_scan_backop_flags_mesh(mlmeext, SS_BACKOP_EN_NL))) + backop_flags |= mlmeext_scan_backop_flags_mesh(mlmeext); +#endif + + return backop_flags; +} +#endif + +#define SCANNING_TIMEOUT_EX 2000 +u32 rtw_scan_timeout_decision(_adapter *padapter) +{ + u32 back_op_times= 0; + u8 max_chan_num; + u16 scan_ms; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct ss_res *ss = &pmlmeext->sitesurvey_res; + + if (is_supported_5g(padapter->registrypriv.wireless_mode) + && IsSupported24G(padapter->registrypriv.wireless_mode)) + max_chan_num = MAX_CHANNEL_NUM;/* dual band */ + else + max_chan_num = MAX_CHANNEL_NUM_2G;/*single band*/ + + #ifdef CONFIG_SCAN_BACKOP + if (rtw_scan_backop_decision(padapter)) + back_op_times = (max_chan_num / ss->scan_cnt_max) * ss->backop_ms; + #endif + + if (ss->duration) + scan_ms = ss->duration; + else + #if defined(CONFIG_RTW_ACS) && defined(CONFIG_RTW_ACS_DBG) + if (IS_ACS_ENABLE(padapter) && rtw_is_acs_st_valid(padapter)) + scan_ms = rtw_acs_get_adv_st(padapter); + else + #endif /*CONFIG_RTW_ACS*/ + scan_ms = ss->scan_ch_ms; + + ss->scan_timeout_ms = (scan_ms * max_chan_num) + back_op_times + SCANNING_TIMEOUT_EX; + #ifdef DBG_SITESURVEY + RTW_INFO("%s , scan_timeout_ms = %d (ms)\n", __func__, ss->scan_timeout_ms); + #endif /*DBG_SITESURVEY*/ + return ss->scan_timeout_ms; +} + +static int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out, + u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num, bool no_sparse, int reason) +{ + int i, j; + int set_idx; + u8 chan; + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct registry_priv *regsty = dvobj_to_regsty(adapter_to_dvobj(padapter)); +#ifdef CONFIG_RTW_ROAM_QUICKSCAN + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#endif + + /* clear first */ + _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel) * out_num); + +#ifdef CONFIG_RTW_ROAM_QUICKSCAN + if ((reason == RTW_AUTO_SCAN_REASON_ROAM) && (pmlmeext->quickscan_next)) { + pmlmeext->quickscan_next = _FALSE; + _rtw_memcpy(out, pmlmeext->roam_ch, sizeof(struct rtw_ieee80211_channel)*RTW_CHANNEL_SCAN_AMOUNT); + return pmlmeext->roam_ch_num; + } +#endif + + /* acquire channels from in */ + j = 0; + for (i = 0; i < in_num; i++) { + + if (0) + RTW_INFO(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i])); + + if (!in[i].hw_value || (in[i].flags & RTW_IEEE80211_CHAN_DISABLED)) + continue; + if (rtw_mlme_band_check(padapter, in[i].hw_value) == _FALSE) + continue; + + set_idx = rtw_chset_search_ch(rfctl->channel_set, in[i].hw_value); + if (set_idx >= 0) { + if (j >= out_num) { + RTW_PRINT(FUNC_ADPT_FMT" out_num:%u not enough\n", + FUNC_ADPT_ARG(padapter), out_num); + break; + } + + _rtw_memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel)); + + if (rfctl->channel_set[set_idx].flags & (RTW_CHF_NO_IR | RTW_CHF_DFS)) + out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN; + + j++; + } + if (j >= out_num) + break; + } + + /* if out is empty, use channel_set as default */ + if (j == 0) { + for (i = 0; i < rfctl->max_chan_nums; i++) { + chan = rfctl->channel_set[i].ChannelNum; + if (rtw_mlme_band_check(padapter, chan) == _TRUE) { + if (rtw_mlme_ignore_chan(padapter, chan) == _TRUE) + continue; + + if (0) + RTW_INFO(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), chan); + + if (j >= out_num) { + RTW_PRINT(FUNC_ADPT_FMT" out_num:%u not enough\n", + FUNC_ADPT_ARG(padapter), out_num); + break; + } + + out[j].hw_value = chan; + + if (rfctl->channel_set[i].flags & (RTW_CHF_NO_IR | RTW_CHF_DFS)) + out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN; + + j++; + } + } + } + + if (!no_sparse + && !regsty->wifi_spec + && j > 6 /* assume ch_num > 6 is normal scan */ + ) { + /* scan_sparse */ + j = rtw_scan_sparse(padapter, out, j); + } + + return j; +} + +static void sitesurvey_res_reset(_adapter *adapter, struct sitesurvey_parm *parm) +{ + struct ss_res *ss = &adapter->mlmeextpriv.sitesurvey_res; + RT_CHANNEL_INFO *chset = adapter_to_chset(adapter); + int i, reason = 0; + + ss->bss_cnt = 0; + ss->activate_ch_cnt = 0; + ss->channel_idx = 0; + ss->force_ssid_scan = 0; + ss->igi_scan = 0; + ss->igi_before_scan = 0; +#ifdef CONFIG_SCAN_BACKOP + ss->scan_cnt = 0; +#endif +#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL) + ss->is_sw_antdiv_bl_scan = 0; +#endif + ss->ssid_num = 0; + for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) { + if (parm->ssid[i].SsidLength) { + _rtw_memcpy(ss->ssid[i].Ssid, parm->ssid[i].Ssid, IW_ESSID_MAX_SIZE); + ss->ssid[i].SsidLength = parm->ssid[i].SsidLength; + ss->ssid_num++; + } else + ss->ssid[i].SsidLength = 0; + } + reason = parm->reason; + ss->ch_num = rtw_scan_ch_decision(adapter + , ss->ch, RTW_CHANNEL_SCAN_AMOUNT + , parm->ch, parm->ch_num + , parm->acs + , reason + ); + + for (i = 0; i < MAX_CHANNEL_NUM; i++) + chset[i].hidden_bss_cnt = 0; + + ss->bw = parm->bw; + ss->igi = parm->igi; + ss->token = parm->token; + ss->duration = parm->duration; + ss->scan_mode = parm->scan_mode; + ss->token = parm->token; + ss->acs = parm->acs; +} + +static u8 sitesurvey_pick_ch_behavior(_adapter *padapter, u8 *ch, RT_SCAN_TYPE *type) +{ + u8 next_state; + u8 scan_ch = 0; + RT_SCAN_TYPE scan_type = SCAN_PASSIVE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct ss_res *ss = &pmlmeext->sitesurvey_res; + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + int ch_set_idx; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif +#ifdef CONFIG_SCAN_BACKOP + u8 backop_flags = 0; +#endif + + /* handle scan abort request */ + scan_abort_hdl(padapter); + +#ifdef CONFIG_P2P + if (pwdinfo->rx_invitereq_info.scan_op_ch_only || pwdinfo->p2p_info.scan_op_ch_only) { + if (pwdinfo->rx_invitereq_info.scan_op_ch_only) + scan_ch = pwdinfo->rx_invitereq_info.operation_ch[ss->channel_idx]; + else + scan_ch = pwdinfo->p2p_info.operation_ch[ss->channel_idx]; + scan_type = SCAN_ACTIVE; + } else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) { + /* + * Commented by Albert 2011/06/03 + * The driver is in the find phase, it should go through the social channel. + */ + scan_ch = pwdinfo->social_chan[ss->channel_idx]; + ch_set_idx = rtw_chset_search_ch(rfctl->channel_set, scan_ch); + if (ch_set_idx >= 0) + scan_type = rfctl->channel_set[ch_set_idx].flags & RTW_CHF_NO_IR ? SCAN_PASSIVE : SCAN_ACTIVE; + else + scan_type = SCAN_ACTIVE; + } else +#endif /* CONFIG_P2P */ + { + struct rtw_ieee80211_channel *ch; + + #ifdef CONFIG_SCAN_BACKOP + backop_flags = rtw_scan_backop_decision(padapter); + #endif + + #ifdef CONFIG_SCAN_BACKOP + if (!(backop_flags && ss->scan_cnt >= ss->scan_cnt_max)) + #endif + { + #ifdef CONFIG_RTW_WIFI_HAL + if (adapter_to_dvobj(padapter)->nodfs) { + while (ss->channel_idx < ss->ch_num && rtw_chset_is_dfs_ch(rfctl->channel_set, ss->ch[ss->channel_idx].hw_value)) + ss->channel_idx++; + } else + #endif + if (ss->channel_idx != 0 && ss->force_ssid_scan == 0 + && pmlmeext->sitesurvey_res.ssid_num + && (ss->ch[ss->channel_idx - 1].flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) + ) { + ch_set_idx = rtw_chset_search_ch(rfctl->channel_set, ss->ch[ss->channel_idx - 1].hw_value); + if (ch_set_idx != -1 && rfctl->channel_set[ch_set_idx].hidden_bss_cnt + && (!IS_DFS_SLAVE_WITH_RD(rfctl) + || rtw_rfctl_dfs_domain_unknown(rfctl) + || !CH_IS_NON_OCP(&rfctl->channel_set[ch_set_idx])) + ) { + ss->channel_idx--; + ss->force_ssid_scan = 1; + } + } else + ss->force_ssid_scan = 0; + } + + if (ss->channel_idx < ss->ch_num) { + ch = &ss->ch[ss->channel_idx]; + scan_ch = ch->hw_value; + + #if defined(CONFIG_RTW_ACS) && defined(CONFIG_RTW_ACS_DBG) + if (IS_ACS_ENABLE(padapter) && rtw_is_acs_passiv_scan(padapter)) + scan_type = SCAN_PASSIVE; + else + #endif /*CONFIG_RTW_ACS*/ + scan_type = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE; + } + } + + if (scan_ch != 0) { + next_state = SCAN_PROCESS; + + #ifdef CONFIG_SCAN_BACKOP + if (backop_flags) { + if (ss->scan_cnt < ss->scan_cnt_max) + ss->scan_cnt++; + else { + mlmeext_assign_scan_backop_flags(pmlmeext, backop_flags); + next_state = SCAN_BACKING_OP; + } + } + #endif + + } else if (rtw_p2p_findphase_ex_is_needed(pwdinfo)) { + /* go p2p listen */ + next_state = SCAN_TO_P2P_LISTEN; + +#ifdef CONFIG_ANTENNA_DIVERSITY + } else if (rtw_hal_antdiv_before_linked(padapter)) { + /* go sw antdiv before link */ + next_state = SCAN_SW_ANTDIV_BL; +#endif + } else { + next_state = SCAN_COMPLETE; + +#if defined(DBG_SCAN_SW_ANTDIV_BL) + { + /* for SCAN_SW_ANTDIV_BL state testing */ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + int i; + bool is_linked = _FALSE; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (rtw_linked_check(dvobj->padapters[i])) + is_linked = _TRUE; + } + + if (!is_linked) { + static bool fake_sw_antdiv_bl_state = 0; + + if (fake_sw_antdiv_bl_state == 0) { + next_state = SCAN_SW_ANTDIV_BL; + fake_sw_antdiv_bl_state = 1; + } else + fake_sw_antdiv_bl_state = 0; + } + } +#endif /* defined(DBG_SCAN_SW_ANTDIV_BL) */ + } + +#ifdef CONFIG_SCAN_BACKOP + if (next_state != SCAN_PROCESS) + ss->scan_cnt = 0; +#endif + + +#ifdef DBG_FIXED_CHAN + if (pmlmeext->fixed_chan != 0xff && next_state == SCAN_PROCESS) + scan_ch = pmlmeext->fixed_chan; +#endif + + if (ch) + *ch = scan_ch; + if (type) + *type = scan_type; + + return next_state; +} + +void site_survey(_adapter *padapter, u8 survey_channel, RT_SCAN_TYPE ScanType) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct ss_res *ss = &pmlmeext->sitesurvey_res; + u8 ssid_scan = 0; + +#ifdef CONFIG_P2P +#ifndef CONFIG_IOCTL_CFG80211 + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif +#endif + + if (survey_channel != 0) { + set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + + if (ScanType == SCAN_PASSIVE && ss->force_ssid_scan) + ssid_scan = 1; + else if (ScanType == SCAN_ACTIVE) { +#ifdef CONFIG_P2P + #ifdef CONFIG_IOCTL_CFG80211 + if (rtw_cfg80211_is_p2p_scan(padapter)) + #else + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) + || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) + #endif + { + issue_probereq_p2p(padapter, NULL); + issue_probereq_p2p(padapter, NULL); + issue_probereq_p2p(padapter, NULL); + } else +#endif /* CONFIG_P2P */ + { + if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) { + /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */ + if (padapter->registrypriv.wifi_spec) + issue_probereq(padapter, NULL, NULL); + else + issue_probereq_ex(padapter, NULL, NULL, 0, 0, 0, 0); + issue_probereq(padapter, NULL, NULL); + } + + ssid_scan = 1; + } + } + + if (ssid_scan) { + int i; + + for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) { + if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) { + /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */ + if (padapter->registrypriv.wifi_spec) + issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL); + else + issue_probereq_ex(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL, 0, 0, 0, 0); + issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL); + } + } + } + } else { + /* channel number is 0 or this channel is not valid. */ + rtw_warn_on(1); + } + + return; +} + +void survey_done_set_ch_bw(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + u8 cur_channel = 0; + u8 cur_bwmode; + u8 cur_ch_offset; + +#ifdef CONFIG_MCC_MODE + if (!rtw_hal_mcc_change_scan_flag(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset)) { + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to AP channel - ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset); + goto exit; + } +#endif + + if (rtw_mi_get_ch_setting_union(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset) != 0) { + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset); + } else { +#ifdef CONFIG_P2P + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + _adapter *iface; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + +#ifdef CONFIG_IOCTL_CFG80211 + if (iface->wdinfo.driver_interface == DRIVER_CFG80211 && !adapter_wdev_data(iface)->p2p_enabled) + continue; +#endif + + if (rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_LISTEN)) { + cur_channel = iface->wdinfo.listen_channel; + cur_bwmode = CHANNEL_WIDTH_20; + cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to "ADPT_FMT"'s listen ch - ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), ADPT_ARG(iface), cur_channel, cur_bwmode, cur_ch_offset); + break; + } + } +#endif /* CONFIG_P2P */ + + if (cur_channel == 0) { + cur_channel = pmlmeext->cur_channel; + cur_bwmode = pmlmeext->cur_bwmode; + cur_ch_offset = pmlmeext->cur_ch_offset; + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset); + } + } +#ifdef CONFIG_MCC_MODE +exit: +#endif + set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode); +} + +/** + * rtw_ps_annc - check and doing ps announcement for all the adapters + * @adapter: the requesting adapter + * @ps: power saving or not + * + * Returns: 0: no ps announcement is doing. 1: ps announcement is doing + */ +u8 rtw_ps_annc(_adapter *adapter, bool ps) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + _adapter *iface; + int i; + u8 ps_anc = 0; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface) + continue; + + if (MLME_IS_STA(iface)) { + if (is_client_associated_to_ap(iface) == _TRUE) { + /* TODO: TDLS peers */ + #ifdef CONFIG_MCC_MODE + /* for two station case */ + if (MCC_EN(adapter) && rtw_hal_check_mcc_status(adapter, MCC_STATUS_NEED_MCC)) { + u8 ch = iface->mlmeextpriv.cur_channel; + u8 offset = iface->mlmeextpriv.cur_ch_offset; + u8 bw = iface->mlmeextpriv.cur_bwmode; + + set_channel_bwmode(iface, ch, offset, bw); + } + #endif /* CONFIG_MCC_MODE */ + issue_nulldata(iface, NULL, ps, 3, 500); + ps_anc = 1; + } + #ifdef CONFIG_RTW_MESH + } else if (MLME_IS_MESH(iface)) { + if (rtw_mesh_ps_annc(iface, ps)) + ps_anc = 1; + #endif + } + } + return ps_anc; +} + +void rtw_leave_opch(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(adapter) && rtw_hal_check_mcc_status(adapter, MCC_STATUS_DOING_MCC)) + return; +#endif + + _enter_critical_mutex(&rfctl->offch_mutex, NULL); + + if (rfctl->offch_state == OFFCHS_NONE) { + /* prepare to leave operating channel */ + rfctl->offch_state = OFFCHS_LEAVING_OP; + + /* clear HW TX queue */ + rtw_hal_set_hwreg(adapter, HW_VAR_CHECK_TXBUF, 0); + + rtw_hal_macid_sleep_all_used(adapter); + + rtw_ps_annc(adapter, 1); + + rfctl->offch_state = OFFCHS_LEAVE_OP; + } + + _exit_critical_mutex(&rfctl->offch_mutex, NULL); +} + +void rtw_back_opch(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(adapter) && rtw_hal_check_mcc_status(adapter, MCC_STATUS_DOING_MCC)) + return; +#endif + + _enter_critical_mutex(&rfctl->offch_mutex, NULL); + + if (rfctl->offch_state != OFFCHS_NONE) { + rfctl->offch_state = OFFCHS_BACKING_OP; + rtw_hal_macid_wakeup_all_used(adapter); + rtw_ps_annc(adapter, 0); + + rfctl->offch_state = OFFCHS_NONE; + rtw_mi_os_xmit_schedule(adapter); + } + + _exit_critical_mutex(&rfctl->offch_mutex, NULL); +} + +void sitesurvey_set_igi(_adapter *adapter) +{ + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + struct ss_res *ss = &mlmeext->sitesurvey_res; + u8 igi; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &adapter->wdinfo; +#endif + + switch (mlmeext_scan_state(mlmeext)) { + case SCAN_ENTER: + #ifdef CONFIG_P2P + #ifdef CONFIG_IOCTL_CFG80211 + if (pwdinfo->driver_interface == DRIVER_CFG80211 && rtw_cfg80211_is_p2p_scan(adapter)) + igi = 0x30; + else + #endif /* CONFIG_IOCTL_CFG80211 */ + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + igi = 0x28; + else + #endif /* CONFIG_P2P */ + + if (ss->igi) + igi = ss->igi; + else + #if defined(CONFIG_RTW_ACS) && defined(CONFIG_RTW_ACS_DBG) + if (IS_ACS_ENABLE(adapter) && rtw_is_acs_igi_valid(adapter)) + igi = rtw_acs_get_adv_igi(adapter); + else + #endif /*CONFIG_RTW_ACS*/ + igi = 0x1e; + + /* record IGI status */ + ss->igi_scan = igi; + rtw_hal_get_odm_var(adapter, HAL_ODM_INITIAL_GAIN, &ss->igi_before_scan, NULL); + + /* disable DIG and set IGI for scan */ + rtw_hal_set_odm_var(adapter, HAL_ODM_INITIAL_GAIN, &igi, _FALSE); + break; + case SCAN_COMPLETE: + case SCAN_TO_P2P_LISTEN: + /* enable DIG and restore IGI */ + igi = 0xff; + rtw_hal_set_odm_var(adapter, HAL_ODM_INITIAL_GAIN, &igi, _FALSE); + break; +#ifdef CONFIG_SCAN_BACKOP + case SCAN_BACKING_OP: + /* write IGI for op channel when DIG is not enabled */ + odm_write_dig(adapter_to_phydm(adapter), ss->igi_before_scan); + break; + case SCAN_LEAVE_OP: + /* write IGI for scan when DIG is not enabled */ + odm_write_dig(adapter_to_phydm(adapter), ss->igi_scan); + break; +#endif /* CONFIG_SCAN_BACKOP */ + default: + rtw_warn_on(1); + break; + } +} +void sitesurvey_set_msr(_adapter *adapter, bool enter) +{ + u8 network_type; + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (enter) { +#ifdef CONFIG_MI_WITH_MBSSID_CAM + rtw_hal_get_hwreg(adapter, HW_VAR_MEDIA_STATUS, (u8 *)(&pmlmeinfo->hw_media_state)); +#endif + /* set MSR to no link state */ + network_type = _HW_STATE_NOLINK_; + } else { +#ifdef CONFIG_MI_WITH_MBSSID_CAM + network_type = pmlmeinfo->hw_media_state; +#else + network_type = pmlmeinfo->state & 0x3; +#endif + } + Set_MSR(adapter, network_type); +} + +void sitesurvey_set_offch_state(_adapter *adapter, u8 scan_state) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + + _enter_critical_mutex(&rfctl->offch_mutex, NULL); + + switch (scan_state) { + case SCAN_DISABLE: + case SCAN_BACK_OP: + rfctl->offch_state = OFFCHS_NONE; + break; + case SCAN_START: + case SCAN_LEAVING_OP: + rfctl->offch_state = OFFCHS_LEAVING_OP; + break; + case SCAN_ENTER: + case SCAN_LEAVE_OP: + rfctl->offch_state = OFFCHS_LEAVE_OP; + break; + case SCAN_COMPLETE: + case SCAN_BACKING_OP: + rfctl->offch_state = OFFCHS_BACKING_OP; + break; + default: + break; + } + + _exit_critical_mutex(&rfctl->offch_mutex, NULL); +} + +#ifdef CONFIG_RTW_ROAM_QUICKSCAN +extern inline int is_same_ess(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b); +extern int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork); +void generate_quickss(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct wlan_network *pnetwork = NULL; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct rtw_ieee80211_channel *roam_ch = pmlmeext->roam_ch; + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + int chan; + _irqL irqL; + _list *plist, *phead; + u8 *target_ssid=NULL, *ssid=NULL, ds_cfg, j, ch_num; + u32 target_ssid_len=0, ssid_len=0; + u8 mark[166]; + + target_ssid = pmlmepriv->cur_network.network.Ssid.Ssid; + target_ssid_len = pmlmepriv->cur_network.network.Ssid.SsidLength; + _rtw_memset(mark, 0, sizeof(u8)*166); + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + phead = get_list_head(&(pmlmepriv->scanned_queue)); + if (!phead) { + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + return; + } + plist = get_next(phead); + if (!plist) { + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + return; + } + while (1) { + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + if (!pnetwork) + break; + + ds_cfg = pnetwork->network.Configuration.DSConfig; + if (is_same_ess(&pnetwork->network, &pmlmepriv->cur_network.network) && + rtw_is_desired_network(padapter, pnetwork)) + mark[ds_cfg] = 1; + + plist = get_next(plist); + } + /* ex: assume roaming channel=1/6/40/165, then mark[1]/[6]/[40]/[165] are 1. */ + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + ch_num = 0; + for (j = 0; j < rfctl->max_chan_nums; j++) { + chan = rfctl->channel_set[j].ChannelNum; + if (mark[chan]) { + roam_ch[ch_num].hw_value = chan; + roam_ch[ch_num++].flags = rfctl->channel_set[j].flags; + } + } + pmlmeext->roam_ch_num = ch_num; +} +#endif + +u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf) +{ + struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf; +#ifdef DBG_CHECK_FW_PS_STATE + struct dvobj_priv *dvobj = padapter->dvobj; + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; +#endif + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct ss_res *ss = &pmlmeext->sitesurvey_res; +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); +#endif + u8 val8; + +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#ifdef CONFIG_CONCURRENT_MODE + struct roch_info *prochinfo = &padapter->rochinfo; +#endif +#endif + +#ifdef DBG_CHECK_FW_PS_STATE + if (rtw_fw_ps_state(padapter) == _FAIL) { + RTW_INFO("scan without leave 32k\n"); + pdbgpriv->dbg_scan_pwr_state_cnt++; + } +#endif /* DBG_CHECK_FW_PS_STATE */ + + /* increase channel idx */ + if (mlmeext_chk_scan_state(pmlmeext, SCAN_PROCESS)) + ss->channel_idx++; + + /* update scan state to next state (assigned by previous cmd hdl) */ + if (mlmeext_scan_state(pmlmeext) != mlmeext_scan_next_state(pmlmeext)) + mlmeext_set_scan_state(pmlmeext, mlmeext_scan_next_state(pmlmeext)); + +operation_by_state: + switch (mlmeext_scan_state(pmlmeext)) { + + case SCAN_DISABLE: + /* + * SW parameter initialization + */ + + sitesurvey_res_reset(padapter, pparm); + mlmeext_set_scan_state(pmlmeext, SCAN_START); + goto operation_by_state; + + case SCAN_START: +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + if ((pwdev_priv->pno_mac_addr[0] != 0xFF) + && (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) == _TRUE) + && (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == _FALSE)) { + u16 seq_num; + +#ifdef CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI + rtw_hal_pno_random_gen_mac_addr(padapter); +#endif + rtw_hal_set_hw_mac_addr(padapter, pwdev_priv->pno_mac_addr); + get_random_bytes(&seq_num, 2); + pwdev_priv->pno_scan_seq_num = seq_num & 0xFFF; + RTW_INFO("%s pno_scan_seq_num %d\n", __func__, + pwdev_priv->pno_scan_seq_num); + } +#endif + + /* + * prepare to leave operating channel + */ + +#ifdef CONFIG_MCC_MODE + rtw_hal_set_mcc_setting_scan_start(padapter); +#endif /* CONFIG_MCC_MODE */ + + /* apply rx ampdu setting */ + if (ss->rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID + || ss->rx_ampdu_size != RX_AMPDU_SIZE_INVALID) + rtw_rx_ampdu_apply(padapter); + + /* clear HW TX queue before scan */ + rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0); + + rtw_hal_macid_sleep_all_used(padapter); + + /* power save state announcement */ + if (rtw_ps_annc(padapter, 1)) { + mlmeext_set_scan_state(pmlmeext, SCAN_PS_ANNC_WAIT); + mlmeext_set_scan_next_state(pmlmeext, SCAN_ENTER); + set_survey_timer(pmlmeext, 50); /* delay 50ms to protect nulldata(1) */ + } else { + mlmeext_set_scan_state(pmlmeext, SCAN_ENTER); + goto operation_by_state; + } + + break; + + case SCAN_ENTER: + /* + * HW register and DM setting for enter scan + */ + + rtw_phydm_ability_backup(padapter); + + sitesurvey_set_igi(padapter); + + /* config dynamic functions for off channel */ + rtw_phydm_func_for_offchannel(padapter); + /* set MSR to no link state */ + sitesurvey_set_msr(padapter, _TRUE); + + val8 = 1; /* under site survey */ + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + mlmeext_set_scan_state(pmlmeext, SCAN_PROCESS); + goto operation_by_state; + + case SCAN_PROCESS: { + u8 scan_ch; + RT_SCAN_TYPE scan_type; + u8 next_state; + u32 scan_ms; + +#ifdef CONFIG_RTW_ACS + if (IS_ACS_ENABLE(padapter)) + rtw_acs_get_rst(padapter); +#endif + + next_state = sitesurvey_pick_ch_behavior(padapter, &scan_ch, &scan_type); + + if (next_state != SCAN_PROCESS) { + mlmeext_set_scan_state(pmlmeext, next_state); + goto operation_by_state; + } + + /* still SCAN_PROCESS state */ + #ifdef DBG_SITESURVEY + #ifdef CONFIG_P2P + RTW_INFO(FUNC_ADPT_FMT" %s ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c%c\n" + , FUNC_ADPT_ARG(padapter) + , mlmeext_scan_state_str(pmlmeext) + , scan_ch + , pwdinfo->find_phase_state_exchange_cnt, ss->channel_idx + , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time) + , scan_type ? 'A' : 'P', ss->scan_mode ? 'A' : 'P' + , ss->ssid[0].SsidLength ? 'S' : ' ' + , ss->force_ssid_scan ? 'F' : ' ' + ); + #else + RTW_INFO(FUNC_ADPT_FMT" %s ch:%u (idx:%d) at %dms, %c%c%c%c\n" + , FUNC_ADPT_ARG(padapter) + , mlmeext_scan_state_str(pmlmeext) + , scan_ch + , ss->channel_idx + , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time) + , scan_type ? 'A' : 'P', ss->scan_mode ? 'A' : 'P' + , ss->ssid[0].SsidLength ? 'S' : ' ' + , ss->force_ssid_scan ? 'F' : ' ' + ); + #endif /* CONFIG_P2P */ + #endif /*DBG_SITESURVEY*/ +#ifdef DBG_FIXED_CHAN + if (pmlmeext->fixed_chan != 0xff) + RTW_INFO(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan); +#endif + + site_survey(padapter, scan_ch, scan_type); + +#if defined(CONFIG_ATMEL_RC_PATCH) + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + scan_ms = 20; + else + scan_ms = 40; +#else + #if defined(CONFIG_RTW_ACS) && defined(CONFIG_RTW_ACS_DBG) + if (IS_ACS_ENABLE(padapter) && rtw_is_acs_st_valid(padapter)) + scan_ms = rtw_acs_get_adv_st(padapter); + else + #endif /*CONFIG_RTW_ACS*/ + scan_ms = ss->scan_ch_ms; +#endif + +#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL) + if (ss->is_sw_antdiv_bl_scan) + scan_ms = scan_ms / 2; +#endif + +#ifdef CONFIG_RTW_ACS + if (IS_ACS_ENABLE(padapter)) { + if (pparm->token) + rtw_acs_trigger(padapter, scan_ms, scan_ch, NHM_PID_IEEE_11K_HIGH); + else + rtw_acs_trigger(padapter, scan_ms, scan_ch, NHM_PID_ACS); + } +#endif + +#ifdef CONFIG_BACKGROUND_NOISE_MONITOR + if (IS_NM_ENABLE(padapter)) + rtw_noise_measure(padapter, scan_ch, _FALSE, 0, scan_ms / 2); +#endif + set_survey_timer(pmlmeext, scan_ms); + break; + } + +#ifdef CONFIG_SCAN_BACKOP + case SCAN_BACKING_OP: { + u8 back_ch, back_bw, back_ch_offset; + u8 need_ch_setting_union = _TRUE; + +#ifdef CONFIG_MCC_MODE + need_ch_setting_union = rtw_hal_mcc_change_scan_flag(padapter, + &back_ch, &back_bw, &back_ch_offset); +#endif /* CONFIG_MCC_MODE */ + + if (need_ch_setting_union) { + if (rtw_mi_get_ch_setting_union(padapter, &back_ch, &back_bw, &back_ch_offset) == 0) { + rtw_warn_on(1); + back_ch = pmlmeext->cur_channel; + back_bw = pmlmeext->cur_bwmode; + back_ch_offset = pmlmeext->cur_ch_offset; + } + } + + #ifdef DBG_SITESURVEY + RTW_INFO(FUNC_ADPT_FMT" %s ch:%u, bw:%u, offset:%u at %dms\n" + , FUNC_ADPT_ARG(padapter) + , mlmeext_scan_state_str(pmlmeext) + , back_ch, back_bw, back_ch_offset + , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time) + ); + #endif /*DBG_SITESURVEY*/ + set_channel_bwmode(padapter, back_ch, back_ch_offset, back_bw); + + sitesurvey_set_msr(padapter, _FALSE); + + val8 = 0; /* survey done */ + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_PS_ANNC)) { + sitesurvey_set_igi(padapter); + rtw_hal_macid_wakeup_all_used(padapter); + rtw_ps_annc(padapter, 0); + } + + mlmeext_set_scan_state(pmlmeext, SCAN_BACK_OP); + ss->backop_time = rtw_get_current_time(); + + if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_TX_RESUME)) + rtw_mi_os_xmit_schedule(padapter); + + + goto operation_by_state; + } + + case SCAN_BACK_OP: + if (rtw_get_passing_time_ms(ss->backop_time) >= ss->backop_ms + || pmlmeext->scan_abort + ) { + mlmeext_set_scan_state(pmlmeext, SCAN_LEAVING_OP); + goto operation_by_state; + } + set_survey_timer(pmlmeext, 50); + break; + + case SCAN_LEAVING_OP: + /* + * prepare to leave operating channel + */ + + /* clear HW TX queue before scan */ + rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0); + + rtw_hal_macid_sleep_all_used(padapter); + if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_PS_ANNC) + && rtw_ps_annc(padapter, 1) + ) { + mlmeext_set_scan_state(pmlmeext, SCAN_PS_ANNC_WAIT); + mlmeext_set_scan_next_state(pmlmeext, SCAN_LEAVE_OP); + set_survey_timer(pmlmeext, 50); /* delay 50ms to protect nulldata(1) */ + } else { + mlmeext_set_scan_state(pmlmeext, SCAN_LEAVE_OP); + goto operation_by_state; + } + + break; + + case SCAN_LEAVE_OP: + /* + * HW register and DM setting for enter scan + */ + + if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_PS_ANNC)) + sitesurvey_set_igi(padapter); + + sitesurvey_set_msr(padapter, _TRUE); + + val8 = 1; /* under site survey */ + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + mlmeext_set_scan_state(pmlmeext, SCAN_PROCESS); + goto operation_by_state; + +#endif /* CONFIG_SCAN_BACKOP */ + +#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL) + case SCAN_SW_ANTDIV_BL: + /* + * 20100721 + * For SW antenna diversity before link, it needs to switch to another antenna and scan again. + * It compares the scan result and select better one to do connection. + */ + ss->bss_cnt = 0; + ss->channel_idx = 0; + ss->is_sw_antdiv_bl_scan = 1; + + mlmeext_set_scan_next_state(pmlmeext, SCAN_PROCESS); + set_survey_timer(pmlmeext, ss->scan_ch_ms); + break; +#endif + +#ifdef CONFIG_P2P + case SCAN_TO_P2P_LISTEN: + /* + * Set the P2P State to the listen state of find phase + * and set the current channel to the listen channel + */ + set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN); + + /* turn on phy-dynamic functions */ + rtw_phydm_ability_restore(padapter); + + sitesurvey_set_igi(padapter); + + mlmeext_set_scan_state(pmlmeext, SCAN_P2P_LISTEN); + _set_timer(&pwdinfo->find_phase_timer, (u32)((u32)pwdinfo->listen_dwell * 100)); + break; + + case SCAN_P2P_LISTEN: + mlmeext_set_scan_state(pmlmeext, SCAN_PROCESS); + ss->channel_idx = 0; + goto operation_by_state; +#endif /* CONFIG_P2P */ + + case SCAN_COMPLETE: +#if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + rtw_hal_set_hw_mac_addr(padapter, adapter_mac_addr(padapter)); +#endif +#ifdef CONFIG_P2P + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) + || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) + ) { +#ifdef CONFIG_CONCURRENT_MODE + if (pwdinfo->driver_interface == DRIVER_WEXT) { + if (rtw_mi_check_status(padapter, MI_LINKED)) + _set_timer(&prochinfo->ap_roch_ch_switch_timer, 500); + } +#endif + + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + } + rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE); +#endif /* CONFIG_P2P */ + + /* switch channel */ + survey_done_set_ch_bw(padapter); + + sitesurvey_set_msr(padapter, _FALSE); + + val8 = 0; /* survey done */ + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + + /* turn on phy-dynamic functions */ + rtw_phydm_ability_restore(padapter); + + sitesurvey_set_igi(padapter); + +#ifdef CONFIG_MCC_MODE + /* start MCC fail, then tx null data */ + if (!rtw_hal_set_mcc_setting_scan_complete(padapter)) +#endif + { + rtw_hal_macid_wakeup_all_used(padapter); + rtw_ps_annc(padapter, 0); + } + + /* apply rx ampdu setting */ + rtw_rx_ampdu_apply(padapter); + + mlmeext_set_scan_state(pmlmeext, SCAN_DISABLE); + + report_surveydone_event(padapter, ss->acs); +#ifdef CONFIG_RTW_ACS + if (IS_ACS_ENABLE(padapter)) + rtw_acs_select_best_chan(padapter); +#endif + +#if defined(CONFIG_BACKGROUND_NOISE_MONITOR) && defined(DBG_NOISE_MONITOR) + if (IS_NM_ENABLE(padapter)) + rtw_noise_info_dump(RTW_DBGDUMP, padapter); +#endif + issue_action_BSSCoexistPacket(padapter); + issue_action_BSSCoexistPacket(padapter); + issue_action_BSSCoexistPacket(padapter); + +#ifdef CONFIG_RTW_80211K + if (ss->token) + rm_post_event(padapter, ss->token, RM_EV_survey_done); +#endif /* CONFIG_RTW_80211K */ +#ifdef CONFIG_RTW_ROAM_QUICKSCAN + if (padapter->mlmepriv.need_to_roam == _TRUE) + generate_quickss(padapter); +#endif + break; + } + + return H2C_SUCCESS; +} + +u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct setauth_parm *pparm = (struct setauth_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pparm->mode < 4) + pmlmeinfo->auth_algo = pparm->mode; + + return H2C_SUCCESS; +} + +static u8 amsdu_spp_enable(_adapter *pdapter, enum security_type type) +{ + u8 ret = _FALSE; + + if (pdapter->registrypriv.amsdu_mode == RTW_AMSDU_MODE_SPP) { + if ( type == _AES_ || type == _CCMP_256_ + || type == _GCMP_ || type == _GCMP_256_ ) + ret = _SUCCESS; + } + + return ret; +} + +/* +SEC CAM Entry format (32 bytes) +DW0 - MAC_ADDR[15:0] | Valid[15] | MFB[14:8] | RSVD[7] | GK[6] | MIC_KEY[5] | SEC_TYPE[4:2] | KID[1:0] +DW0 - MAC_ADDR[15:0] | Valid[15] |RSVD[14:9] | RPT_MODE[8] | SPP_MODE[7] | GK[6] | MIC_KEY[5] | SEC_TYPE[4:2] | KID[1:0] (92E/8812A/8814A) +DW1 - MAC_ADDR[47:16] +DW2 - KEY[31:0] +DW3 - KEY[63:32] +DW4 - KEY[95:64] +DW5 - KEY[127:96] +DW6 - RSVD +DW7 - RSVD +*/ + +/*Set WEP key or Group Key*/ +u8 setkey_hdl(_adapter *padapter, u8 *pbuf) +{ + u16 ctrl = 0; + s16 cam_id = 0; + struct setkey_parm *pparm = (struct setkey_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + unsigned char null_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + u8 *addr; + bool used = _FALSE; + + /* main tx key for wep. */ + if (pparm->set_tx) + pmlmeinfo->key_index = pparm->keyid; + +#ifndef SEC_DEFAULT_KEY_SEARCH + if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) + cam_id = rtw_iface_bcmc_id_get(padapter); + else +#endif + cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid, 1, + !!(pparm->algorithm & _SEC_TYPE_256_), &used); + + if (cam_id < 0) + goto enable_mc; + +#ifdef SEC_DEFAULT_KEY_SEARCH + if (cam_id >= 0 && cam_id <= 3) { + /* default key camid */ + addr = null_addr; + } else +#endif + { + /* not default key camid */ + if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) { + /* group TX, force sec cam entry_id */ + addr = adapter_mac_addr(padapter); + } else { + /* group RX, searched by A2 (TA) */ + addr = get_bssid(&padapter->mlmepriv); + } + } + +#ifdef CONFIG_LPS_PG + if (adapter_to_pwrctl(padapter)->lps_level == LPS_PG) + LPS_Leave(padapter, "SET_KEY"); +#endif + + /* cam entry searched is pairwise key */ + if (used == _TRUE && rtw_camid_is_gk(padapter, cam_id) == _FALSE) { + s16 camid_clr; + + RTW_PRINT(FUNC_ADPT_FMT" group key with "MAC_FMT" id:%u the same key id as pairwise key\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(addr), pparm->keyid); + + /* HW has problem to distinguish this group key with existing pairwise key, stop HW enc and dec for BMC */ + rtw_camctl_set_flags(padapter, SEC_STATUS_STA_PK_GK_CONFLICT_DIS_BMC_SEARCH); + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, NULL); + + /* clear group key */ + while ((camid_clr = rtw_camid_search(padapter, addr, -1, 1)) >= 0) { + RTW_PRINT("clear group key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(addr), camid_clr); + clear_cam_entry(padapter, camid_clr); + rtw_camid_free(padapter, camid_clr); + } + + goto enable_mc; + } + + ctrl = BIT(15) | BIT(6) | ((pparm->algorithm & 0x07) << 2) | pparm->keyid; + + RTW_PRINT("set group key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n" + , cam_id, MAC_ARG(addr), pparm->keyid, security_type_str(pparm->algorithm)); + + if (pparm->algorithm & _SEC_TYPE_256_) { + RTW_INFO_DUMP("GTK : ", pparm->key, sizeof(pparm->key)); + ctrl |= BIT(9); + } + + if (amsdu_spp_enable(padapter, pparm->algorithm) == _SUCCESS) + ctrl |= BIT(7); + write_cam(padapter, cam_id, ctrl, addr, pparm->key); + + /* if ((cam_id > 3) && (((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)))*/ +#ifndef SEC_DEFAULT_KEY_SEARCH + if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) { + if (is_wep_enc(pparm->algorithm)) { + padapter->securitypriv.dot11Def_camid[pparm->keyid] = cam_id; + padapter->securitypriv.dot118021x_bmc_cam_id = + padapter->securitypriv.dot11Def_camid[padapter->securitypriv.dot11PrivacyKeyIndex]; + RTW_PRINT("wep group key - force camid:%d\n", padapter->securitypriv.dot118021x_bmc_cam_id); + } else { + /*u8 org_cam_id = padapter->securitypriv.dot118021x_bmc_cam_id;*/ + + /*force GK's cam id*/ + padapter->securitypriv.dot118021x_bmc_cam_id = cam_id; + + /* for GTK rekey + if ((org_cam_id != INVALID_SEC_MAC_CAM_ID) && + (org_cam_id != cam_id)) { + RTW_PRINT("clear group key for addr:"MAC_FMT", org_camid:%d new_camid:%d\n", MAC_ARG(addr), org_cam_id, cam_id); + clear_cam_entry(padapter, org_cam_id); + rtw_camid_free(padapter, org_cam_id); + }*/ + } + } +#else + if (cam_id >= 0 && cam_id <= 3) + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)_TRUE); +#endif + + /* 8814au should set both broadcast and unicast CAM entry for WEP key in STA mode */ + if (is_wep_enc(pparm->algorithm) && check_mlmeinfo_state(pmlmeext, WIFI_FW_STATION_STATE) && + _rtw_camctl_chk_cap(padapter, SEC_CAP_CHK_BMC)) { + struct set_stakey_parm sta_pparm; + + _rtw_memset(&sta_pparm, 0, sizeof(struct set_stakey_parm)); + sta_pparm.algorithm = pparm->algorithm; + sta_pparm.keyid = pparm->keyid; + _rtw_memcpy(sta_pparm.key, pparm->key, 16); + _rtw_memcpy(sta_pparm.addr, get_bssid(&padapter->mlmepriv), ETH_ALEN); + set_stakey_hdl(padapter, (u8 *)&sta_pparm); + } + +enable_mc: + /* allow multicast packets to driver */ + rtw_hal_set_hwreg(padapter, HW_VAR_ON_RCR_AM, null_addr); +#ifdef CONFIG_LAYER2_ROAMING + if (padapter->mlmepriv.roam_network) { + struct xmit_frame *rframe; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _list *plist, *phead; + _irqL irqL; + _pkt *pkt; + + padapter->mlmepriv.roam_network = NULL; + _enter_critical_bh(&pxmitpriv->rpkt_queue.lock, &irqL); + phead = get_list_head(&pxmitpriv->rpkt_queue); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + rframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + plist = get_next(plist); + rtw_list_delete(&rframe->list); + pkt = rframe->pkt; + rframe->pkt = NULL; + rtw_xmit_posthandle(padapter, rframe, pkt); + } + _exit_critical_bh(&pxmitpriv->rpkt_queue.lock, &irqL); + } +#endif + return H2C_SUCCESS; +} + +void rtw_ap_wep_pk_setting(_adapter *adapter, struct sta_info *psta) +{ + struct security_priv *psecuritypriv = &(adapter->securitypriv); + struct set_stakey_parm sta_pparm; + sint keyid; + + if (!is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm)) + return; + + for (keyid = 0; keyid < 4; keyid++) { + if ((psecuritypriv->key_mask & BIT(keyid)) && (keyid == psecuritypriv->dot11PrivacyKeyIndex)) { + sta_pparm.algorithm = psecuritypriv->dot11PrivacyAlgrthm; + sta_pparm.keyid = keyid; + sta_pparm.gk = 0; + _rtw_memcpy(sta_pparm.key, &(psecuritypriv->dot11DefKey[keyid].skey[0]), 16); + _rtw_memcpy(sta_pparm.addr, psta->cmn.mac_addr, ETH_ALEN); + + RTW_PRINT(FUNC_ADPT_FMT"set WEP - PK with "MAC_FMT" keyid:%u\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr), keyid); + + set_stakey_hdl(adapter, (u8 *)&sta_pparm); + } + } +} + +u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf) +{ + u16 ctrl = 0; + s16 cam_id = 0; + bool used; + u8 ret = H2C_SUCCESS; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct set_stakey_parm *pparm = (struct set_stakey_parm *)pbuf; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + + if (pparm->algorithm == _NO_PRIVACY_) + goto write_to_cam; + + psta = rtw_get_stainfo(pstapriv, pparm->addr); + if (!psta) { + RTW_PRINT("%s sta:"MAC_FMT" not found\n", __func__, MAC_ARG(pparm->addr)); + ret = H2C_REJECTED; + goto exit; + } + + pmlmeinfo->enc_algo = pparm->algorithm; + + cam_id = rtw_camid_alloc(padapter, psta, pparm->keyid, pparm->gk, + !!(pparm->algorithm & _SEC_TYPE_256_), &used); + if (cam_id < 0) + goto exit; + +#ifdef CONFIG_LPS_PG + if (adapter_to_pwrctl(padapter)->lps_level == LPS_PG) + LPS_Leave(padapter, "SET_KEY"); +#endif + + /* cam entry searched is group key when setting pariwise key */ + if (!pparm->gk && used == _TRUE && rtw_camid_is_gk(padapter, cam_id) == _TRUE) { + s16 camid_clr; + + RTW_PRINT(FUNC_ADPT_FMT" pairwise key with "MAC_FMT" id:%u the same key id as group key\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(pparm->addr), pparm->keyid); + + /* HW has problem to distinguish this pairwise key with existing group key, stop HW enc and dec for BMC */ + rtw_camctl_set_flags(padapter, SEC_STATUS_STA_PK_GK_CONFLICT_DIS_BMC_SEARCH); + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, NULL); + + /* clear group key */ + while ((camid_clr = rtw_camid_search(padapter, pparm->addr, -1, 1)) >= 0) { + RTW_PRINT("clear group key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), camid_clr); + clear_cam_entry(padapter, camid_clr); + rtw_camid_free(padapter, camid_clr); + } + } + +write_to_cam: + if (pparm->algorithm == _NO_PRIVACY_) { + while ((cam_id = rtw_camid_search(padapter, pparm->addr, -1, -1)) >= 0) { + RTW_PRINT("clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), cam_id); + clear_cam_entry(padapter, cam_id); + rtw_camid_free(padapter, cam_id); + } + } else { + RTW_PRINT("set %s key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n" + , pparm->gk ? "group" : "pairwise" + , cam_id, MAC_ARG(pparm->addr), pparm->keyid, security_type_str(pparm->algorithm)); + ctrl = BIT(15) | ((pparm->algorithm & 0x07) << 2) | pparm->keyid; + if (pparm->gk) + ctrl |= BIT(6); + if (pparm->algorithm & _SEC_TYPE_256_) { + RTW_INFO_DUMP("PTK : ", pparm->key, sizeof(pparm->key)); + ctrl |= BIT(9); + } + if (amsdu_spp_enable(padapter, pparm->algorithm) == _SUCCESS) + ctrl |= BIT(7); + write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key); + if (!(pparm->gk)) + ATOMIC_INC(&psta->keytrack); /*CVE-2020-24587*/ + } + ret = H2C_SUCCESS_RSP; + +exit: + return ret; +} + +u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr); + + if (!psta) + return H2C_SUCCESS; + +#ifdef CONFIG_80211N_HT + if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) || + ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) { + /* pmlmeinfo->ADDBA_retry_count = 0; */ + /* pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid); */ + /* psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid); */ + issue_addba_req(padapter, pparm->addr, (u8)pparm->tid); + _set_timer(&psta->addba_retry_timer, ADDBA_TO); + } +#ifdef CONFIG_TDLS + else if ((psta->tdls_sta_state & TDLS_LINKED_STATE) && + (psta->htpriv.ht_option == _TRUE) && + (psta->htpriv.ampdu_enable == _TRUE)) { + issue_addba_req(padapter, pparm->addr, (u8)pparm->tid); + _set_timer(&psta->addba_retry_timer, ADDBA_TO); + } +#endif /* CONFIG */ + else + psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid); +#endif /* CONFIG_80211N_HT */ + return H2C_SUCCESS; +} + + +u8 add_ba_rsp_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct addBaRsp_parm *pparm = (struct addBaRsp_parm *)pbuf; + struct recv_reorder_ctrl *preorder_ctrl; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + u8 ret = _TRUE; + + psta = rtw_get_stainfo(pstapriv, pparm->addr); + if (!psta) + goto exit; + + preorder_ctrl = &psta->recvreorder_ctrl[pparm->tid]; + ret = issue_addba_rsp_wait_ack(padapter, pparm->addr, pparm->tid, pparm->status, pparm->size, 3, 50); + +#ifdef CONFIG_UPDATE_INDICATE_SEQ_WHILE_PROCESS_ADDBA_REQ + /* status = 0 means accept this addba req, so update indicate seq = start_seq under this compile flag */ + if (pparm->status == 0) { + preorder_ctrl->indicate_seq = pparm->start_seq; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_UPDATE indicate_seq:%d, start_seq:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, pparm->start_seq); + #endif + } +#else + rtw_set_bit(RTW_RECV_ACK_OR_TIMEOUT, &preorder_ctrl->rec_abba_rsp_ack); + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_CLEAR indicate_seq:%d, start_seq:%d preorder_ctrl->rec_abba_rsp_ack =%lu \n" + , FUNC_ADPT_ARG(padapter) + , preorder_ctrl->tid + , preorder_ctrl->indicate_seq + , pparm->start_seq + ,preorder_ctrl->rec_abba_rsp_ack + ); + #endif +#endif + + /* + * status = 0 means accept this addba req + * status = 37 means reject this addba req + */ + if (pparm->status == 0) { + preorder_ctrl->enable = _TRUE; + preorder_ctrl->ampdu_size = pparm->size; + } else if (pparm->status == 37) + preorder_ctrl->enable = _FALSE; + +exit: + return H2C_SUCCESS; +} + +u8 chk_bmc_sleepq_cmd(_adapter *padapter) +{ + struct cmd_obj *ph2c; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + u8 res = _SUCCESS; + + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + init_h2fwcmd_w_parm_no_parm_rsp(ph2c, CMD_CHK_BMCSLEEPQ); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + + + return res; +} + +u8 set_tx_beacon_cmd(_adapter *padapter, u8 flags) +{ + struct cmd_obj *ph2c; + struct Tx_Beacon_param *ptxBeacon_parm; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct submit_ctx sctx; + u8 res = _SUCCESS; + int len_diff = 0; + + /*prepare cmd parameter*/ + ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param)); + if (ptxBeacon_parm == NULL) { + res = _FAIL; + goto exit; + } + + _rtw_memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX)); + + len_diff = update_hidden_ssid( + ptxBeacon_parm->network.IEs + _BEACON_IE_OFFSET_ + , ptxBeacon_parm->network.IELength - _BEACON_IE_OFFSET_ + , pmlmeinfo->hidden_ssid_mode + ); + ptxBeacon_parm->network.IELength += len_diff; + + + /* need enqueue, prepare cmd_obj and enqueue */ + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + rtw_mfree((u8 *)ptxBeacon_parm, sizeof(*ptxBeacon_parm)); + goto exit; + } + + init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, CMD_TX_BEACON); + + if (flags & RTW_CMDF_WAIT_ACK) { + ph2c->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + ph2c->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + } + + +exit: + + + return res; +} + + +u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct rtw_evt_header *evt_hdr; + u8 *peventbuf; + void (*event_callback)(_adapter *dev, u8 *pbuf); + struct evt_priv *pevt_priv = &(padapter->evtpriv); + + if (pbuf == NULL) + goto _abort_event_; + + evt_hdr = (struct rtw_evt_header *)pbuf; + peventbuf = pbuf + sizeof(struct rtw_evt_header); + +#ifdef CHECK_EVENT_SEQ + /* checking event sequence... */ + if (evt_hdr->seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f)) { + pevt_priv->event_seq = (evt_hdr->seq + 1) & 0x7f; + goto _abort_event_; + } +#endif + + /* checking if event code is valid */ + if (evt_hdr->id >= EVT_ID_MAX) { + goto _abort_event_; + } + + /* checking if event size match the event parm size */ + if ((wlanevents[evt_hdr->id].parmsize != 0) && + (wlanevents[evt_hdr->id].parmsize != evt_hdr->len)) { + + goto _abort_event_; + + } + + ATOMIC_INC(&pevt_priv->event_seq); + + if (peventbuf) { + event_callback = wlanevents[evt_hdr->id].event_callback; + event_callback(padapter, (u8 *)peventbuf); + pevt_priv->evt_done_cnt++; + } + +_abort_event_: + return H2C_SUCCESS; + +} + +u8 chk_bmc_sleepq_hdl(_adapter *padapter, unsigned char *pbuf) +{ +#ifdef CONFIG_AP_MODE + _irqL irqL; + struct sta_info *psta_bmc; + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + + /* for BC/MC Frames */ + psta_bmc = rtw_get_bcmc_stainfo(padapter); + if (!psta_bmc) + return H2C_SUCCESS; + + if ((rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, 0)) && (psta_bmc->sleepq_len > 0)) { +#ifndef CONFIG_PCI_HCI + rtw_msleep_os(10);/* 10ms, ATIM(HIQ) Windows */ +#endif + /* _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL); */ + _enter_critical_bh(&pxmitpriv->lock, &irqL); + + xmitframe_phead = get_list_head(&psta_bmc->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + rtw_list_delete(&pxmitframe->list); + + psta_bmc->sleepq_len--; + if (psta_bmc->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + pxmitframe->attrib.triggered = 1; + + if (xmitframe_hiq_filter(pxmitframe) == _TRUE) + pxmitframe->attrib.qsel = QSLT_HIGH;/* HIQ */ + +#if 0 + _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL); + if (rtw_hal_xmit(padapter, pxmitframe) == _TRUE) + rtw_os_xmit_complete(padapter, pxmitframe); + _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL); +#endif + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + } + + /* _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL); */ + _exit_critical_bh(&pxmitpriv->lock, &irqL); + + if (rtw_get_intf_type(padapter) != RTW_PCIE) { + /* check hi queue and bmc_sleepq */ + rtw_chk_hi_queue_cmd(padapter); + } + } +#endif + + return H2C_SUCCESS; +} + +u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf) +{ +#ifdef CONFIG_AP_MODE + /*RTW_INFO(FUNC_ADPT_FMT, FUNC_ADPT_ARG(padapter));*/ +#ifdef CONFIG_SWTIMER_BASED_TXBCN + + tx_beacon_handlder(padapter->dvobj); + +#else + + if (send_beacon(padapter) == _FAIL) { + RTW_INFO("issue_beacon, fail!\n"); + return H2C_PARAMETERS_ERROR; + } + + /* tx bc/mc frames after update TIM */ + chk_bmc_sleepq_hdl(padapter, NULL); +#endif +#endif /* CONFIG_AP_MODE */ + return H2C_SUCCESS; +} + +/* +* according to channel +* add/remove WLAN_BSSID_EX.IEs's ERP ie +* set WLAN_BSSID_EX.SupportedRates +* update WLAN_BSSID_EX.IEs's Supported Rate and Extended Supported Rate ie +*/ +#ifdef CONFIG_AP_MODE +void change_band_update_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 ch) +{ + u8 network_type, rate_len, total_rate_len, remainder_rate_len; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + u8 erpinfo = 0x4; + + if (ch >= 36) { + network_type = WIRELESS_11A; + total_rate_len = IEEE80211_NUM_OFDM_RATESLEN; + rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_); + #ifdef CONFIG_80211AC_VHT + /* if channel in 5G band, then add vht ie . */ + if ((pmlmepriv->htpriv.ht_option == _TRUE) + && REGSTY_IS_11AC_ENABLE(&padapter->registrypriv) + && is_supported_vht(padapter->registrypriv.wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + if (REGSTY_IS_11AC_AUTO(&padapter->registrypriv) + || pmlmepriv->ori_vht_en) + rtw_vht_ies_attach(padapter, pnetwork); + } + #endif + } else { + network_type = 0; + total_rate_len = 0; + if (padapter->registrypriv.wireless_mode & WIRELESS_11B) { + network_type |= WIRELESS_11B; + total_rate_len += IEEE80211_CCK_RATE_LEN; + } + if (padapter->registrypriv.wireless_mode & WIRELESS_11G) { + network_type |= WIRELESS_11G; + total_rate_len += IEEE80211_NUM_OFDM_RATESLEN; + } + rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1); + #ifdef CONFIG_80211AC_VHT + rtw_vht_ies_detach(padapter, pnetwork); + #endif + } + + rtw_set_supported_rate(pnetwork->SupportedRates, network_type); + + UpdateBrateTbl(padapter, pnetwork->SupportedRates); + + if (total_rate_len > 8) { + rate_len = 8; + remainder_rate_len = total_rate_len - 8; + } else { + rate_len = total_rate_len; + remainder_rate_len = 0; + } + + rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len); + + if (remainder_rate_len) + rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates + 8), remainder_rate_len); + else + rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_); + + pnetwork->Length = get_WLAN_BSSID_EX_sz(pnetwork); +} +#endif /* CONFIG_AP_MODE */ + +void rtw_join_done_chk_ch(_adapter *adapter, int join_res) +{ +#define DUMP_ADAPTERS_STATUS 0 + + struct dvobj_priv *dvobj; + _adapter *iface; + struct mlme_priv *mlme; + struct mlme_ext_priv *mlmeext; + u8 u_ch, u_offset, u_bw; + int i, ret; + + dvobj = adapter_to_dvobj(adapter); + + if (DUMP_ADAPTERS_STATUS) { + RTW_INFO(FUNC_ADPT_FMT" enter\n", FUNC_ADPT_ARG(adapter)); + dump_adapters_status(RTW_DBGDUMP , dvobj); + } + + ret = rtw_mi_get_ch_setting_union(adapter, &u_ch, &u_bw, &u_offset); + if (join_res >= 0 && ret <= 0) { + join_res = -1; + dump_adapters_status(RTW_DBGDUMP , dvobj); + rtw_warn_on(1); + } + + if (join_res >= 0) { +#ifdef CONFIG_MCC_MODE + /* MCC setting success, don't go to ch union process */ + if (rtw_hal_set_mcc_setting_join_done_chk_ch(adapter)) + return; +#endif /* CONFIG_MCC_MODE */ + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + mlme = &iface->mlmepriv; + mlmeext = &iface->mlmeextpriv; + + if (!iface || iface == adapter) + continue; + +#ifdef CONFIG_AP_MODE + if ((MLME_IS_AP(iface) || MLME_IS_MESH(iface)) + && check_fwstate(mlme, WIFI_ASOC_STATE) + ) { + u8 ori_ch, ori_bw, ori_offset; + bool is_grouped = rtw_is_chbw_grouped(u_ch, u_bw, u_offset + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset); + + if (is_grouped == _FALSE) { + /* handle AP which need to switch ch setting */ + + ori_ch = mlmeext->cur_channel; + ori_bw = mlmeext->cur_bwmode; + ori_offset = mlmeext->cur_ch_offset; + + /* restore original bw, adjust bw by registry setting on target ch */ + mlmeext->cur_bwmode = mlme->ori_bw; + mlmeext->cur_channel = u_ch; + rtw_adjust_chbw(iface, mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(iface)) + rtw_mesh_adjust_chbw(mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset); + #endif + + rtw_chset_sync_chbw(adapter_to_chset(adapter) + , &mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset + , &u_ch, &u_bw, &u_offset, 1, 0); + + RTW_INFO(FUNC_ADPT_FMT" %u,%u,%u => %u,%u,%u\n", FUNC_ADPT_ARG(iface) + , ori_ch, ori_bw, ori_offset + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset); + + rtw_ap_update_bss_chbw(iface, &(mlmeext->mlmext_info.network) + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset); + + _rtw_memcpy(&(mlme->cur_network.network), &(mlmeext->mlmext_info.network), sizeof(WLAN_BSSID_EX)); + + rtw_start_bss_hdl_after_chbw_decided(iface); + + { + #if defined(CONFIG_IOCTL_CFG80211) && (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 5, 0)) + u8 ht_option = 0; + + #ifdef CONFIG_80211N_HT + ht_option = mlme->htpriv.ht_option; + #endif + + rtw_cfg80211_ch_switch_notify(iface + , mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset + , ht_option, 0); + #endif + } + } + + clr_fwstate(mlme, WIFI_OP_CH_SWITCHING); + update_beacon(iface, 0xFF, NULL, _TRUE, 0); + } +#endif /* CONFIG_AP_MODE */ + } + +#ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(adapter, MLME_STA_CONNECTED, 0); +#endif + } else { + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + mlme = &iface->mlmepriv; + mlmeext = &iface->mlmeextpriv; + + if (!iface || iface == adapter) + continue; +#ifdef CONFIG_AP_MODE + if ((MLME_IS_AP(iface) || MLME_IS_MESH(iface)) + && check_fwstate(mlme, WIFI_ASOC_STATE) + ) { + clr_fwstate(mlme, WIFI_OP_CH_SWITCHING); + update_beacon(iface, 0xFF, NULL, _TRUE, 0); + } +#endif + } +#ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(adapter, MLME_STA_DISCONNECTED, 0); +#endif + } + + if (rtw_mi_get_ch_setting_union(adapter, &u_ch, &u_bw, &u_offset)) { + RTW_INFO(FUNC_ADPT_FMT" union:%u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset); + set_channel_bwmode(adapter, u_ch, u_offset, u_bw); + } + + rtw_mi_update_union_chan_inf(adapter, u_ch, u_offset, u_bw); + + if (join_res >= 0) + rtw_rfctl_update_op_mode(adapter_to_rfctl(adapter), BIT(adapter->iface_id), 1); + + if (DUMP_ADAPTERS_STATUS) { + RTW_INFO(FUNC_ADPT_FMT" exit\n", FUNC_ADPT_ARG(adapter)); + dump_adapters_status(RTW_DBGDUMP , dvobj); + } +} + +int rtw_chk_start_clnt_join(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset) +{ +#ifdef CONFIG_CONCURRENT_MODE + bool chbw_allow = _TRUE; +#endif + bool connect_allow = _TRUE; + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + u8 cur_ch, cur_bw, cur_ch_offset; + u8 u_ch, u_offset, u_bw; + + u_ch = cur_ch = pmlmeext->cur_channel; + u_bw = cur_bw = pmlmeext->cur_bwmode; + u_offset = cur_ch_offset = pmlmeext->cur_ch_offset; + + if (!ch || !bw || !offset) { + connect_allow = _FALSE; + rtw_warn_on(1); + goto exit; + } + + if (cur_ch == 0) { + connect_allow = _FALSE; + RTW_ERR(FUNC_ADPT_FMT" cur_ch:%u\n" + , FUNC_ADPT_ARG(adapter), cur_ch); + rtw_warn_on(1); + goto exit; + } + RTW_INFO(FUNC_ADPT_FMT" req: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset); + +#ifdef CONFIG_CONCURRENT_MODE + { + struct dvobj_priv *dvobj; + _adapter *iface; + struct mlme_priv *mlme; + struct mlme_ext_priv *mlmeext; + struct mi_state mstate; + int i; + + dvobj = adapter_to_dvobj(adapter); + + rtw_mi_status_no_self(adapter, &mstate); + RTW_INFO(FUNC_ADPT_FMT" others ld_sta_num:%u, ap_num:%u, mesh_num:%u\n" + , FUNC_ADPT_ARG(adapter), MSTATE_STA_LD_NUM(&mstate) + , MSTATE_AP_NUM(&mstate), MSTATE_MESH_NUM(&mstate)); + + if (!MSTATE_STA_LD_NUM(&mstate) && !MSTATE_AP_NUM(&mstate) && !MSTATE_MESH_NUM(&mstate)) { + /* consider linking STA? */ + goto connect_allow_hdl; + } + + if (rtw_mi_get_ch_setting_union_no_self(adapter, &u_ch, &u_bw, &u_offset) <= 0) { + dump_adapters_status(RTW_DBGDUMP , dvobj); + rtw_warn_on(1); + } + RTW_INFO(FUNC_ADPT_FMT" others union:%u,%u,%u\n" + , FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset); + + /* chbw_allow? */ + chbw_allow = rtw_is_chbw_grouped(pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset + , u_ch, u_bw, u_offset); + + RTW_INFO(FUNC_ADPT_FMT" chbw_allow:%d\n" + , FUNC_ADPT_ARG(adapter), chbw_allow); + +#ifdef CONFIG_MCC_MODE + /* check setting success, don't go to ch union process */ + if (rtw_hal_set_mcc_setting_chk_start_clnt_join(adapter, &u_ch, &u_bw, &u_offset, chbw_allow)) + goto exit; +#endif + + if (chbw_allow == _TRUE) { + rtw_sync_chbw(&cur_ch, &cur_bw, &cur_ch_offset, &u_ch, &u_bw, &u_offset); + rtw_warn_on(cur_ch != pmlmeext->cur_channel); + rtw_warn_on(cur_bw != pmlmeext->cur_bwmode); + rtw_warn_on(cur_ch_offset != pmlmeext->cur_ch_offset); + goto connect_allow_hdl; + } + +#ifdef CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT + /* chbw_allow is _FALSE, connect allow? */ + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + mlme = &iface->mlmepriv; + mlmeext = &iface->mlmeextpriv; + + if (check_fwstate(mlme, WIFI_STATION_STATE) + && check_fwstate(mlme, WIFI_ASOC_STATE) +#if defined(CONFIG_P2P) + && rtw_p2p_chk_state(&(iface->wdinfo), P2P_STATE_NONE) +#endif + ) { + connect_allow = _FALSE; + break; + } + } +#endif /* CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT */ + + if (MSTATE_STA_LD_NUM(&mstate) + MSTATE_AP_LD_NUM(&mstate) + MSTATE_MESH_LD_NUM(&mstate) >= 4) + connect_allow = _FALSE; + + RTW_INFO(FUNC_ADPT_FMT" connect_allow:%d\n" + , FUNC_ADPT_ARG(adapter), connect_allow); + + if (connect_allow == _FALSE) + goto exit; + +connect_allow_hdl: + /* connect_allow == _TRUE */ + + if (chbw_allow == _FALSE) { + u_ch = cur_ch; + u_bw = cur_bw; + u_offset = cur_ch_offset; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + mlme = &iface->mlmepriv; + mlmeext = &iface->mlmeextpriv; + + if (!iface || iface == adapter) + continue; + + #ifdef CONFIG_AP_MODE + if ((MLME_IS_AP(iface) || MLME_IS_MESH(iface)) + && check_fwstate(mlme, WIFI_ASOC_STATE) + ) { + #ifdef CONFIG_SPCT_CH_SWITCH + if (1) + rtw_ap_inform_ch_switch(iface, pmlmeext->cur_channel , pmlmeext->cur_ch_offset); + else + #endif + rtw_sta_flush(iface, _FALSE); + + rtw_hal_set_hwreg(iface, HW_VAR_CHECK_TXBUF, 0); + set_fwstate(mlme, WIFI_OP_CH_SWITCHING); + } else + #endif /* CONFIG_AP_MODE */ + if (check_fwstate(mlme, WIFI_STATION_STATE) + && check_fwstate(mlme, WIFI_ASOC_STATE) + ) { + rtw_disassoc_cmd(iface, 500, RTW_CMDF_DIRECTLY); + rtw_indicate_disconnect(iface, 0, _FALSE); + rtw_free_assoc_resources(iface, _TRUE); + } + } + } + + #ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(adapter, MLME_STA_CONNECTING, 0); + #endif + } +#endif /* CONFIG_CONCURRENT_MODE */ + +exit: + + if (connect_allow == _TRUE) { + RTW_INFO(FUNC_ADPT_FMT" union: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset); + rtw_mi_update_union_chan_inf(adapter, u_ch, u_offset, u_bw); + *ch = u_ch; + *bw = u_bw; + *offset = u_offset; + +#if defined(CONFIG_IOCTL_CFG80211) && (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 5, 0)) + { + u8 ht_option = 0; + +#ifdef CONFIG_80211N_HT + ht_option = adapter->mlmepriv.htpriv.ht_option; +#endif /* CONFIG_80211N_HT */ + + /* + when supplicant send the mlme frame, + the bss freq is updated by channel switch event. + */ + rtw_cfg80211_ch_switch_notify(adapter, + cur_ch, cur_bw, cur_ch_offset, ht_option, 1); + } +#endif + } + + return connect_allow == _TRUE ? _SUCCESS : _FAIL; +} + +void rtw_set_external_auth_status(_adapter *padapter, + const void *data, int len) +{ +#ifdef CONFIG_IOCTL_CFG80211 + struct net_device *dev = padapter->pnetdev; + struct wiphy *wiphy = adapter_to_wiphy(padapter); + struct rtw_external_auth_params params; + + /* convert data to external_auth_params */ + params.action = RTW_GET_BE32((u8 *)data); + _rtw_memcpy(¶ms.bssid, (u8 *)data + 4, ETH_ALEN); + _rtw_memcpy(¶ms.ssid.ssid, (u8 *)data + 10, WLAN_SSID_MAXLEN); + params.ssid.ssid_len = RTW_GET_BE64((u8 *)data + 42); + params.key_mgmt_suite = RTW_GET_BE32((u8 *)data + 58); + params.status = RTW_GET_BE16((u8 *)data + 62); + _rtw_memcpy(¶ms.pmkid, (u8 *)data + 64, PMKID_LEN); + + rtw_cfg80211_external_auth_status(wiphy, dev, ¶ms); +#endif /* CONFIG_IOCTL_CFG80211 */ +} + +u8 rtw_iqk_hdl(_adapter *padapter, unsigned char *pbuf) +{ + rtw_hal_phydm_cal_trigger(padapter); + return H2C_SUCCESS; +} + +u8 rtw_set_chbw_hdl(_adapter *padapter, u8 *pbuf) +{ + struct set_ch_parm *set_ch_parm; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + u8 ifbmp_s = rtw_mi_get_ld_sta_ifbmp(padapter); + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + u8 u_ch, u_bw, u_offset; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + set_ch_parm = (struct set_ch_parm *)pbuf; + + RTW_INFO(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n", + FUNC_NDEV_ARG(padapter->pnetdev), + set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset); + + /* update ch, bw, offset for all asoc STA ifaces */ + if (ifbmp_s) { + _adapter *iface; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface || !(ifbmp_s & BIT(iface->iface_id))) + continue; + + /* update STA mode ch/bw/offset */ + iface->mlmeextpriv.cur_channel = set_ch_parm->ch; + iface->mlmeextpriv.cur_bwmode = set_ch_parm->bw; + iface->mlmeextpriv.cur_ch_offset = set_ch_parm->ch_offset; + /* updaet STA mode DSConfig , ap mode will update in rtw_change_bss_chbw_cmd */ + iface->mlmepriv.cur_network.network.Configuration.DSConfig = set_ch_parm->ch; + } + } + + LeaveAllPowerSaveModeDirect(padapter); + +#ifdef CONFIG_MONITOR_MODE_XMIT + pmlmeext->cur_channel = set_ch_parm->ch; + pmlmeext->cur_ch_offset = set_ch_parm->ch_offset; + pmlmeext->cur_bwmode = set_ch_parm->bw; +#endif /* CONFIG_MONITOR_MODE_XMIT */ + + set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw); + + rtw_mi_get_ch_setting_union(padapter, &u_ch, &u_bw, &u_offset); + rtw_mi_update_union_chan_inf(padapter, u_ch, u_offset, u_bw); + rtw_rfctl_update_op_mode(dvobj_to_rfctl(dvobj), 0, 0); + + return H2C_SUCCESS; +} + +u8 rtw_set_chplan_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct SetChannelPlan_param *param; + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + param = (struct SetChannelPlan_param *)pbuf; + + if (param->regd_src == REGD_SRC_RTK_PRIV + && !rtw_is_channel_plan_valid(param->channel_plan)) + return H2C_PARAMETERS_ERROR; + +#ifdef CONFIG_REGD_SRC_FROM_OS + if (rfctl->regd_src == REGD_SRC_OS) + rtw_mfree((void *)rfctl->country_ent, sizeof(struct country_chplan)); +#endif + + rfctl->regd_src = param->regd_src; + rfctl->country_ent = param->country_ent; + rfctl->ChannelPlan = param->channel_plan; + +#if CONFIG_TXPWR_LIMIT + rtw_txpwr_init_regd(rfctl); +#endif + + rtw_rfctl_chplan_init(padapter); + + rtw_hal_set_odm_var(padapter, HAL_ODM_REGULATION, NULL, _TRUE); + +#ifdef CONFIG_IOCTL_CFG80211 + rtw_regd_apply_flags(adapter_to_wiphy(padapter)); +#endif + + rtw_nlrtw_reg_change_event(padapter); + + if (GET_HAL_DATA(padapter)->txpwr_limit_loaded + && rtw_get_hw_init_completed(padapter)) + rtw_hal_update_txpwr_level(padapter); + + return H2C_SUCCESS; +} + +u8 rtw_get_chplan_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct get_channel_plan_param *param; + struct get_chplan_resp *resp; + struct rf_ctl_t *rfctl; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + rfctl = adapter_to_rfctl(padapter); + param = (struct get_channel_plan_param *)pbuf; + + resp = rtw_vmalloc(sizeof(struct get_chplan_resp) + sizeof(RT_CHANNEL_INFO) * rfctl->max_chan_nums); + if (!resp) + return H2C_CMD_FAIL; + + resp->regd_src = rfctl->regd_src; + + if (rfctl->country_ent) { + _rtw_memcpy(&resp->country_ent, rfctl->country_ent, sizeof(struct country_chplan)); + resp->has_country = 1; + } else + resp->has_country = 0; + + resp->channel_plan = rfctl->ChannelPlan; +#if CONFIG_TXPWR_LIMIT + resp->regd_name = rfctl->regd_name; +#endif +#ifdef CONFIG_DFS_MASTER + resp->dfs_domain = rtw_rfctl_get_dfs_domain(rfctl); +#endif + resp->chset_num = rfctl->max_chan_nums; + + _rtw_memcpy(resp->chset, rfctl->channel_set, sizeof(RT_CHANNEL_INFO) * rfctl->max_chan_nums); + *param->resp = resp; + + return H2C_SUCCESS; +} + +u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf) +{ + struct LedBlink_param *ledBlink_param; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + ledBlink_param = (struct LedBlink_param *)pbuf; + +#ifdef CONFIG_RTW_LED_HANDLED_BY_CMD_THREAD + BlinkHandler((PLED_DATA)ledBlink_param->pLed); +#endif + + return H2C_SUCCESS; +} + +void csa_timer_hdl(void *FunctionContext) +{ + _adapter *padapter = (_adapter *)FunctionContext; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 i, update_beacon = _FALSE; + + for (i = 0; i < dvobj->iface_nums; i++) { + _adapter *iface; + iface = dvobj->padapters[i]; + if (!iface) + continue; + if (check_fwstate(pmlmepriv, WIFI_CSA_UPDATE_BEACON)) { + clr_fwstate(pmlmepriv, WIFI_CSA_UPDATE_BEACON); + update_beacon = _TRUE; + } + } + + /* wait beacons more than 70 seconds */ + if(update_beacon == _TRUE) { + RTW_INFO("wait beacons more than 70 seconds\n"); + return ; + } + + if(rfctl->csa_ch == 0) { + RTW_INFO("channel switch done\n"); + return ; + } + + /* channel switch */ + if (rtw_set_csa_cmd(padapter) != _SUCCESS) { + rfctl->csa_ch = 0; + rfctl->csa_switch_cnt = 0; + rfctl->csa_ch_offset = 0; + rfctl->csa_ch_width = 0; + rfctl->csa_ch_freq_seg0 = 0; + rfctl->csa_ch_freq_seg1 = 0; + } +} + +u8 set_csa_hdl(_adapter *adapter, unsigned char *pbuf) +{ +#if CONFIG_DFS + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + + if (rfctl->csa_ch) + rtw_dfs_ch_switch_hdl(adapter_to_dvobj(adapter)); +#endif + return H2C_SUCCESS; +} + +u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf) +{ +#ifdef CONFIG_TDLS + _irqL irqL; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; +#ifdef CONFIG_TDLS_CH_SW + struct tdls_ch_switch *pchsw_info = &ptdlsinfo->chsw_info; +#endif + struct TDLSoption_param *TDLSoption; + struct sta_info *ptdls_sta = NULL; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + struct sta_info *ap_sta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(&(pmlmeinfo->network))); + u8 survey_channel, i, min, option; + struct tdls_txmgmt txmgmt; + u32 setchtime, resp_sleep = 0, wait_time; + u8 zaddr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + u8 ret; + u8 doiqk; + u64 tx_ra_bitmap = 0; + + if (!pbuf) + return H2C_PARAMETERS_ERROR; + + TDLSoption = (struct TDLSoption_param *)pbuf; + option = TDLSoption->option; + + if (!_rtw_memcmp(TDLSoption->addr, zaddr, ETH_ALEN)) { + ptdls_sta = rtw_get_stainfo(&(padapter->stapriv), TDLSoption->addr); + if (ptdls_sta == NULL) + return H2C_REJECTED; + } else { + if (!(option == TDLS_RS_RCR)) + return H2C_REJECTED; + } + + /* _enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL); */ + /* RTW_INFO("[%s] option:%d\n", __FUNCTION__, option); */ + + switch (option) { + case TDLS_ESTABLISHED: { + /* As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0 */ + /* So we can receive all kinds of data frames. */ + u8 sta_band = 0; + + /* leave ALL PS when TDLS is established */ + rtw_pwr_wakeup(padapter); + + rtw_hal_rcr_set_chk_bssid(padapter, MLME_TDLS_LINKED); + RTW_INFO("Created Direct Link with "MAC_FMT"\n", MAC_ARG(ptdls_sta->cmn.mac_addr)); + + /* Set TDLS sta rate. */ + /* Update station supportRate */ + rtw_hal_update_sta_ra_info(padapter, ptdls_sta); + tx_ra_bitmap = ptdls_sta->cmn.ra_info.ramask; + + if (pmlmeext->cur_channel > 14) { + if (tx_ra_bitmap & 0xffff000) + sta_band |= WIRELESS_11_5N ; + + if (tx_ra_bitmap & 0xff0) + sta_band |= WIRELESS_11A; + + /* 5G band */ +#ifdef CONFIG_80211AC_VHT + if (ptdls_sta->vhtpriv.vht_option) + sta_band = WIRELESS_11_5AC; +#endif + + } else { + if (tx_ra_bitmap & 0xffff000) + sta_band |= WIRELESS_11_24N; + + if (tx_ra_bitmap & 0xff0) + sta_band |= WIRELESS_11G; + + if (tx_ra_bitmap & 0x0f) + sta_band |= WIRELESS_11B; + } + ptdls_sta->wireless_mode = sta_band; + rtw_hal_update_sta_wset(padapter, ptdls_sta); + /* Sta mode */ + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, ptdls_sta, _TRUE); + + set_sta_rate(padapter, ptdls_sta); + rtw_sta_media_status_rpt(padapter, ptdls_sta, 1); + break; + } + case TDLS_ISSUE_PTI: + ptdls_sta->tdls_sta_state |= TDLS_WAIT_PTR_STATE; + issue_tdls_peer_traffic_indication(padapter, ptdls_sta); + _set_timer(&ptdls_sta->pti_timer, TDLS_PTI_TIME); + break; +#ifdef CONFIG_TDLS_CH_SW + case TDLS_CH_SW_RESP: + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.status_code = 0; + _rtw_memcpy(txmgmt.peer, ptdls_sta->cmn.mac_addr, ETH_ALEN); + + if (ap_sta) + rtw_hal_macid_sleep(padapter, ap_sta->cmn.mac_id); + issue_nulldata(padapter, NULL, 1, 3, 3); + + RTW_INFO("[TDLS ] issue tdls channel switch response\n"); + ret = issue_tdls_ch_switch_rsp(padapter, &txmgmt, _TRUE); + + /* If we receive TDLS_CH_SW_REQ at off channel which it's target is AP's channel */ + /* then we just switch to AP's channel*/ + if (padapter->mlmeextpriv.cur_channel == pchsw_info->off_ch_num) { + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_END_TO_BASE_CHNL); + break; + } + + if (ret == _SUCCESS) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_TO_OFF_CHNL); + else + RTW_INFO("[TDLS] issue_tdls_ch_switch_rsp wait ack fail !!!!!!!!!!\n"); + + break; + case TDLS_CH_SW_PREPARE: + pchsw_info->ch_sw_state |= TDLS_CH_SWITCH_PREPARE_STATE; + + /* to collect IQK info of off-chnl */ + doiqk = _TRUE; + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, &doiqk); + set_channel_bwmode(padapter, pchsw_info->off_ch_num, pchsw_info->ch_offset, (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20); + doiqk = _FALSE; + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, &doiqk); + + /* switch back to base-chnl */ + doiqk = _TRUE; + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, &doiqk); + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + doiqk = _FALSE; + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, &doiqk); + + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_START); + + pchsw_info->ch_sw_state &= ~(TDLS_CH_SWITCH_PREPARE_STATE); + + break; + case TDLS_CH_SW_START: + rtw_tdls_set_ch_sw_oper_control(padapter, _TRUE); + break; + case TDLS_CH_SW_TO_OFF_CHNL: + if (ap_sta) + rtw_hal_macid_sleep(padapter, ap_sta->cmn.mac_id); + issue_nulldata(padapter, NULL, 1, 3, 3); + + if (padapter->registrypriv.wifi_spec == 0) { + if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE)) + _set_timer(&ptdls_sta->ch_sw_timer, (u32)(ptdls_sta->ch_switch_timeout) / 1000); + } + + if (rtw_tdls_do_ch_sw(padapter, ptdls_sta, TDLS_CH_SW_OFF_CHNL, pchsw_info->off_ch_num, + pchsw_info->ch_offset, (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20, ptdls_sta->ch_switch_time) == _SUCCESS) { + pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE); + if (pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE) { + if (issue_nulldata_to_TDLS_peer_STA(ptdls_sta->padapter, ptdls_sta->cmn.mac_addr, 0, 1, + (padapter->registrypriv.wifi_spec == 0) ? 3 : 0) == _FAIL) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_TO_BASE_CHNL); + } + } else { + if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE)) + _cancel_timer_ex(&ptdls_sta->ch_sw_timer); + } + + + break; + case TDLS_CH_SW_END: + case TDLS_CH_SW_END_TO_BASE_CHNL: + rtw_tdls_set_ch_sw_oper_control(padapter, _FALSE); + _cancel_timer_ex(&ptdls_sta->ch_sw_timer); + _cancel_timer_ex(&ptdls_sta->stay_on_base_chnl_timer); + _cancel_timer_ex(&ptdls_sta->ch_sw_monitor_timer); +#if 0 + _rtw_memset(pHalData->tdls_ch_sw_iqk_info_base_chnl, 0x00, sizeof(pHalData->tdls_ch_sw_iqk_info_base_chnl)); + _rtw_memset(pHalData->tdls_ch_sw_iqk_info_off_chnl, 0x00, sizeof(pHalData->tdls_ch_sw_iqk_info_off_chnl)); +#endif + + if (option == TDLS_CH_SW_END_TO_BASE_CHNL) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_TO_BASE_CHNL); + + break; + case TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED: + case TDLS_CH_SW_TO_BASE_CHNL: + pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE | TDLS_WAIT_CH_RSP_STATE); + + if (option == TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED) { + if (ptdls_sta != NULL) { + /* Send unsolicited channel switch rsp. to peer */ + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.status_code = 0; + _rtw_memcpy(txmgmt.peer, ptdls_sta->cmn.mac_addr, ETH_ALEN); + issue_tdls_ch_switch_rsp(padapter, &txmgmt, _FALSE); + } + } + + if (rtw_tdls_do_ch_sw(padapter, ptdls_sta, TDLS_CH_SW_BASE_CHNL, pmlmeext->cur_channel, + pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode, ptdls_sta->ch_switch_time) == _SUCCESS) { + if (ap_sta) + rtw_hal_macid_wakeup(padapter, ap_sta->cmn.mac_id); + issue_nulldata(padapter, NULL, 0, 3, 3); + /* set ch sw monitor timer for responder */ + if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE)) + _set_timer(&ptdls_sta->ch_sw_monitor_timer, TDLS_CH_SW_MONITOR_TIMEOUT); + } + + break; +#endif + case TDLS_RS_RCR: + rtw_hal_rcr_set_chk_bssid(padapter, MLME_TDLS_NOLINK); + break; + case TDLS_TEARDOWN_STA: + case TDLS_TEARDOWN_STA_NO_WAIT: + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.status_code = _RSON_TDLS_TEAR_UN_RSN_; + _rtw_memcpy(txmgmt.peer, ptdls_sta->cmn.mac_addr, ETH_ALEN); + + issue_tdls_teardown(padapter, &txmgmt, (option == TDLS_TEARDOWN_STA) ? _TRUE : _FALSE); + + break; + case TDLS_TEARDOWN_STA_LOCALLY: + case TDLS_TEARDOWN_STA_LOCALLY_POST: +#ifdef CONFIG_TDLS_CH_SW + if (_rtw_memcmp(TDLSoption->addr, pchsw_info->addr, ETH_ALEN) == _TRUE) { + pchsw_info->ch_sw_state &= ~(TDLS_CH_SW_INITIATOR_STATE | + TDLS_CH_SWITCH_ON_STATE | + TDLS_PEER_AT_OFF_STATE); + rtw_tdls_set_ch_sw_oper_control(padapter, _FALSE); + _rtw_memset(pchsw_info->addr, 0x00, ETH_ALEN); + } +#endif + + if (option == TDLS_TEARDOWN_STA_LOCALLY) + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + + rtw_tdls_teardown_post_hdl(padapter, ptdls_sta, _FALSE); + + if (ptdlsinfo->tdls_sctx != NULL) + rtw_sctx_done(&(ptdlsinfo->tdls_sctx)); + + break; + } + + /* _exit_critical_bh(&(ptdlsinfo->hdl_lock), &irqL); */ + + return H2C_SUCCESS; +#else + return H2C_REJECTED; +#endif /* CONFIG_TDLS */ + +} + +u8 run_in_thread_hdl(_adapter *padapter, u8 *pbuf) +{ + struct RunInThread_param *p; + + + if (NULL == pbuf) + return H2C_PARAMETERS_ERROR; + p = (struct RunInThread_param *)pbuf; + + if (p->func) + p->func(p->context); + + return H2C_SUCCESS; +} + +int rtw_sae_preprocess(_adapter *adapter, const u8 *buf, u32 len, u8 tx) +{ +#ifdef CONFIG_IOCTL_CFG80211 + const u8 *frame_body = buf + sizeof(struct rtw_ieee80211_hdr_3addr); + u16 alg; + u16 seq; + u16 status; + int ret = _FAIL; + + alg = RTW_GET_LE16(frame_body); + if (alg != WLAN_AUTH_SAE) + goto exit; + + seq = RTW_GET_LE16(frame_body + 2); + status = RTW_GET_LE16(frame_body + 4); + + RTW_INFO("RTW_%s:AUTH alg:0x%04x, seq:0x%04x, status:0x%04x, mesg:%s\n", + (tx == _TRUE) ? "Tx" : "Rx", alg, seq, status, + (seq == 1) ? "Commit" : "Confirm"); + + ret = _SUCCESS; + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + rtw_mesh_sae_check_frames(adapter, buf, len, tx, alg, seq, status); + goto exit; + } +#endif + + if (tx && (seq == 2) && (status == 0)) { + /* quere commit frame until external auth statue update */ + struct sta_priv *pstapriv = &adapter->stapriv; + struct sta_info *psta = NULL; + _irqL irqL; + + psta = rtw_get_stainfo(pstapriv, GetAddr1Ptr(buf)); + if (psta) { + _enter_critical_bh(&psta->lock, &irqL); + if (psta->pauth_frame) { + rtw_mfree(psta->pauth_frame, psta->auth_len); + psta->pauth_frame = NULL; + psta->auth_len = 0; + } + + psta->pauth_frame = rtw_zmalloc(len); + if (psta->pauth_frame) { + _rtw_memcpy(psta->pauth_frame, buf, len); + psta->auth_len = len; + } + _exit_critical_bh(&psta->lock, &irqL); + + ret = 2; + } + } +exit: + return ret; +#else + return _SUCCESS; +#endif /* CONFIG_IOCTL_CFG80211 */ +} + +/* Needs to confirm with FW the value of REG_RX_BEACON_LENGTH */ +u8 rtw_write_bcnlen_hdl(_adapter *padapter, u8 *pbuf) +{ +#ifdef CONFIG_WRITE_BCN_LEN_TO_FW + struct write_bcnlen_param *parm = (struct write_bcnlen_param *)pbuf; + u16 bcn_len = parm->bcn_len; + + rtw_write16(padapter, REG_RX_BEACON_LENGTH + 2, bcn_len); +#endif + return H2C_SUCCESS; +} diff --git a/core/rtw_mp.c b/core/rtw_mp.c new file mode 100644 index 0000000..ccc1fce --- /dev/null +++ b/core/rtw_mp.c @@ -0,0 +1,3992 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_MP_C_ +#include +#ifdef PLATFORM_FREEBSD + #include /* for RFHIGHPID */ +#endif + +#include "../hal/phydm/phydm_precomp.h" +#if defined(CONFIG_RTL8723B) || defined(CONFIG_RTL8821A) + #include +#endif + +#ifdef CONFIG_MP_VHT_HW_TX_MODE +#define CEILING_POS(X) ((X - (int)(X)) > 0 ? (int)(X + 1) : (int)(X)) +#define CEILING_NEG(X) ((X - (int)(X)) < 0 ? (int)(X - 1) : (int)(X)) +#define ceil(X) (((X) > 0) ? CEILING_POS(X) : CEILING_NEG(X)) + +int rtfloor(float x) +{ + int i = x - 2; + while + (++i <= x - 1) + ; + return i; +} +#endif + +#ifdef CONFIG_MP_INCLUDED +u32 read_macreg(_adapter *padapter, u32 addr, u32 sz) +{ + u32 val = 0; + + switch (sz) { + case 1: + val = rtw_read8(padapter, addr); + break; + case 2: + val = rtw_read16(padapter, addr); + break; + case 4: + val = rtw_read32(padapter, addr); + break; + default: + val = 0xffffffff; + break; + } + + return val; + +} + +void write_macreg(_adapter *padapter, u32 addr, u32 val, u32 sz) +{ + switch (sz) { + case 1: + rtw_write8(padapter, addr, (u8)val); + break; + case 2: + rtw_write16(padapter, addr, (u16)val); + break; + case 4: + rtw_write32(padapter, addr, val); + break; + default: + break; + } + +} + +u32 read_bbreg(_adapter *padapter, u32 addr, u32 bitmask) +{ + return rtw_hal_read_bbreg(padapter, addr, bitmask); +} + +void write_bbreg(_adapter *padapter, u32 addr, u32 bitmask, u32 val) +{ + rtw_hal_write_bbreg(padapter, addr, bitmask, val); +} + +u32 _read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask) +{ + return rtw_hal_read_rfreg(padapter, rfpath, addr, bitmask); +} + +void _write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val) +{ + rtw_hal_write_rfreg(padapter, rfpath, addr, bitmask, val); +} + +u32 read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr) +{ + return _read_rfreg(padapter, rfpath, addr, bRFRegOffsetMask); +} + +void write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 val) +{ + _write_rfreg(padapter, rfpath, addr, bRFRegOffsetMask, val); +} + +static void _init_mp_priv_(struct mp_priv *pmp_priv) +{ + WLAN_BSSID_EX *pnetwork; + + _rtw_memset(pmp_priv, 0, sizeof(struct mp_priv)); + + pmp_priv->mode = MP_OFF; + + pmp_priv->channel = 1; + pmp_priv->bandwidth = CHANNEL_WIDTH_20; + pmp_priv->prime_channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + pmp_priv->rateidx = RATE_1M; + pmp_priv->txpoweridx = 0x2A; + + pmp_priv->antenna_tx = ANTENNA_A; + pmp_priv->antenna_rx = ANTENNA_AB; + + pmp_priv->check_mp_pkt = 0; + + pmp_priv->tx_pktcount = 0; + + pmp_priv->rx_bssidpktcount = 0; + pmp_priv->rx_pktcount = 0; + pmp_priv->rx_crcerrpktcount = 0; + + pmp_priv->network_macaddr[0] = 0x00; + pmp_priv->network_macaddr[1] = 0xE0; + pmp_priv->network_macaddr[2] = 0x4C; + pmp_priv->network_macaddr[3] = 0x87; + pmp_priv->network_macaddr[4] = 0x66; + pmp_priv->network_macaddr[5] = 0x55; + + pmp_priv->bSetRxBssid = _FALSE; + pmp_priv->bRTWSmbCfg = _FALSE; + pmp_priv->bloopback = _FALSE; + + pmp_priv->bloadefusemap = _FALSE; + pmp_priv->brx_filter_beacon = _FALSE; + pmp_priv->mplink_brx = _FALSE; + + pnetwork = &pmp_priv->mp_network.network; + _rtw_memcpy(pnetwork->MacAddress, pmp_priv->network_macaddr, ETH_ALEN); + + pnetwork->Ssid.SsidLength = 8; + _rtw_memcpy(pnetwork->Ssid.Ssid, "mp_871x", pnetwork->Ssid.SsidLength); + + pmp_priv->tx.payload = MP_TX_Payload_default_random; +#ifdef CONFIG_80211N_HT + pmp_priv->tx.attrib.ht_en = 1; +#endif + + pmp_priv->mpt_ctx.mpt_rate_index = 1; + +} + + +static void mp_init_xmit_attrib(struct mp_tx *pmptx, PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + + struct pkt_attrib *pattrib; + + /* init xmitframe attribute */ + pattrib = &pmptx->attrib; + _rtw_memset(pattrib, 0, sizeof(struct pkt_attrib)); + _rtw_memset(pmptx->desc, 0, TXDESC_SIZE); + + pattrib->ether_type = 0x8712; +#if 0 + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); +#endif + _rtw_memset(pattrib->dst, 0xFF, ETH_ALEN); + + /* pattrib->dhcp_pkt = 0; + * pattrib->pktlen = 0; */ + pattrib->ack_policy = 0; + /* pattrib->pkt_hdrlen = ETH_HLEN; */ + pattrib->hdrlen = WLAN_HDR_A3_LEN; + pattrib->subtype = WIFI_DATA; + pattrib->priority = 0; + pattrib->qsel = pattrib->priority; + /* do_queue_select(padapter, pattrib); */ + pattrib->nr_frags = 1; + pattrib->encrypt = 0; + pattrib->bswenc = _FALSE; + pattrib->qos_en = _FALSE; + + pattrib->pktlen = 1500; + + if (pHalData->rf_type == RF_2T2R) + pattrib->raid = RATEID_IDX_BGN_40M_2SS; + else + pattrib->raid = RATEID_IDX_BGN_40M_1SS; + +#ifdef CONFIG_80211AC_VHT + if (pHalData->rf_type == RF_1T1R) + pattrib->raid = RATEID_IDX_VHT_1SS; + else if (pHalData->rf_type == RF_2T2R || pHalData->rf_type == RF_2T4R) + pattrib->raid = RATEID_IDX_VHT_2SS; + else if (pHalData->rf_type == RF_3T3R) + pattrib->raid = RATEID_IDX_VHT_3SS; + else + pattrib->raid = RATEID_IDX_BGN_40M_1SS; +#endif +} + +s32 init_mp_priv(PADAPTER padapter) +{ + struct mp_priv *pmppriv = &padapter->mppriv; + PHAL_DATA_TYPE pHalData; + + pHalData = GET_HAL_DATA(padapter); + + _init_mp_priv_(pmppriv); + pmppriv->papdater = padapter; + if (IS_HARDWARE_TYPE_8822C(padapter)) + pmppriv->mp_dm = 1;/* default enable dpk tracking */ + else + pmppriv->mp_dm = 0; + + pmppriv->tx.stop = 1; + pmppriv->bSetTxPower = 0; /*for manually set tx power*/ + pmppriv->bTxBufCkFail = _FALSE; + pmppriv->pktInterval = 0; + pmppriv->pktLength = 1000; + pmppriv->bprocess_mp_mode = _FALSE; + pmppriv->efuse_update_file= _FALSE; + pmppriv->efuse_update_on = _FALSE; + + mp_init_xmit_attrib(&pmppriv->tx, padapter); + + switch (GET_HAL_RFPATH(padapter)) { + case RF_1T1R: + pmppriv->antenna_tx = ANTENNA_A; + pmppriv->antenna_rx = ANTENNA_A; + break; + case RF_1T2R: + default: + pmppriv->antenna_tx = ANTENNA_A; + pmppriv->antenna_rx = ANTENNA_AB; + break; + case RF_2T2R: + pmppriv->antenna_tx = ANTENNA_AB; + pmppriv->antenna_rx = ANTENNA_AB; + break; + case RF_2T4R: + pmppriv->antenna_tx = ANTENNA_BC; + pmppriv->antenna_rx = ANTENNA_ABCD; + break; + } + + pHalData->AntennaRxPath = pmppriv->antenna_rx; + pHalData->antenna_tx_path = pmppriv->antenna_tx; + + return _SUCCESS; +} + +void free_mp_priv(struct mp_priv *pmp_priv) +{ + if (pmp_priv->pallocated_mp_xmitframe_buf) { + rtw_mfree(pmp_priv->pallocated_mp_xmitframe_buf, 0); + pmp_priv->pallocated_mp_xmitframe_buf = NULL; + } + pmp_priv->pmp_xmtframe_buf = NULL; +} + +#if 0 +static void PHY_IQCalibrate_default( + PADAPTER pAdapter, + BOOLEAN bReCovery +) +{ + RTW_INFO("%s\n", __func__); +} + +static void PHY_LCCalibrate_default( + PADAPTER pAdapter +) +{ + RTW_INFO("%s\n", __func__); +} + +static void PHY_SetRFPathSwitch_default( + PADAPTER pAdapter, + BOOLEAN bMain +) +{ + RTW_INFO("%s\n", __func__); +} +#endif + +void mpt_InitHWConfig(PADAPTER Adapter) +{ + PHAL_DATA_TYPE hal; + + hal = GET_HAL_DATA(Adapter); + + if (IS_HARDWARE_TYPE_8723B(Adapter)) { + /* TODO: <20130114, Kordan> The following setting is only for DPDT and Fixed board type. */ + /* TODO: A better solution is configure it according EFUSE during the run-time. */ + + phy_set_mac_reg(Adapter, 0x64, BIT20, 0x0); /* 0x66[4]=0 */ + phy_set_mac_reg(Adapter, 0x64, BIT24, 0x0); /* 0x66[8]=0 */ + phy_set_mac_reg(Adapter, 0x40, BIT4, 0x0); /* 0x40[4]=0 */ + phy_set_mac_reg(Adapter, 0x40, BIT3, 0x1); /* 0x40[3]=1 */ + phy_set_mac_reg(Adapter, 0x4C, BIT24, 0x1); /* 0x4C[24:23]=10 */ + phy_set_mac_reg(Adapter, 0x4C, BIT23, 0x0); /* 0x4C[24:23]=10 */ + phy_set_bb_reg(Adapter, 0x944, BIT1 | BIT0, 0x3); /* 0x944[1:0]=11 */ + phy_set_bb_reg(Adapter, 0x930, bMaskByte0, 0x77);/* 0x930[7:0]=77 */ + phy_set_mac_reg(Adapter, 0x38, BIT11, 0x1);/* 0x38[11]=1 */ + + /* TODO: <20130206, Kordan> The default setting is wrong, hard-coded here. */ + phy_set_mac_reg(Adapter, 0x778, 0x3, 0x3); /* Turn off hardware PTA control (Asked by Scott) */ + phy_set_mac_reg(Adapter, 0x64, bMaskDWord, 0x36000000);/* Fix BT S0/S1 */ + phy_set_mac_reg(Adapter, 0x948, bMaskDWord, 0x0); /* Fix BT can't Tx */ + + /* <20130522, Kordan> Turn off equalizer to improve Rx sensitivity. (Asked by EEChou) */ + phy_set_bb_reg(Adapter, 0xA00, BIT8, 0x0); /*0xA01[0] = 0*/ + } else if (IS_HARDWARE_TYPE_8821(Adapter)) { + /* <20131121, VincentL> Add for 8821AU DPDT setting and fix switching antenna issue (Asked by Rock) + <20131122, VincentL> Enable for all 8821A/8811AU (Asked by Alex)*/ + phy_set_mac_reg(Adapter, 0x4C, BIT23, 0x0); /*0x4C[23:22]=01*/ + phy_set_mac_reg(Adapter, 0x4C, BIT22, 0x1); /*0x4C[23:22]=01*/ + } else if (IS_HARDWARE_TYPE_8188ES(Adapter)) + phy_set_mac_reg(Adapter, 0x4C , BIT23, 0); /*select DPDT_P and DPDT_N as output pin*/ +#ifdef CONFIG_RTL8814A + else if (IS_HARDWARE_TYPE_8814A(Adapter)) + PlatformEFIOWrite2Byte(Adapter, REG_RXFLTMAP1_8814A, 0x2000); +#endif + +#ifdef CONFIG_RTL8812A + else if (IS_HARDWARE_TYPE_8812(Adapter)) { + rtw_write32(Adapter, 0x520, rtw_read32(Adapter, 0x520) | 0x8000); + rtw_write32(Adapter, 0x524, rtw_read32(Adapter, 0x524) & (~0x800)); + } +#endif + + +#ifdef CONFIG_RTL8822B + else if (IS_HARDWARE_TYPE_8822B(Adapter)) { + u32 tmp_reg = 0; + + PlatformEFIOWrite2Byte(Adapter, REG_RXFLTMAP1_8822B, 0x2000); + /* fixed wifi can't 2.4g tx suggest by Szuyitasi 20160504 */ + phy_set_bb_reg(Adapter, 0x70, bMaskByte3, 0x0e); + RTW_INFO(" 0x73 = 0x%x\n", phy_query_bb_reg(Adapter, 0x70, bMaskByte3)); + phy_set_bb_reg(Adapter, 0x1704, bMaskDWord, 0x0000ff00); + RTW_INFO(" 0x1704 = 0x%x\n", phy_query_bb_reg(Adapter, 0x1704, bMaskDWord)); + phy_set_bb_reg(Adapter, 0x1700, bMaskDWord, 0xc00f0038); + RTW_INFO(" 0x1700 = 0x%x\n", phy_query_bb_reg(Adapter, 0x1700, bMaskDWord)); + } +#endif /* CONFIG_RTL8822B */ +#ifdef CONFIG_RTL8821C + else if (IS_HARDWARE_TYPE_8821C(Adapter)) + PlatformEFIOWrite2Byte(Adapter, REG_RXFLTMAP1_8821C, 0x2000); +#endif /* CONFIG_RTL8821C */ +#if defined(CONFIG_RTL8188F) || defined(CONFIG_RTL8188GTV) + else if (IS_HARDWARE_TYPE_8188F(Adapter) || IS_HARDWARE_TYPE_8188GTV(Adapter)) { + if (IS_A_CUT(hal->version_id) || IS_B_CUT(hal->version_id)) { + RTW_INFO("%s() Active large power detection\n", __func__); + phy_active_large_power_detection_8188f(&(GET_HAL_DATA(Adapter)->odmpriv)); + } + } +#endif +#if defined(CONFIG_RTL8822C) + else if( IS_HARDWARE_TYPE_8822C(Adapter)) { + rtw_write16(Adapter, REG_RXFLTMAP1_8822C, 0x2000); + /* 0x7D8[31] : time out enable when cca is not assert + 0x60D[7:0] : time out value (Unit : us)*/ + rtw_write8(Adapter, 0x7db, 0xc0); + RTW_INFO(" 0x7d8 = 0x%x\n", rtw_read8(Adapter, 0x7d8)); + rtw_write8(Adapter, 0x60d, 0x0c); + RTW_INFO(" 0x60d = 0x%x\n", rtw_read8(Adapter, 0x60d)); + phy_set_bb_reg(Adapter, 0x1c44, BIT10, 0x1); + RTW_INFO(" 0x1c44 = 0x%x\n", phy_query_bb_reg(Adapter, 0x1c44, bMaskDWord)); + } +#endif +#if defined(CONFIG_RTL8814B) + else if(IS_HARDWARE_TYPE_8814B(Adapter)) + { + PlatformEFIOWrite2Byte(Adapter, REG_RXFLTMAP1_8814B, 0x2000); + } +#endif +#if defined(CONFIG_RTL8723F) + /* todo: 8723F not verify yet */ + else if (IS_HARDWARE_TYPE_8723F(Adapter)) { + /* 8723F mac is similar with 8723D, + * but can't find 8723D here. + */ + } +#endif +} + +static void PHY_IQCalibrate(PADAPTER padapter, u8 bReCovery) +{ + halrf_iqk_trigger(&(GET_HAL_DATA(padapter)->odmpriv), bReCovery); +} + +static void PHY_LCCalibrate(PADAPTER padapter) +{ + halrf_lck_trigger(&(GET_HAL_DATA(padapter)->odmpriv)); +} + +static u8 PHY_QueryRFPathSwitch(PADAPTER padapter) +{ + u8 bmain = 0; +/* + if (IS_HARDWARE_TYPE_8723B(padapter)) { +#ifdef CONFIG_RTL8723B + bmain = PHY_QueryRFPathSwitch_8723B(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8188E(padapter)) { +#ifdef CONFIG_RTL8188E + bmain = PHY_QueryRFPathSwitch_8188E(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8814A(padapter)) { +#ifdef CONFIG_RTL8814A + bmain = PHY_QueryRFPathSwitch_8814A(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8812(padapter) || IS_HARDWARE_TYPE_8821(padapter)) { +#if defined(CONFIG_RTL8812A) || defined(CONFIG_RTL8821A) + bmain = PHY_QueryRFPathSwitch_8812A(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8192E(padapter)) { +#ifdef CONFIG_RTL8192E + bmain = PHY_QueryRFPathSwitch_8192E(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8703B(padapter)) { +#ifdef CONFIG_RTL8703B + bmain = PHY_QueryRFPathSwitch_8703B(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8188F(padapter)) { +#ifdef CONFIG_RTL8188F + bmain = PHY_QueryRFPathSwitch_8188F(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8188GTV(padapter)) { +#ifdef CONFIG_RTL8188GTV + bmain = PHY_QueryRFPathSwitch_8188GTV(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8822B(padapter)) { +#ifdef CONFIG_RTL8822B + bmain = PHY_QueryRFPathSwitch_8822B(padapter); +#endif + } else if (IS_HARDWARE_TYPE_8723D(padapter)) { +#ifdef CONFIG_RTL8723D + bmain = PHY_QueryRFPathSwitch_8723D(padapter); +#endif + } else +*/ + + if (IS_HARDWARE_TYPE_8821C(padapter)) { +#ifdef CONFIG_RTL8821C + bmain = phy_query_rf_path_switch_8821c(padapter); +#endif + } + + return bmain; +} + +static void PHY_SetRFPathSwitch(PADAPTER padapter , BOOLEAN bMain) { + + PHAL_DATA_TYPE hal = GET_HAL_DATA(padapter); + struct dm_struct *phydm = &hal->odmpriv; + + if (IS_HARDWARE_TYPE_8723B(padapter)) { +#ifdef CONFIG_RTL8723B + phy_set_rf_path_switch_8723b(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8188E(padapter)) { +#ifdef CONFIG_RTL8188E + phy_set_rf_path_switch_8188e(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8814A(padapter)) { +#ifdef CONFIG_RTL8814A + phy_set_rf_path_switch_8814a(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8812(padapter) || IS_HARDWARE_TYPE_8821(padapter)) { +#if defined(CONFIG_RTL8812A) || defined(CONFIG_RTL8821A) + phy_set_rf_path_switch_8812a(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8192E(padapter)) { +#ifdef CONFIG_RTL8192E + phy_set_rf_path_switch_8192e(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8703B(padapter)) { +#ifdef CONFIG_RTL8703B + phy_set_rf_path_switch_8703b(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8188F(padapter) || IS_HARDWARE_TYPE_8188GTV(padapter)) { +#if defined(CONFIG_RTL8188F) || defined(CONFIG_RTL8188GTV) + phy_set_rf_path_switch_8188f(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8192F(padapter)) { +#ifdef CONFIG_RTL8192F + phy_set_rf_path_switch_8192f(padapter, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8822B(padapter)) { +#ifdef CONFIG_RTL8822B + phy_set_rf_path_switch_8822b(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8723D(padapter)) { +#ifdef CONFIG_RTL8723D + phy_set_rf_path_switch_8723d(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8821C(padapter)) { +#ifdef CONFIG_RTL8821C + phy_set_rf_path_switch_8821c(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8822C(padapter)) { +#ifdef CONFIG_RTL8822C + phy_set_rf_path_switch_8822c(phydm, bMain); +#endif + } else if (IS_HARDWARE_TYPE_8814B(padapter)) { +#ifdef CONFIG_RTL8814B + /* phy_set_rf_path_switch_8814b(phydm, bMain); */ +#endif + } else if (IS_HARDWARE_TYPE_8723F(padapter)) { +#ifdef CONFIG_RTL8723F + phy_set_rf_path_switch_8723f(phydm, bMain); +#endif + } +} + + +static void phy_switch_rf_path_set(PADAPTER padapter , u8 *prf_set_State) { +#ifdef CONFIG_RTL8821C + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct dm_struct *p_dm = &pHalData->odmpriv; + + if (IS_HARDWARE_TYPE_8821C(padapter)) { + config_phydm_set_ant_path(p_dm, *prf_set_State, p_dm->current_ant_num_8821c); + /* Do IQK when switching to BTG/WLG, requested by RF Binson */ + if (*prf_set_State == SWITCH_TO_BTG || *prf_set_State == SWITCH_TO_WLG) + PHY_IQCalibrate(padapter, FALSE); + } +#endif + +} + + +#ifdef CONFIG_ANTENNA_DIVERSITY +u8 rtw_mp_set_antdiv(PADAPTER padapter, BOOLEAN bMain) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + u8 cur_ant, change_ant; + + if (!pHalData->AntDivCfg) + return _FALSE; + /*rtw_hal_get_odm_var(padapter, HAL_ODM_ANTDIV_SELECT, &cur_ant, NULL);*/ + change_ant = (bMain == MAIN_ANT) ? MAIN_ANT : AUX_ANT; + + RTW_INFO("%s: config %s\n", __func__, (bMain == MAIN_ANT) ? "MAIN_ANT" : "AUX_ANT"); + rtw_antenna_select_cmd(padapter, change_ant, _FALSE); + + return _TRUE; +} +#endif + +s32 +MPT_InitializeAdapter( + PADAPTER pAdapter, + u8 Channel +) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(pAdapter); + s32 rtStatus = _SUCCESS; + PMPT_CONTEXT pMptCtx = &pAdapter->mppriv.mpt_ctx; + u32 ledsetting; + + pMptCtx->bMptDrvUnload = _FALSE; + pMptCtx->bMassProdTest = _FALSE; + pMptCtx->bMptIndexEven = _TRUE; /* default gain index is -6.0db */ + pMptCtx->h2cReqNum = 0x0; + /* init for BT MP */ +#if defined(CONFIG_RTL8723B) + pMptCtx->bMPh2c_timeout = _FALSE; + pMptCtx->MptH2cRspEvent = _FALSE; + pMptCtx->MptBtC2hEvent = _FALSE; + _rtw_init_sema(&pMptCtx->MPh2c_Sema, 0); + rtw_init_timer(&pMptCtx->MPh2c_timeout_timer, pAdapter, MPh2c_timeout_handle, pAdapter); +#endif + + mpt_InitHWConfig(pAdapter); + +#ifdef CONFIG_RTL8723B + rtl8723b_InitAntenna_Selection(pAdapter); + if (IS_HARDWARE_TYPE_8723B(pAdapter)) { + + /* <20130522, Kordan> Turn off equalizer to improve Rx sensitivity. (Asked by EEChou)*/ + phy_set_bb_reg(pAdapter, 0xA00, BIT8, 0x0); + PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/); /*default use Main*/ + + if (pHalData->PackageType == PACKAGE_DEFAULT) + phy_set_rf_reg(pAdapter, RF_PATH_A, 0x51, bRFRegOffsetMask, 0x6B04E); + else + phy_set_rf_reg(pAdapter, RF_PATH_A, 0x51, bRFRegOffsetMask, 0x6F10E); + + } + /*set ant to wifi side in mp mode*/ + rtw_write16(pAdapter, 0x870, 0x300); + rtw_write16(pAdapter, 0x860, 0x110); +#endif + + pMptCtx->bMptWorkItemInProgress = _FALSE; + pMptCtx->CurrMptAct = NULL; + pMptCtx->mpt_rf_path = RF_PATH_A; + /* ------------------------------------------------------------------------- */ + /* Don't accept any packets */ + rtw_write32(pAdapter, REG_RCR, 0); + + /* ledsetting = rtw_read32(pAdapter, REG_LEDCFG0); */ + /* rtw_write32(pAdapter, REG_LEDCFG0, ledsetting & ~LED0DIS); */ + + /* rtw_write32(pAdapter, REG_LEDCFG0, 0x08080); */ + ledsetting = rtw_read32(pAdapter, REG_LEDCFG0); + + + PHY_LCCalibrate(pAdapter); + PHY_IQCalibrate(pAdapter, _FALSE); + /* dm_check_txpowertracking(&pHalData->odmpriv); */ /* trigger thermal meter */ + + PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/); /* default use Main */ + + pMptCtx->backup0xc50 = (u8)phy_query_bb_reg(pAdapter, rOFDM0_XAAGCCore1, bMaskByte0); + pMptCtx->backup0xc58 = (u8)phy_query_bb_reg(pAdapter, rOFDM0_XBAGCCore1, bMaskByte0); + pMptCtx->backup0xc30 = (u8)phy_query_bb_reg(pAdapter, rOFDM0_RxDetector1, bMaskByte0); + pMptCtx->backup0x52_RF_A = (u8)phy_query_rf_reg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0); + pMptCtx->backup0x52_RF_B = (u8)phy_query_rf_reg(pAdapter, RF_PATH_B, RF_0x52, 0x000F0); +#ifdef CONFIG_RTL8188E + rtw_write32(pAdapter, REG_MACID_NO_LINK_0, 0x0); + rtw_write32(pAdapter, REG_MACID_NO_LINK_1, 0x0); +#endif +#ifdef CONFIG_RTL8814A + if (IS_HARDWARE_TYPE_8814A(pAdapter)) { + pHalData->BackUp_IG_REG_4_Chnl_Section[0] = (u8)phy_query_bb_reg(pAdapter, rA_IGI_Jaguar, bMaskByte0); + pHalData->BackUp_IG_REG_4_Chnl_Section[1] = (u8)phy_query_bb_reg(pAdapter, rB_IGI_Jaguar, bMaskByte0); + pHalData->BackUp_IG_REG_4_Chnl_Section[2] = (u8)phy_query_bb_reg(pAdapter, rC_IGI_Jaguar2, bMaskByte0); + pHalData->BackUp_IG_REG_4_Chnl_Section[3] = (u8)phy_query_bb_reg(pAdapter, rD_IGI_Jaguar2, bMaskByte0); + } +#endif + return rtStatus; +} + +/*----------------------------------------------------------------------------- + * Function: MPT_DeInitAdapter() + * + * Overview: Extra DeInitialization for Mass Production Test. + * + * Input: PADAPTER pAdapter + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 05/08/2007 MHC Create Version 0. + * 05/18/2007 MHC Add normal driver MPHalt code. + * + *---------------------------------------------------------------------------*/ +void +MPT_DeInitAdapter( + PADAPTER pAdapter +) +{ + PMPT_CONTEXT pMptCtx = &pAdapter->mppriv.mpt_ctx; + + pMptCtx->bMptDrvUnload = _TRUE; +#if defined(CONFIG_RTL8723B) + _rtw_free_sema(&(pMptCtx->MPh2c_Sema)); + _cancel_timer_ex(&pMptCtx->MPh2c_timeout_timer); +#endif +#if defined(CONFIG_RTL8723B) + phy_set_bb_reg(pAdapter, 0xA01, BIT0, 1); /* /suggestion by jerry for MP Rx. */ +#endif +#if 0 /* for Windows */ + PlatformFreeWorkItem(&(pMptCtx->MptWorkItem)); + + while (pMptCtx->bMptWorkItemInProgress) { + if (NdisWaitEvent(&(pMptCtx->MptWorkItemEvent), 50)) + break; + } + NdisFreeSpinLock(&(pMptCtx->MptWorkItemSpinLock)); +#endif +} + +static u8 mpt_ProStartTest(PADAPTER padapter) +{ + PMPT_CONTEXT pMptCtx = &padapter->mppriv.mpt_ctx; + + pMptCtx->bMassProdTest = _TRUE; + pMptCtx->is_start_cont_tx = _FALSE; + pMptCtx->bCckContTx = _FALSE; + pMptCtx->bOfdmContTx = _FALSE; + pMptCtx->bSingleCarrier = _FALSE; + pMptCtx->is_carrier_suppression = _FALSE; + pMptCtx->is_single_tone = _FALSE; + pMptCtx->HWTxmode = PACKETS_TX; + + return _SUCCESS; +} + +/* + * General use + */ +s32 SetPowerTracking(PADAPTER padapter, u8 enable) +{ + + hal_mpt_SetPowerTracking(padapter, enable); + return 0; +} + +void GetPowerTracking(PADAPTER padapter, u8 *enable) +{ + hal_mpt_GetPowerTracking(padapter, enable); +} + +void rtw_mp_trigger_iqk(PADAPTER padapter) +{ + PHY_IQCalibrate(padapter, _FALSE); +} + +void rtw_mp_trigger_lck(PADAPTER padapter) +{ + PHY_LCCalibrate(padapter); +} + +void rtw_mp_trigger_dpk(PADAPTER padapter) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct dm_struct *pDM_Odm = &pHalData->odmpriv; + + halrf_dpk_trigger(pDM_Odm); +} + +static void init_mp_data(PADAPTER padapter) +{ + u8 v8; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct dm_struct *pDM_Odm = &pHalData->odmpriv; + + /*disable BCN*/ +#ifdef CONFIG_PROTSEL_PORT + rtw_hal_hw_port_disable(padapter); +#else + v8 = rtw_read8(padapter, REG_BCN_CTRL); + v8 &= ~EN_BCN_FUNCTION; + rtw_write8(padapter, REG_BCN_CTRL, v8); +#endif + + pDM_Odm->rf_calibrate_info.txpowertrack_control = _FALSE; +} + + +void MPT_PwrCtlDM(PADAPTER padapter, u32 trk_type) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct dm_struct *pDM_Odm = &pHalData->odmpriv; + u32 rf_ability; + + padapter->mppriv.tssitrk_on = trk_type == 3; + + if (trk_type == 0) { /* thermal PwrTrk off*/ + struct txpwrtrack_cfg c; + u8 chnl = 0 ; + + RTW_INFO("in Thermal tracking off\n"); + rf_ability = ((u32)halrf_cmn_info_get(pDM_Odm, HALRF_CMNINFO_ABILITY)) & ~HAL_RF_TX_PWR_TRACK; + halrf_cmn_info_set(pDM_Odm, HALRF_CMNINFO_ABILITY, rf_ability); + halrf_cmn_info_set(pDM_Odm, HALRF_CMNINFO_POWER_TRACK_CONTROL, trk_type); + halrf_set_pwr_track(pDM_Odm, FALSE); + pDM_Odm->rf_calibrate_info.txpowertrack_control = trk_type; + if (IS_HARDWARE_TYPE_8822C(padapter)) + padapter->mppriv.mp_dm = 1; /* default enable dpk tracking */ + else + padapter->mppriv.mp_dm = 0; + + _rtw_memset(&c, 0, sizeof(struct txpwrtrack_cfg)); + configure_txpower_track(pDM_Odm, &c); + odm_clear_txpowertracking_state(pDM_Odm); + if (*c.odm_tx_pwr_track_set_pwr) { + if (pDM_Odm->support_ic_type == ODM_RTL8188F) + (*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, MIX_MODE, RF_PATH_A, chnl); + else if (pDM_Odm->support_ic_type == ODM_RTL8723D) { + (*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, BBSWING, RF_PATH_A, chnl); + SetTxPower(padapter); + } else if (pDM_Odm->support_ic_type == ODM_RTL8192F) { + (*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, MIX_MODE, RF_PATH_A, chnl); + (*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, MIX_MODE, RF_PATH_B, chnl); + } else { + (*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, BBSWING, RF_PATH_A, chnl); + (*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, BBSWING, RF_PATH_B, chnl); + } + } + return ; + } + + rf_ability = ((u32)halrf_cmn_info_get(pDM_Odm, HALRF_CMNINFO_ABILITY)) | HAL_RF_TX_PWR_TRACK; + halrf_cmn_info_set(pDM_Odm, HALRF_CMNINFO_ABILITY, rf_ability); + halrf_cmn_info_set(pDM_Odm, HALRF_CMNINFO_POWER_TRACK_CONTROL, trk_type); + if (trk_type == 1 || trk_type == 3) /* Thermal PwrTrk ON , TSSI PwrTrk ON */ + halrf_set_pwr_track(pDM_Odm, TRUE); + else + halrf_set_pwr_track(pDM_Odm, false);/* TSSI K */ + pDM_Odm->rf_calibrate_info.txpowertrack_control = trk_type; + padapter->mppriv.mp_dm = 1; + +} + + +u32 mp_join(PADAPTER padapter, u8 mode) +{ + WLAN_BSSID_EX bssid; + struct sta_info *psta; + u32 length; + _irqL irqL; + s32 res = _SUCCESS; + + struct mp_priv *pmppriv = &padapter->mppriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *tgt_network = &pmlmepriv->cur_network; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network)); + + /* 1. initialize a new WLAN_BSSID_EX */ + _rtw_memset(&bssid, 0, sizeof(WLAN_BSSID_EX)); + RTW_INFO("%s ,pmppriv->network_macaddr=%x %x %x %x %x %x\n", __func__, + pmppriv->network_macaddr[0], pmppriv->network_macaddr[1], pmppriv->network_macaddr[2], pmppriv->network_macaddr[3], pmppriv->network_macaddr[4], + pmppriv->network_macaddr[5]); + _rtw_memcpy(bssid.MacAddress, pmppriv->network_macaddr, ETH_ALEN); + + if (mode == WIFI_FW_ADHOC_STATE) { + bssid.Ssid.SsidLength = strlen("mp_pseudo_adhoc"); + _rtw_memcpy(bssid.Ssid.Ssid, (u8 *)"mp_pseudo_adhoc", bssid.Ssid.SsidLength); + bssid.InfrastructureMode = Ndis802_11IBSS; + bssid.IELength = 0; + bssid.Configuration.DSConfig = pmppriv->channel; + + } else if (mode == WIFI_FW_STATION_STATE) { + bssid.Ssid.SsidLength = strlen("mp_pseudo_STATION"); + _rtw_memcpy(bssid.Ssid.Ssid, (u8 *)"mp_pseudo_STATION", bssid.Ssid.SsidLength); + bssid.InfrastructureMode = Ndis802_11Infrastructure; + bssid.IELength = 0; + } + + length = get_WLAN_BSSID_EX_sz(&bssid); + if (length % 4) + bssid.Length = ((length >> 2) + 1) << 2; /* round up to multiple of 4 bytes. */ + else + bssid.Length = length; + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) + goto end_of_mp_start_test; + + /* init mp_start_test status */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) { + rtw_disassoc_cmd(padapter, 500, 0); + rtw_indicate_disconnect(padapter, 0, _FALSE); + rtw_free_assoc_resources_cmd(padapter, _TRUE, 0); + } + pmppriv->prev_fw_state = get_fwstate(pmlmepriv); + /*pmlmepriv->fw_state = WIFI_MP_STATE;*/ + init_fwstate(pmlmepriv, WIFI_MP_STATE); + + set_fwstate(pmlmepriv, WIFI_UNDER_LINKING); + + /* 3 2. create a new psta for mp driver */ + /* clear psta in the cur_network, if any */ + psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress); + if (psta) + rtw_free_stainfo(padapter, psta); + + psta = rtw_alloc_stainfo(&padapter->stapriv, bssid.MacAddress); + if (psta == NULL) { + /*pmlmepriv->fw_state = pmppriv->prev_fw_state;*/ + init_fwstate(pmlmepriv, pmppriv->prev_fw_state); + res = _FAIL; + goto end_of_mp_start_test; + } + if (mode == WIFI_FW_ADHOC_STATE) + set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE); + else + set_fwstate(pmlmepriv, WIFI_STATION_STATE); + /* 3 3. join psudo AdHoc */ + tgt_network->join_res = 1; + tgt_network->aid = psta->cmn.aid = 1; + + _rtw_memcpy(&padapter->registrypriv.dev_network, &bssid, length); + rtw_update_registrypriv_dev_network(padapter); + _rtw_memcpy(&tgt_network->network, &padapter->registrypriv.dev_network, padapter->registrypriv.dev_network.Length); + _rtw_memcpy(pnetwork, &padapter->registrypriv.dev_network, padapter->registrypriv.dev_network.Length); + + rtw_indicate_connect(padapter); + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + set_fwstate(pmlmepriv, WIFI_ASOC_STATE); + +end_of_mp_start_test: + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + if (1) { /* (res == _SUCCESS) */ + /* set MSR to WIFI_FW_ADHOC_STATE */ + if (mode == WIFI_FW_ADHOC_STATE) { + /* set msr to WIFI_FW_ADHOC_STATE */ + pmlmeinfo->state = WIFI_FW_ADHOC_STATE; + Set_MSR(padapter, (pmlmeinfo->state & 0x3)); + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress); + rtw_hal_rcr_set_chk_bssid(padapter, MLME_ADHOC_STARTED); + pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS; + } else { + Set_MSR(padapter, WIFI_FW_STATION_STATE); + + RTW_INFO("%s , pmppriv->network_macaddr =%x %x %x %x %x %x\n", __func__, + pmppriv->network_macaddr[0], pmppriv->network_macaddr[1], pmppriv->network_macaddr[2], pmppriv->network_macaddr[3], pmppriv->network_macaddr[4], + pmppriv->network_macaddr[5]); + + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmppriv->network_macaddr); + } + } + + return res; +} +/* This function initializes the DUT to the MP test mode */ +s32 mp_start_test(PADAPTER padapter) +{ + struct mp_priv *pmppriv = &padapter->mppriv; +#ifdef CONFIG_PCI_HCI + PHAL_DATA_TYPE hal; +#endif + s32 res = _SUCCESS; + + padapter->registrypriv.mp_mode = 1; + + init_mp_data(padapter); +#ifdef CONFIG_RTL8814A + rtl8814_InitHalDm(padapter); +#endif /* CONFIG_RTL8814A */ +#ifdef CONFIG_RTL8812A + rtl8812_InitHalDm(padapter); +#endif /* CONFIG_RTL8812A */ +#ifdef CONFIG_RTL8723B + rtl8723b_InitHalDm(padapter); +#endif /* CONFIG_RTL8723B */ +#ifdef CONFIG_RTL8703B + rtl8703b_InitHalDm(padapter); +#endif /* CONFIG_RTL8703B */ +#ifdef CONFIG_RTL8192E + rtl8192e_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8188F + rtl8188f_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8188GTV + rtl8188gtv_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8188E + rtl8188e_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8723D + rtl8723d_InitHalDm(padapter); +#endif /* CONFIG_RTL8723D */ + +#ifdef CONFIG_PCI_HCI + hal = GET_HAL_DATA(padapter); + hal->pci_backdoor_ctrl = 0; + rtw_pci_aspm_config(padapter); +#endif + + + /* 3 0. update mp_priv */ + switch (GET_HAL_RFPATH(padapter)) { + case RF_1T1R: + pmppriv->antenna_tx = ANTENNA_A; + pmppriv->antenna_rx = ANTENNA_A; + break; + case RF_1T2R: + default: + pmppriv->antenna_tx = ANTENNA_A; + pmppriv->antenna_rx = ANTENNA_AB; + break; + case RF_2T2R: + pmppriv->antenna_tx = ANTENNA_AB; + pmppriv->antenna_rx = ANTENNA_AB; + break; + case RF_2T4R: + pmppriv->antenna_tx = ANTENNA_AB; + pmppriv->antenna_rx = ANTENNA_ABCD; + break; + } + + mpt_ProStartTest(padapter); + + mp_join(padapter, WIFI_FW_ADHOC_STATE); + + return res; +} +/* ------------------------------------------------------------------------------ + * This function change the DUT from the MP test mode into normal mode */ +void mp_stop_test(PADAPTER padapter) +{ + struct mp_priv *pmppriv = &padapter->mppriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wlan_network *tgt_network = &pmlmepriv->cur_network; + struct sta_info *psta; +#ifdef CONFIG_PCI_HCI + struct registry_priv *registry_par = &padapter->registrypriv; + PHAL_DATA_TYPE hal; +#endif + + _irqL irqL; + + if (pmppriv->mode == MP_ON) { + pmppriv->bSetTxPower = 0; + _enter_critical_bh(&pmlmepriv->lock, &irqL); + if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _FALSE) + goto end_of_mp_stop_test; + + /* 3 1. disconnect psudo AdHoc */ + rtw_indicate_disconnect(padapter, 0, _FALSE); + + /* 3 2. clear psta used in mp test mode. + * rtw_free_assoc_resources(padapter, _TRUE); */ + psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress); + if (psta) + rtw_free_stainfo(padapter, psta); + + /* 3 3. return to normal state (default:station mode) */ + /*pmlmepriv->fw_state = pmppriv->prev_fw_state; */ /* WIFI_STATION_STATE;*/ + init_fwstate(pmlmepriv, pmppriv->prev_fw_state); + + /* flush the cur_network */ + _rtw_memset(tgt_network, 0, sizeof(struct wlan_network)); + + _clr_fwstate_(pmlmepriv, WIFI_MP_STATE); + +end_of_mp_stop_test: + + _exit_critical_bh(&pmlmepriv->lock, &irqL); + +#ifdef CONFIG_PCI_HCI + hal = GET_HAL_DATA(padapter); + hal->pci_backdoor_ctrl = registry_par->pci_aspm_config; + rtw_pci_aspm_config(padapter); +#endif + +#ifdef CONFIG_RTL8812A + rtl8812_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8723B + rtl8723b_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8703B + rtl8703b_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8192E + rtl8192e_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8188F + rtl8188f_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8188GTV + rtl8188gtv_InitHalDm(padapter); +#endif +#ifdef CONFIG_RTL8723D + rtl8723d_InitHalDm(padapter); +#endif + } +} +/*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/ +#if 0 +/* #ifdef CONFIG_USB_HCI */ +static void mpt_AdjustRFRegByRateByChan92CU(PADAPTER pAdapter, u8 RateIdx, u8 Channel, u8 BandWidthID) +{ + u8 eRFPath; + u32 rfReg0x26; + struct hal_spec_t *hal_spec = GET_HAL_SPEC(pAdapter); + + + if (RateIdx < MPT_RATE_6M) /* CCK rate,for 88cu */ + rfReg0x26 = 0xf400; + else if ((RateIdx >= MPT_RATE_6M) && (RateIdx <= MPT_RATE_54M)) {/* OFDM rate,for 88cu */ + if ((4 == Channel) || (8 == Channel) || (12 == Channel)) + rfReg0x26 = 0xf000; + else if ((5 == Channel) || (7 == Channel) || (13 == Channel) || (14 == Channel)) + rfReg0x26 = 0xf400; + else + rfReg0x26 = 0x4f200; + } else if ((RateIdx >= MPT_RATE_MCS0) && (RateIdx <= MPT_RATE_MCS15)) { + /* MCS 20M ,for 88cu */ /* MCS40M rate,for 88cu */ + + if (CHANNEL_WIDTH_20 == BandWidthID) { + if ((4 == Channel) || (8 == Channel)) + rfReg0x26 = 0xf000; + else if ((5 == Channel) || (7 == Channel) || (13 == Channel) || (14 == Channel)) + rfReg0x26 = 0xf400; + else + rfReg0x26 = 0x4f200; + } else { + if ((4 == Channel) || (8 == Channel)) + rfReg0x26 = 0xf000; + else if ((5 == Channel) || (7 == Channel)) + rfReg0x26 = 0xf400; + else + rfReg0x26 = 0x4f200; + } + } + + for (eRFPath = 0; eRFPath < hal_spec->rf_reg_path_num; eRFPath++) + write_rfreg(pAdapter, eRFPath, RF_SYN_G2, rfReg0x26); +} +#endif +/*----------------------------------------------------------------------------- + * Function: mpt_SwitchRfSetting + * + * Overview: Change RF Setting when we siwthc channel/rate/BW for MP. + * + * Input: PADAPTER pAdapter + * + * Output: NONE + * + * Return: NONE + * + * Revised History: + * When Who Remark + * 01/08/2009 MHC Suggestion from SD3 Willis for 92S series. + * 01/09/2009 MHC Add CCK modification for 40MHZ. Suggestion from SD3. + * + *---------------------------------------------------------------------------*/ +#if 0 +static void mpt_SwitchRfSetting(PADAPTER pAdapter) +{ + hal_mpt_SwitchRfSetting(pAdapter); +} + +/*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/ +/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/ +static void MPT_CCKTxPowerAdjust(PADAPTER Adapter, BOOLEAN bInCH14) +{ + hal_mpt_CCKTxPowerAdjust(Adapter, bInCH14); +} +#endif + +/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/ + +/* + * SetChannel + * Description + * Use H2C command to change channel, + * not only modify rf register, but also other setting need to be done. + */ +void SetChannel(PADAPTER pAdapter) +{ + hal_mpt_SetChannel(pAdapter); +} + +/* + * Notice + * Switch bandwitdth may change center frequency(channel) + */ +void SetBandwidth(PADAPTER pAdapter) +{ + hal_mpt_SetBandwidth(pAdapter); + +} + +void SetAntenna(PADAPTER pAdapter) +{ + hal_mpt_SetAntenna(pAdapter); +} + +int SetTxPower(PADAPTER pAdapter) +{ + + hal_mpt_SetTxPower(pAdapter); + return _TRUE; +} + +void SetTxAGCOffset(PADAPTER pAdapter, u32 ulTxAGCOffset) +{ + u32 TxAGCOffset_B, TxAGCOffset_C, TxAGCOffset_D, tmpAGC; + + TxAGCOffset_B = (ulTxAGCOffset & 0x000000ff); + TxAGCOffset_C = ((ulTxAGCOffset & 0x0000ff00) >> 8); + TxAGCOffset_D = ((ulTxAGCOffset & 0x00ff0000) >> 16); + + tmpAGC = (TxAGCOffset_D << 8 | TxAGCOffset_C << 4 | TxAGCOffset_B); + write_bbreg(pAdapter, rFPGA0_TxGainStage, + (bXBTxAGC | bXCTxAGC | bXDTxAGC), tmpAGC); +} + +void SetDataRate(PADAPTER pAdapter) +{ + hal_mpt_SetDataRate(pAdapter); +} + +void MP_PHY_SetRFPathSwitch(PADAPTER pAdapter , BOOLEAN bMain) +{ + + PHY_SetRFPathSwitch(pAdapter, bMain); + +} + +void mp_phy_switch_rf_path_set(PADAPTER pAdapter , u8 *pstate) +{ + + phy_switch_rf_path_set(pAdapter, pstate); + +} + +u8 MP_PHY_QueryRFPathSwitch(PADAPTER pAdapter) +{ + return PHY_QueryRFPathSwitch(pAdapter); +} + +s32 SetThermalMeter(PADAPTER pAdapter, u8 target_ther) +{ + return hal_mpt_SetThermalMeter(pAdapter, target_ther); +} + +#if 0 +static void TriggerRFThermalMeter(PADAPTER pAdapter) +{ + hal_mpt_TriggerRFThermalMeter(pAdapter); +} + +static u8 ReadRFThermalMeter(PADAPTER pAdapter) +{ + return hal_mpt_ReadRFThermalMeter(pAdapter); +} +#endif + +void GetThermalMeter(PADAPTER pAdapter, u8 rfpath ,u8 *value) +{ + hal_mpt_GetThermalMeter(pAdapter, rfpath, value); +} + +void SetSingleCarrierTx(PADAPTER pAdapter, u8 bStart) +{ + PhySetTxPowerLevel(pAdapter); + hal_mpt_SetSingleCarrierTx(pAdapter, bStart); +} + +void SetSingleToneTx(PADAPTER pAdapter, u8 bStart) +{ + PhySetTxPowerLevel(pAdapter); + hal_mpt_SetSingleToneTx(pAdapter, bStart); +} + +void SetCarrierSuppressionTx(PADAPTER pAdapter, u8 bStart) +{ + PhySetTxPowerLevel(pAdapter); + hal_mpt_SetCarrierSuppressionTx(pAdapter, bStart); +} + +void SetContinuousTx(PADAPTER pAdapter, u8 bStart) +{ + PhySetTxPowerLevel(pAdapter); + hal_mpt_SetContinuousTx(pAdapter, bStart); +} + + +void PhySetTxPowerLevel(PADAPTER pAdapter) +{ + struct mp_priv *pmp_priv = &pAdapter->mppriv; + + + if (pmp_priv->bSetTxPower == 0) /* for NO manually set power index */ + rtw_hal_set_tx_power_level(pAdapter, pmp_priv->channel); +} + +/* ------------------------------------------------------------------------------ */ +static void dump_mpframe(PADAPTER padapter, struct xmit_frame *pmpframe) +{ + rtw_hal_mgnt_xmit(padapter, pmpframe); +} + +static struct xmit_frame *alloc_mp_xmitframe(struct xmit_priv *pxmitpriv) +{ + struct xmit_frame *pmpframe; + struct xmit_buf *pxmitbuf; + + pmpframe = rtw_alloc_xmitframe(pxmitpriv, 0); + if (pmpframe == NULL) + return NULL; + + pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv); + if (pxmitbuf == NULL) { + rtw_free_xmitframe(pxmitpriv, pmpframe); + return NULL; + } + + pmpframe->frame_tag = MP_FRAMETAG; + + pmpframe->pxmitbuf = pxmitbuf; + + pmpframe->buf_addr = pxmitbuf->pbuf; + + pxmitbuf->priv_data = pmpframe; + + return pmpframe; + +} + +#ifdef CONFIG_PCI_HCI +static u8 check_nic_enough_desc(_adapter *padapter, struct pkt_attrib *pattrib) +{ + u32 prio; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct rtw_tx_ring *ring; + + switch (pattrib->qsel) { + case 0: + case 3: + prio = BE_QUEUE_INX; + break; + case 1: + case 2: + prio = BK_QUEUE_INX; + break; + case 4: + case 5: + prio = VI_QUEUE_INX; + break; + case 6: + case 7: + prio = VO_QUEUE_INX; + break; + default: + prio = BE_QUEUE_INX; + break; + } + + ring = &pxmitpriv->tx_ring[prio]; + + /* + * for now we reserve two free descriptor as a safety boundary + * between the tail and the head + */ + if ((ring->entries - ring->qlen) >= 2) + return _TRUE; + else + return _FALSE; +} +#endif + +static thread_return mp_xmit_packet_thread(thread_context context) +{ + struct xmit_frame *pxmitframe; + struct mp_tx *pmptx; + struct mp_priv *pmp_priv; + struct xmit_priv *pxmitpriv; + PADAPTER padapter; + + pmp_priv = (struct mp_priv *)context; + pmptx = &pmp_priv->tx; + padapter = pmp_priv->papdater; + pxmitpriv = &(padapter->xmitpriv); + + thread_enter("RTW_MP_THREAD"); + + RTW_INFO("%s:pkTx Start\n", __func__); + while (1) { + pxmitframe = alloc_mp_xmitframe(pxmitpriv); +#ifdef CONFIG_PCI_HCI + if(check_nic_enough_desc(padapter, &pmptx->attrib) == _FALSE) { + rtw_usleep_os(1000); + continue; + } +#endif + if (pxmitframe == NULL) { + if (pmptx->stop || + RTW_CANNOT_RUN(padapter)) + goto exit; + else { + rtw_usleep_os(10); + continue; + } + } + _rtw_memcpy((u8 *)(pxmitframe->buf_addr + TXDESC_OFFSET), pmptx->buf, pmptx->write_size); + _rtw_memcpy(&(pxmitframe->attrib), &(pmptx->attrib), sizeof(struct pkt_attrib)); + + + rtw_usleep_os(padapter->mppriv.pktInterval); + dump_mpframe(padapter, pxmitframe); + + pmptx->sended++; + pmp_priv->tx_pktcount++; + + if (pmptx->stop || + RTW_CANNOT_RUN(padapter)) + goto exit; + if ((pmptx->count != 0) && + (pmptx->count == pmptx->sended)) + goto exit; + + flush_signals_thread(); + } + +exit: + /* RTW_INFO("%s:pkTx Exit\n", __func__); */ + rtw_mfree(pmptx->pallocated_buf, pmptx->buf_size); + pmptx->pallocated_buf = NULL; + pmptx->stop = 1; + + thread_exit(NULL); + return 0; +} + +void fill_txdesc_for_mp(PADAPTER padapter, u8 *ptxdesc) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + _rtw_memcpy(ptxdesc, pmp_priv->tx.desc, TXDESC_SIZE); +} + +#if defined(CONFIG_RTL8188E) +void fill_tx_desc_8188e(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct tx_desc *desc = (struct tx_desc *)&(pmp_priv->tx.desc); + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u32 pkt_size = pattrib->last_txcmdsz; + s32 bmcast = IS_MCAST(pattrib->ra); + /* offset 0 */ +#if !defined(CONFIG_RTL8188E_SDIO) && !defined(CONFIG_PCI_HCI) + desc->txdw0 |= cpu_to_le32(OWN | FSG | LSG); + desc->txdw0 |= cpu_to_le32(pkt_size & 0x0000FFFF); /* packet size */ + desc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00FF0000); /* 32 bytes for TX Desc */ + if (bmcast) + desc->txdw0 |= cpu_to_le32(BMC); /* broadcast packet */ + + desc->txdw1 |= cpu_to_le32((0x01 << 26) & 0xff000000); +#endif + + desc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x3F); /* CAM_ID(MAC_ID) */ + desc->txdw1 |= cpu_to_le32((pattrib->qsel << QSEL_SHT) & 0x00001F00); /* Queue Select, TID */ + desc->txdw1 |= cpu_to_le32((pattrib->raid << RATE_ID_SHT) & 0x000F0000); /* Rate Adaptive ID */ + /* offset 8 */ + /* desc->txdw2 |= cpu_to_le32(AGG_BK); */ /* AGG BK */ + + desc->txdw3 |= cpu_to_le32((pattrib->seqnum << 16) & 0x0fff0000); + desc->txdw4 |= cpu_to_le32(HW_SSN); + + desc->txdw4 |= cpu_to_le32(USERATE); + desc->txdw4 |= cpu_to_le32(DISDATAFB); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + desc->txdw4 |= cpu_to_le32(DATA_SHORT); /* CCK Short Preamble */ + } + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + desc->txdw4 |= cpu_to_le32(DATA_BW); + + /* offset 20 */ + desc->txdw5 |= cpu_to_le32(pmp_priv->rateidx & 0x0000001F); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) > MPT_RATE_54M) + desc->txdw5 |= cpu_to_le32(SGI); /* MCS Short Guard Interval */ + } + + desc->txdw5 |= cpu_to_le32(RTY_LMT_EN); /* retry limit enable */ + desc->txdw5 |= cpu_to_le32(0x00180000); /* DATA/RTS Rate Fallback Limit */ + + +} +#endif + +#if defined(CONFIG_RTL8814A) +void fill_tx_desc_8814a(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + u8 *pDesc = (u8 *)&(pmp_priv->tx.desc); + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + + u32 pkt_size = pattrib->last_txcmdsz; + s32 bmcast = IS_MCAST(pattrib->ra); + u8 offset; + + /* SET_TX_DESC_FIRST_SEG_8814A(pDesc, 1); */ + SET_TX_DESC_LAST_SEG_8814A(pDesc, 1); + /* SET_TX_DESC_OWN_(pDesc, 1); */ + + SET_TX_DESC_PKT_SIZE_8814A(pDesc, pkt_size); + + offset = TXDESC_SIZE + OFFSET_SZ; + + SET_TX_DESC_OFFSET_8814A(pDesc, offset); +#if defined(CONFIG_PCI_HCI) + SET_TX_DESC_PKT_OFFSET_8814A(pDesc, 0); /* 8814AE pkt_offset is 0 */ +#else + SET_TX_DESC_PKT_OFFSET_8814A(pDesc, 1); +#endif + + if (bmcast) + SET_TX_DESC_BMC_8814A(pDesc, 1); + + SET_TX_DESC_MACID_8814A(pDesc, pattrib->mac_id); + SET_TX_DESC_RATE_ID_8814A(pDesc, pattrib->raid); + + /* SET_TX_DESC_RATE_ID_8812(pDesc, RATEID_IDX_G); */ + SET_TX_DESC_QUEUE_SEL_8814A(pDesc, pattrib->qsel); + /* SET_TX_DESC_QUEUE_SEL_8812(pDesc, QSLT_MGNT); */ + + if (pmp_priv->preamble) + SET_TX_DESC_DATA_SHORT_8814A(pDesc, 1); + + if (!pattrib->qos_en) { + SET_TX_DESC_HWSEQ_EN_8814A(pDesc, 1); /* Hw set sequence number */ + } else + SET_TX_DESC_SEQ_8814A(pDesc, pattrib->seqnum); + + if (pmp_priv->bandwidth <= CHANNEL_WIDTH_160) + SET_TX_DESC_DATA_BW_8814A(pDesc, pmp_priv->bandwidth); + else { + RTW_INFO("%s:Err: unknown bandwidth %d, use 20M\n", __func__, pmp_priv->bandwidth); + SET_TX_DESC_DATA_BW_8814A(pDesc, CHANNEL_WIDTH_20); + } + + SET_TX_DESC_DISABLE_FB_8814A(pDesc, 1); + SET_TX_DESC_USE_RATE_8814A(pDesc, 1); + SET_TX_DESC_TX_RATE_8814A(pDesc, pmp_priv->rateidx); + +} +#endif + +#if defined(CONFIG_RTL8812A) || defined(CONFIG_RTL8821A) +void fill_tx_desc_8812a(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + u8 *pDesc = (u8 *)&(pmp_priv->tx.desc); + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + + u32 pkt_size = pattrib->last_txcmdsz; + s32 bmcast = IS_MCAST(pattrib->ra); + u8 data_rate, pwr_status, offset; + + SET_TX_DESC_FIRST_SEG_8812(pDesc, 1); + SET_TX_DESC_LAST_SEG_8812(pDesc, 1); + SET_TX_DESC_OWN_8812(pDesc, 1); + + SET_TX_DESC_PKT_SIZE_8812(pDesc, pkt_size); + + offset = TXDESC_SIZE + OFFSET_SZ; + + SET_TX_DESC_OFFSET_8812(pDesc, offset); + +#if defined(CONFIG_PCI_HCI) + SET_TX_DESC_PKT_OFFSET_8812(pDesc, 0); +#else + SET_TX_DESC_PKT_OFFSET_8812(pDesc, 1); +#endif + if (bmcast) + SET_TX_DESC_BMC_8812(pDesc, 1); + + SET_TX_DESC_MACID_8812(pDesc, pattrib->mac_id); + SET_TX_DESC_RATE_ID_8812(pDesc, pattrib->raid); + + /* SET_TX_DESC_RATE_ID_8812(pDesc, RATEID_IDX_G); */ + SET_TX_DESC_QUEUE_SEL_8812(pDesc, pattrib->qsel); + /* SET_TX_DESC_QUEUE_SEL_8812(pDesc, QSLT_MGNT); */ + + if (!pattrib->qos_en) { + SET_TX_DESC_HWSEQ_EN_8812(pDesc, 1); /* Hw set sequence number */ + } else + SET_TX_DESC_SEQ_8812(pDesc, pattrib->seqnum); + + if (pmp_priv->bandwidth <= CHANNEL_WIDTH_160) + SET_TX_DESC_DATA_BW_8812(pDesc, pmp_priv->bandwidth); + else { + RTW_INFO("%s:Err: unknown bandwidth %d, use 20M\n", __func__, pmp_priv->bandwidth); + SET_TX_DESC_DATA_BW_8812(pDesc, CHANNEL_WIDTH_20); + } + + SET_TX_DESC_DISABLE_FB_8812(pDesc, 1); + SET_TX_DESC_USE_RATE_8812(pDesc, 1); + SET_TX_DESC_TX_RATE_8812(pDesc, pmp_priv->rateidx); + +} +#endif +#if defined(CONFIG_RTL8192E) +void fill_tx_desc_8192e(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + u8 *pDesc = (u8 *)&(pmp_priv->tx.desc); + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + + u32 pkt_size = pattrib->last_txcmdsz; + s32 bmcast = IS_MCAST(pattrib->ra); + u8 data_rate, pwr_status, offset; + + + SET_TX_DESC_PKT_SIZE_92E(pDesc, pkt_size); + + offset = TXDESC_SIZE + OFFSET_SZ; + + SET_TX_DESC_OFFSET_92E(pDesc, offset); +#if defined(CONFIG_PCI_HCI) /* 8192EE */ + + SET_TX_DESC_PKT_OFFSET_92E(pDesc, 0); /* 8192EE pkt_offset is 0 */ +#else /* 8192EU 8192ES */ + SET_TX_DESC_PKT_OFFSET_92E(pDesc, 1); +#endif + + if (bmcast) + SET_TX_DESC_BMC_92E(pDesc, 1); + + SET_TX_DESC_MACID_92E(pDesc, pattrib->mac_id); + SET_TX_DESC_RATE_ID_92E(pDesc, pattrib->raid); + + + SET_TX_DESC_QUEUE_SEL_92E(pDesc, pattrib->qsel); + /* SET_TX_DESC_QUEUE_SEL_8812(pDesc, QSLT_MGNT); */ + + if (!pattrib->qos_en) { + SET_TX_DESC_EN_HWSEQ_92E(pDesc, 1);/* Hw set sequence number */ + SET_TX_DESC_HWSEQ_SEL_92E(pDesc, pattrib->hw_ssn_sel); + } else + SET_TX_DESC_SEQ_92E(pDesc, pattrib->seqnum); + + if ((pmp_priv->bandwidth == CHANNEL_WIDTH_20) || (pmp_priv->bandwidth == CHANNEL_WIDTH_40)) + SET_TX_DESC_DATA_BW_92E(pDesc, pmp_priv->bandwidth); + else { + RTW_INFO("%s:Err: unknown bandwidth %d, use 20M\n", __func__, pmp_priv->bandwidth); + SET_TX_DESC_DATA_BW_92E(pDesc, CHANNEL_WIDTH_20); + } + + /* SET_TX_DESC_DATA_SC_92E(pDesc, SCMapping_92E(padapter,pattrib)); */ + + SET_TX_DESC_DISABLE_FB_92E(pDesc, 1); + SET_TX_DESC_USE_RATE_92E(pDesc, 1); + SET_TX_DESC_TX_RATE_92E(pDesc, pmp_priv->rateidx); + +} +#endif + +#if defined(CONFIG_RTL8723B) +void fill_tx_desc_8723b(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_AGG_BREAK_8723B(ptxdesc, 1); + SET_TX_DESC_MACID_8723B(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8723B(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8723B(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8723B(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8723B(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8723B(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8723B(ptxdesc, 1); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8723B(ptxdesc, 1); + } + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8723B(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8723B(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8723B(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8723B(ptxdesc, 0xF); +} +#endif + +#if defined(CONFIG_RTL8703B) +void fill_tx_desc_8703b(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_AGG_BREAK_8703B(ptxdesc, 1); + SET_TX_DESC_MACID_8703B(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8703B(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8703B(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8703B(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8703B(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8703B(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8703B(ptxdesc, 1); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8703B(ptxdesc, 1); + } + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8703B(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8703B(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8703B(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8703B(ptxdesc, 0xF); +} +#endif + +#if defined(CONFIG_RTL8188F) +void fill_tx_desc_8188f(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_AGG_BREAK_8188F(ptxdesc, 1); + SET_TX_DESC_MACID_8188F(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8188F(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8188F(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8188F(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8188F(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8188F(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8188F(ptxdesc, 1); + + if (pmp_priv->preamble) + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8188F(ptxdesc, 1); + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8188F(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8188F(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8188F(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8188F(ptxdesc, 0xF); +} +#endif + +#if defined(CONFIG_RTL8188GTV) +void fill_tx_desc_8188gtv(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_AGG_BREAK_8188GTV(ptxdesc, 1); + SET_TX_DESC_MACID_8188GTV(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8188GTV(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8188GTV(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8188GTV(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8188GTV(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8188GTV(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8188GTV(ptxdesc, 1); + + if (pmp_priv->preamble) + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8188GTV(ptxdesc, 1); + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8188GTV(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8188GTV(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8188GTV(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8188GTV(ptxdesc, 0xF); +} +#endif + +#if defined(CONFIG_RTL8723D) +void fill_tx_desc_8723d(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_BK_8723D(ptxdesc, 1); + SET_TX_DESC_MACID_8723D(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8723D(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8723D(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8723D(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8723D(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8723D(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8723D(ptxdesc, 1); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8723D(ptxdesc, 1); + } + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8723D(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8723D(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8723D(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8723D(ptxdesc, 0xF); +} +#endif + +#if defined(CONFIG_RTL8710B) +void fill_tx_desc_8710b(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_BK_8710B(ptxdesc, 1); + SET_TX_DESC_MACID_8710B(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8710B(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8710B(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8710B(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8710B(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8710B(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8710B(ptxdesc, 1); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8710B(ptxdesc, 1); + } + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8710B(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8710B(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8710B(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8710B(ptxdesc, 0xF); +} +#endif + +#if defined(CONFIG_RTL8192F) +void fill_tx_desc_8192f(PADAPTER padapter) +{ + struct mp_priv *pmp_priv = &padapter->mppriv; + struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib); + u8 *ptxdesc = pmp_priv->tx.desc; + + SET_TX_DESC_BK_8192F(ptxdesc, 1); + SET_TX_DESC_MACID_8192F(ptxdesc, pattrib->mac_id); + SET_TX_DESC_QUEUE_SEL_8192F(ptxdesc, pattrib->qsel); + + SET_TX_DESC_RATE_ID_8192F(ptxdesc, pattrib->raid); + SET_TX_DESC_SEQ_8192F(ptxdesc, pattrib->seqnum); + SET_TX_DESC_HWSEQ_EN_8192F(ptxdesc, 1); + SET_TX_DESC_USE_RATE_8192F(ptxdesc, 1); + SET_TX_DESC_DISABLE_FB_8192F(ptxdesc, 1); + + if (pmp_priv->preamble) { + if (HwRateToMPTRate(pmp_priv->rateidx) <= MPT_RATE_54M) + SET_TX_DESC_DATA_SHORT_8192F(ptxdesc, 1); + } + + if (pmp_priv->bandwidth == CHANNEL_WIDTH_40) + SET_TX_DESC_DATA_BW_8192F(ptxdesc, 1); + + SET_TX_DESC_TX_RATE_8192F(ptxdesc, pmp_priv->rateidx); + + SET_TX_DESC_DATA_RATE_FB_LIMIT_8192F(ptxdesc, 0x1F); + SET_TX_DESC_RTS_RATE_FB_LIMIT_8192F(ptxdesc, 0xF); +} + +#endif +static void Rtw_MPSetMacTxEDCA(PADAPTER padapter) +{ + + rtw_write32(padapter, 0x508 , 0x00a422); /* Disable EDCA BE Txop for MP pkt tx adjust Packet interval */ + /* RTW_INFO("%s:write 0x508~~~~~~ 0x%x\n", __func__,rtw_read32(padapter, 0x508)); */ + phy_set_mac_reg(padapter, 0x458 , bMaskDWord , 0x0); + /*RTW_INFO("%s()!!!!! 0x460 = 0x%x\n" ,__func__, phy_query_bb_reg(padapter, 0x460, bMaskDWord));*/ + phy_set_mac_reg(padapter, 0x460 , bMaskLWord , 0x0); /* fast EDCA queue packet interval & time out value*/ + /*phy_set_mac_reg(padapter, ODM_EDCA_VO_PARAM ,bMaskLWord , 0x431C);*/ + /*phy_set_mac_reg(padapter, ODM_EDCA_BE_PARAM ,bMaskLWord , 0x431C);*/ + /*phy_set_mac_reg(padapter, ODM_EDCA_BK_PARAM ,bMaskLWord , 0x431C);*/ + RTW_INFO("%s()!!!!! 0x460 = 0x%x\n" , __func__, phy_query_bb_reg(padapter, 0x460, bMaskDWord)); + +} + +void SetPacketTx(PADAPTER padapter) +{ + u8 *ptr, *pkt_start, *pkt_end; + u32 pkt_size = 0, i = 0, idx = 0, tmp_idx = 0; + struct rtw_ieee80211_hdr *hdr; + u8 payload; + s32 bmcast; + struct pkt_attrib *pattrib; + struct mp_priv *pmp_priv; + + pmp_priv = &padapter->mppriv; + + if (pmp_priv->tx.stop) + return; + pmp_priv->tx.sended = 0; + pmp_priv->tx.stop = 0; + pmp_priv->tx_pktcount = 0; + + /* 3 1. update_attrib() */ + pattrib = &pmp_priv->tx.attrib; + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + bmcast = IS_MCAST(pattrib->ra); + if (bmcast) + pattrib->psta = rtw_get_bcmc_stainfo(padapter); + else + pattrib->psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv)); + + if (pattrib->psta == NULL) { + RTW_INFO("%s:psta = NULL !!\n", __func__); + return; + } + + pattrib->mac_id = pattrib->psta->cmn.mac_id; + pattrib->mbssid = 0; + + pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->pktlen; + + /* 3 2. allocate xmit buffer */ + pkt_size = pattrib->last_txcmdsz; + + if (pmp_priv->tx.pallocated_buf) + rtw_mfree(pmp_priv->tx.pallocated_buf, pmp_priv->tx.buf_size); + pmp_priv->tx.write_size = pkt_size; + pmp_priv->tx.buf_size = pkt_size + XMITBUF_ALIGN_SZ; + pmp_priv->tx.pallocated_buf = rtw_zmalloc(pmp_priv->tx.buf_size); + if (pmp_priv->tx.pallocated_buf == NULL) { + RTW_INFO("%s: malloc(%d) fail!!\n", __func__, pmp_priv->tx.buf_size); + return; + } + pmp_priv->tx.buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pmp_priv->tx.pallocated_buf), XMITBUF_ALIGN_SZ); + ptr = pmp_priv->tx.buf; + + _rtw_memset(pmp_priv->tx.desc, 0, TXDESC_SIZE); + pkt_start = ptr; + pkt_end = pkt_start + pkt_size; + + /* 3 3. init TX descriptor */ +#if defined(CONFIG_RTL8188E) + if (IS_HARDWARE_TYPE_8188E(padapter)) + fill_tx_desc_8188e(padapter); +#endif + +#if defined(CONFIG_RTL8814A) + if (IS_HARDWARE_TYPE_8814A(padapter)) + fill_tx_desc_8814a(padapter); +#endif /* defined(CONFIG_RTL8814A) */ + +#if defined(CONFIG_RTL8822B) + if (IS_HARDWARE_TYPE_8822B(padapter)) + rtl8822b_prepare_mp_txdesc(padapter, pmp_priv); +#endif /* CONFIG_RTL8822B */ + +#if defined(CONFIG_RTL8822C) + if (IS_HARDWARE_TYPE_8822C(padapter)) + rtl8822c_prepare_mp_txdesc(padapter, pmp_priv); +#endif /* CONFIG_RTL8822C */ + +#if defined(CONFIG_RTL8821C) + if (IS_HARDWARE_TYPE_8821C(padapter)) + rtl8821c_prepare_mp_txdesc(padapter, pmp_priv); +#endif /* CONFIG_RTL8821C */ + +#if defined(CONFIG_RTL8812A) || defined(CONFIG_RTL8821A) + if (IS_HARDWARE_TYPE_8812(padapter) || IS_HARDWARE_TYPE_8821(padapter)) + fill_tx_desc_8812a(padapter); +#endif + +#if defined(CONFIG_RTL8192E) + if (IS_HARDWARE_TYPE_8192E(padapter)) + fill_tx_desc_8192e(padapter); +#endif +#if defined(CONFIG_RTL8723B) + if (IS_HARDWARE_TYPE_8723B(padapter)) + fill_tx_desc_8723b(padapter); +#endif +#if defined(CONFIG_RTL8703B) + if (IS_HARDWARE_TYPE_8703B(padapter)) + fill_tx_desc_8703b(padapter); +#endif + +#if defined(CONFIG_RTL8188F) + if (IS_HARDWARE_TYPE_8188F(padapter)) + fill_tx_desc_8188f(padapter); +#endif + +#if defined(CONFIG_RTL8188GTV) + if (IS_HARDWARE_TYPE_8188GTV(padapter)) + fill_tx_desc_8188gtv(padapter); +#endif + +#if defined(CONFIG_RTL8723D) + if (IS_HARDWARE_TYPE_8723D(padapter)) + fill_tx_desc_8723d(padapter); +#endif +#if defined(CONFIG_RTL8192F) + if (IS_HARDWARE_TYPE_8192F(padapter)) + fill_tx_desc_8192f(padapter); +#endif + +#if defined(CONFIG_RTL8710B) + if (IS_HARDWARE_TYPE_8710B(padapter)) + fill_tx_desc_8710b(padapter); +#endif + +#if defined(CONFIG_RTL8814B) + if (IS_HARDWARE_TYPE_8814B(padapter)) + rtl8814b_prepare_mp_txdesc(padapter, pmp_priv); +#endif /* CONFIG_RTL8814B */ + +#if defined(CONFIG_RTL8723F) + if (IS_HARDWARE_TYPE_8723F(padapter)) + rtl8723f_prepare_mp_txdesc(padapter, pmp_priv); +#endif /* CONFIG_RTL8723F */ + + /* 3 4. make wlan header, make_wlanhdr() */ + hdr = (struct rtw_ieee80211_hdr *)pkt_start; + set_frame_sub_type(&hdr->frame_ctl, pattrib->subtype); + + _rtw_memcpy(hdr->addr1, pattrib->dst, ETH_ALEN); /* DA */ + _rtw_memcpy(hdr->addr2, pattrib->src, ETH_ALEN); /* SA */ + _rtw_memcpy(hdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN); /* RA, BSSID */ + + /* 3 5. make payload */ + ptr = pkt_start + pattrib->hdrlen; + + if (pmp_priv->mplink_btx == _TRUE) { + _rtw_memcpy(ptr, pmp_priv->mplink_buf, pkt_end - ptr); + } else { + switch (pmp_priv->tx.payload) { + case MP_TX_Payload_00: + RTW_INFO("MP packet tx 0x00 payload!\n"); + payload = 0x00; + _rtw_memset(ptr, 0x00, pkt_end - ptr); + break; + case MP_TX_Payload_5a: + RTW_INFO("MP packet tx 0x5a payload!\n"); + payload = 0x5a; + _rtw_memset(ptr, 0x5a, pkt_end - ptr); + break; + case MP_TX_Payload_a5: + RTW_INFO("MP packet tx 0xa5 payload!\n"); + payload = 0xa5; + _rtw_memset(ptr, 0xa5, pkt_end - ptr); + break; + case MP_TX_Payload_ff: + RTW_INFO("MP packet tx 0xff payload!\n"); + payload = 0xff; + _rtw_memset(ptr, 0xff, pkt_end - ptr); + break; + case MP_TX_Payload_prbs9: + RTW_INFO("MP packet tx PRBS9 payload!\n"); + while (idx <= pkt_end - ptr) { + int start = 0x02; + int a = start; + + for (i = 0;; i++) { + int newbit = (((a >> 8) ^ (a >> 4)) & 1); + a = ((a << 1) | newbit) & 0x1ff; + RTW_DBG("%x ", a); + ptr[idx + i] = a; + + if (a == start) { + RTW_INFO("payload repetition period is %d , end %d\n", i , idx); + tmp_idx += i; + break; + } + if (idx + i >= (pkt_end - ptr)) { + tmp_idx += (idx + i); + RTW_INFO(" repetition period payload end curr ptr %d\n", idx + i); + break; + } + } + idx = tmp_idx; + } + break; + case MP_TX_Payload_default_random: + RTW_INFO("MP packet tx default random payload!\n"); + for (i = 0; i < pkt_end - ptr; i++) + ptr[i] = rtw_random32() % 0xFF; + break; + default: + RTW_INFO("Config payload type default use 0x%x\n!", pmp_priv->tx.payload); + _rtw_memset(ptr, pmp_priv->tx.payload, pkt_end - ptr); + break; + } + } + /* 3 6. start thread */ +#ifdef PLATFORM_LINUX + pmp_priv->tx.PktTxThread = kthread_run(mp_xmit_packet_thread, pmp_priv, "RTW_MP_THREAD"); + if (IS_ERR(pmp_priv->tx.PktTxThread)) { + RTW_ERR("Create PktTx Thread Fail !!!!!\n"); + pmp_priv->tx.PktTxThread = NULL; + } +#endif +#ifdef PLATFORM_FREEBSD + { + struct proc *p; + struct thread *td; + pmp_priv->tx.PktTxThread = kproc_kthread_add(mp_xmit_packet_thread, pmp_priv, + &p, &td, RFHIGHPID, 0, "MPXmitThread", "MPXmitThread"); + + if (pmp_priv->tx.PktTxThread < 0) + RTW_INFO("Create PktTx Thread Fail !!!!!\n"); + } +#endif + + Rtw_MPSetMacTxEDCA(padapter); + return; +} + +void SetPacketRx(PADAPTER pAdapter, u8 bStartRx, u8 bAB) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter); + struct mp_priv *pmppriv = &pAdapter->mppriv; + + + if (bStartRx) { +#ifdef CONFIG_RTL8723B + phy_set_mac_reg(pAdapter, 0xe70, BIT23 | BIT22, 0x3); /* Power on adc (in RX_WAIT_CCA state) */ + write_bbreg(pAdapter, 0xa01, BIT0, bDisable);/* improve Rx performance by jerry */ +#endif + pHalData->ReceiveConfig = RCR_AAP | RCR_APM | RCR_AM | RCR_AMF | RCR_HTC_LOC_CTRL; + pHalData->ReceiveConfig |= RCR_ACRC32; + pHalData->ReceiveConfig |= RCR_APP_PHYST_RXFF | RCR_APP_ICV | RCR_APP_MIC; + + if (pmppriv->bSetRxBssid == _TRUE) { + RTW_INFO("%s: pmppriv->network_macaddr=" MAC_FMT "\n", __func__, + MAC_ARG(pmppriv->network_macaddr)); + pHalData->ReceiveConfig = 0; + pHalData->ReceiveConfig |= RCR_CBSSID_DATA | RCR_CBSSID_BCN |RCR_APM | RCR_AM | RCR_AB |RCR_AMF; + pHalData->ReceiveConfig |= RCR_APP_PHYST_RXFF; + +#if defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) +/* todo: 8723F */ + write_bbreg(pAdapter, 0x550, BIT3, bEnable); +#endif + rtw_write16(pAdapter, REG_RXFLTMAP0, 0xFFEF); /* REG_RXFLTMAP0 (RX Filter Map Group 0) */ + pmppriv->brx_filter_beacon = _TRUE; + + } else { + pHalData->ReceiveConfig |= RCR_ADF; + /* Accept all data frames */ + rtw_write16(pAdapter, REG_RXFLTMAP2, 0xFFFF); + } + + if (bAB) + pHalData->ReceiveConfig |= RCR_AB; + } else { +#ifdef CONFIG_RTL8723B + phy_set_mac_reg(pAdapter, 0xe70, BIT23 | BIT22, 0x00); /* Power off adc (in RX_WAIT_CCA state)*/ + write_bbreg(pAdapter, 0xa01, BIT0, bEnable);/* improve Rx performance by jerry */ +#endif + pHalData->ReceiveConfig = 0; + rtw_write16(pAdapter, REG_RXFLTMAP0, 0xFFFF); /* REG_RXFLTMAP0 (RX Filter Map Group 0) */ + } + + rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig); +} + +void ResetPhyRxPktCount(PADAPTER pAdapter) +{ + u32 i, phyrx_set = 0; + + for (i = 0; i <= 0xF; i++) { + phyrx_set = 0; + phyrx_set |= _RXERR_RPT_SEL(i); /* select */ + phyrx_set |= RXERR_RPT_RST; /* set counter to zero */ + rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set); + } +} + +static u32 GetPhyRxPktCounts(PADAPTER pAdapter, u32 selbit) +{ + /* selection */ + u32 phyrx_set = 0, count = 0; + + phyrx_set = _RXERR_RPT_SEL(selbit & 0xF); + rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set); + + /* Read packet count */ + count = rtw_read32(pAdapter, REG_RXERR_RPT) & RXERR_COUNTER_MASK; + + return count; +} + +u32 GetPhyRxPktReceived(PADAPTER pAdapter) +{ + u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0; + + OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_OK); + CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_OK); + HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_OK); + + return OFDM_cnt + CCK_cnt + HT_cnt; +} + +u32 GetPhyRxPktCRC32Error(PADAPTER pAdapter) +{ + u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0; + + OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_FAIL); + CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_FAIL); + HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_FAIL); + + return OFDM_cnt + CCK_cnt + HT_cnt; +} + +struct psd_init_regs { + /* 3 wire */ + int reg_88c; + int reg_c00; + int reg_e00; + int reg_1800; + int reg_1a00; + /* cck */ + int reg_800; + int reg_808; +}; + +static int rtw_mp_psd_init(PADAPTER padapter, struct psd_init_regs *regs) +{ + HAL_DATA_TYPE *phal_data = GET_HAL_DATA(padapter); + + switch (phal_data->rf_type) { + /* 1R */ + case RF_1T1R: + if (hal_chk_proto_cap(padapter, PROTO_CAP_11AC)) { + /* 11AC 1R PSD Setting 3wire & cck off */ + regs->reg_c00 = rtw_read32(padapter, 0xC00); + phy_set_bb_reg(padapter, 0xC00, 0x3, 0x00); + regs->reg_808 = rtw_read32(padapter, 0x808); + phy_set_bb_reg(padapter, 0x808, 0x10000000, 0x0); + } else { + /* 11N 3-wire off 1 */ + regs->reg_88c = rtw_read32(padapter, 0x88C); + phy_set_bb_reg(padapter, 0x88C, 0x300000, 0x3); + /* 11N CCK off */ + regs->reg_800 = rtw_read32(padapter, 0x800); + phy_set_bb_reg(padapter, 0x800, 0x1000000, 0x0); + } + break; + + /* 2R */ + case RF_1T2R: + case RF_2T2R: + if (hal_chk_proto_cap(padapter, PROTO_CAP_11AC)) { + /* 11AC 2R PSD Setting 3wire & cck off */ + regs->reg_c00 = rtw_read32(padapter, 0xC00); + regs->reg_e00 = rtw_read32(padapter, 0xE00); + phy_set_bb_reg(padapter, 0xC00, 0x3, 0x00); + phy_set_bb_reg(padapter, 0xE00, 0x3, 0x00); + regs->reg_808 = rtw_read32(padapter, 0x808); + phy_set_bb_reg(padapter, 0x808, 0x10000000, 0x0); + } else { + /* 11N 3-wire off 2 */ + regs->reg_88c = rtw_read32(padapter, 0x88C); + phy_set_bb_reg(padapter, 0x88C, 0xF00000, 0xF); + /* 11N CCK off */ + regs->reg_800 = rtw_read32(padapter, 0x800); + phy_set_bb_reg(padapter, 0x800, 0x1000000, 0x0); + } + break; + + /* 3R */ + case RF_2T3R: + case RF_3T3R: + if (hal_chk_proto_cap(padapter, PROTO_CAP_11AC)) { + /* 11AC 3R PSD Setting 3wire & cck off */ + regs->reg_c00 = rtw_read32(padapter, 0xC00); + regs->reg_e00 = rtw_read32(padapter, 0xE00); + regs->reg_1800 = rtw_read32(padapter, 0x1800); + phy_set_bb_reg(padapter, 0xC00, 0x3, 0x00); + phy_set_bb_reg(padapter, 0xE00, 0x3, 0x00); + phy_set_bb_reg(padapter, 0x1800, 0x3, 0x00); + regs->reg_808 = rtw_read32(padapter, 0x808); + phy_set_bb_reg(padapter, 0x808, 0x10000000, 0x0); + } else { + RTW_ERR("%s: 11n don't support 3R\n", __func__); + return -1; + } + break; + + /* 4R */ + case RF_2T4R: + case RF_3T4R: + case RF_4T4R: + if (hal_chk_proto_cap(padapter, PROTO_CAP_11AC)) { + /* 11AC 4R PSD Setting 3wire & cck off */ + regs->reg_c00 = rtw_read32(padapter, 0xC00); + regs->reg_e00 = rtw_read32(padapter, 0xE00); + regs->reg_1800 = rtw_read32(padapter, 0x1800); + regs->reg_1a00 = rtw_read32(padapter, 0x1A00); + phy_set_bb_reg(padapter, 0xC00, 0x3, 0x00); + phy_set_bb_reg(padapter, 0xE00, 0x3, 0x00); + phy_set_bb_reg(padapter, 0x1800, 0x3, 0x00); + phy_set_bb_reg(padapter, 0x1A00, 0x3, 0x00); + regs->reg_808 = rtw_read32(padapter, 0x808); + phy_set_bb_reg(padapter, 0x808, 0x10000000, 0x0); + } else { + RTW_ERR("%s: 11n don't support 4R\n", __func__); + return -1; + } + break; + + default: + RTW_ERR("%s: unknown %d rf type\n", __func__, phal_data->rf_type); + return -1; + } + + /* Set PSD points, 0=128, 1=256, 2=512, 3=1024 */ + if (hal_chk_proto_cap(padapter, PROTO_CAP_11AC)) + phy_set_bb_reg(padapter, 0x910, 0xC000, 3); + else + phy_set_bb_reg(padapter, 0x808, 0xC000, 3); + + RTW_INFO("%s: set %d rf type done\n", __func__, phal_data->rf_type); + return 0; +} + +static int rtw_mp_psd_close(PADAPTER padapter, struct psd_init_regs *regs) +{ + HAL_DATA_TYPE *phal_data = GET_HAL_DATA(padapter); + + + if (!hal_chk_proto_cap(padapter, PROTO_CAP_11AC)) { + /* 11n 3wire restore */ + rtw_write32(padapter, 0x88C, regs->reg_88c); + /* 11n cck restore */ + rtw_write32(padapter, 0x800, regs->reg_800); + RTW_INFO("%s: restore %d rf type\n", __func__, phal_data->rf_type); + return 0; + } + + /* 11ac 3wire restore */ + switch (phal_data->rf_type) { + case RF_1T1R: + rtw_write32(padapter, 0xC00, regs->reg_c00); + break; + case RF_1T2R: + case RF_2T2R: + rtw_write32(padapter, 0xC00, regs->reg_c00); + rtw_write32(padapter, 0xE00, regs->reg_e00); + break; + case RF_2T3R: + case RF_3T3R: + rtw_write32(padapter, 0xC00, regs->reg_c00); + rtw_write32(padapter, 0xE00, regs->reg_e00); + rtw_write32(padapter, 0x1800, regs->reg_1800); + break; + case RF_2T4R: + case RF_3T4R: + case RF_4T4R: + rtw_write32(padapter, 0xC00, regs->reg_c00); + rtw_write32(padapter, 0xE00, regs->reg_e00); + rtw_write32(padapter, 0x1800, regs->reg_1800); + rtw_write32(padapter, 0x1A00, regs->reg_1a00); + break; + default: + RTW_WARN("%s: unknown %d rf type\n", __func__, phal_data->rf_type); + break; + } + + /* 11ac cck restore */ + rtw_write32(padapter, 0x808, regs->reg_808); + RTW_INFO("%s: restore %d rf type done\n", __func__, phal_data->rf_type); + return 0; +} + +/* reg 0x808[9:0]: FFT data x + * reg 0x808[22]: 0 --> 1 to get 1 FFT data y + * reg 0x8B4[15:0]: FFT data y report */ +static u32 rtw_GetPSDData(PADAPTER pAdapter, u32 point) +{ + u32 psd_val = 0; + +#if defined(CONFIG_RTL8812A) || defined(CONFIG_RTL8821A) || defined(CONFIG_RTL8814A) \ + || defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) + + u16 psd_reg = 0x910; + u16 psd_regL = 0xF44; +#else + u16 psd_reg = 0x808; + u16 psd_regL = 0x8B4; +#endif + + psd_val = rtw_read32(pAdapter, psd_reg); + + psd_val &= 0xFFBFFC00; + psd_val |= point; + + rtw_write32(pAdapter, psd_reg, psd_val); + rtw_mdelay_os(1); + psd_val |= 0x00400000; + + rtw_write32(pAdapter, psd_reg, psd_val); + rtw_mdelay_os(1); + + psd_val = rtw_read32(pAdapter, psd_regL); +#if defined(CONFIG_RTL8821C) + psd_val = (psd_val & 0x00FFFFFF) / 32; +#else + psd_val &= 0x0000FFFF; +#endif + + return psd_val; +} + +/* + * pts start_point_min stop_point_max + * 128 64 64 + 128 = 192 + * 256 128 128 + 256 = 384 + * 512 256 256 + 512 = 768 + * 1024 512 512 + 1024 = 1536 + * + */ +u32 mp_query_psd(PADAPTER pAdapter, u8 *data) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(pAdapter); + struct dm_struct *p_dm = adapter_to_phydm(pAdapter); + + u32 i, psd_pts = 0, psd_start = 0, psd_stop = 0; + u32 psd_data = 0; + struct psd_init_regs regs = {}; + int psd_analysis = 0; + char *pdata = NULL; + + +#ifdef PLATFORM_LINUX + if (!netif_running(pAdapter->pnetdev)) { + return 0; + } +#endif + + if (check_fwstate(&pAdapter->mlmepriv, WIFI_MP_STATE) == _FALSE) { + return 0; + } + + if (strlen(data) == 0) { /* default value */ + psd_pts = 128; + psd_start = 64; + psd_stop = 128; + } else if (strncmp(data, "analysis,", 9) == 0) { + if (rtw_mp_psd_init(pAdapter, ®s) != 0) + return 0; + psd_analysis = 1; + sscanf(data + 9, "pts=%d,start=%d,stop=%d", &psd_pts, &psd_start, &psd_stop); + } else + sscanf(data, "pts=%d,start=%d,stop=%d", &psd_pts, &psd_start, &psd_stop); + + data[0] = '\0'; + pdata = data; + + if (psd_stop > 1920 || psd_stop < 1) { + rtw_warn_on(1); + psd_stop = 1920; + } + + if (IS_HARDWARE_TYPE_8822C(pAdapter) || IS_HARDWARE_TYPE_8723F(pAdapter)) { + u32 *psdbuf = rtw_zmalloc(sizeof(u32)*256); + + if (psdbuf == NULL) { + RTW_INFO("%s: psd buf malloc fail!!\n", __func__); + return 0; + } + + halrf_cmn_info_set(p_dm, HALRF_CMNINFO_MP_PSD_POINT, psd_pts); + halrf_cmn_info_set(p_dm, HALRF_CMNINFO_MP_PSD_START_POINT, psd_start); + halrf_cmn_info_set(p_dm, HALRF_CMNINFO_MP_PSD_STOP_POINT, psd_stop); + halrf_cmn_info_set(p_dm, HALRF_CMNINFO_MP_PSD_AVERAGE, 0x20000); + + halrf_psd_init(p_dm); +#ifdef CONFIG_LONG_DELAY_ISSUE + rtw_msleep_os(100); +#else + rtw_mdelay_os(10); +#endif + halrf_psd_query(p_dm, psdbuf, 256); + + i = 0; + while (i < 256) { + pdata += sprintf(pdata, "%x ", (psdbuf[i])); + i++; + } + + if (psdbuf) + rtw_mfree(psdbuf, sizeof(u32)*256); + + } else { + i = psd_start; + + while (i < psd_stop) { + if (i >= psd_pts) + psd_data = rtw_GetPSDData(pAdapter, i - psd_pts); + else + psd_data = rtw_GetPSDData(pAdapter, i); + + pdata += sprintf(pdata, "%x ", psd_data); + i++; + } + + } + +#ifdef CONFIG_LONG_DELAY_ISSUE + rtw_msleep_os(100); +#else + rtw_mdelay_os(100); +#endif + + if (psd_analysis) + rtw_mp_psd_close(pAdapter, ®s); + + return strlen(data) + 1; +} + + +#if 0 +void _rtw_mp_xmit_priv(struct xmit_priv *pxmitpriv) +{ + int i, res; + _adapter *padapter = pxmitpriv->adapter; + struct xmit_frame *pxmitframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf; + struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf; + + u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ; + u32 num_xmit_extbuf = NR_XMIT_EXTBUFF; + if (padapter->registrypriv.mp_mode == 0) { + max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ; + num_xmit_extbuf = NR_XMIT_EXTBUFF; + } else { + max_xmit_extbuf_size = 6000; + num_xmit_extbuf = 8; + } + + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf; + for (i = 0; i < num_xmit_extbuf; i++) { + rtw_os_xmit_resource_free(padapter, pxmitbuf, (max_xmit_extbuf_size + XMITBUF_ALIGN_SZ), _FALSE); + + pxmitbuf++; + } + + if (pxmitpriv->pallocated_xmit_extbuf) + rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, num_xmit_extbuf * sizeof(struct xmit_buf) + 4); + + if (padapter->registrypriv.mp_mode == 0) { + max_xmit_extbuf_size = 6000; + num_xmit_extbuf = 8; + } else { + max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ; + num_xmit_extbuf = NR_XMIT_EXTBUFF; + } + + /* Init xmit extension buff */ + _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue); + + pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4); + + if (pxmitpriv->pallocated_xmit_extbuf == NULL) { + res = _FAIL; + goto exit; + } + + pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4); + + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf; + + for (i = 0; i < num_xmit_extbuf; i++) { + _rtw_init_listhead(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_MGNT; + + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, max_xmit_extbuf_size + XMITBUF_ALIGN_SZ, _TRUE); + if (res == _FAIL) { + res = _FAIL; + goto exit; + } + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + max_xmit_extbuf_size; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; +#endif + + rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue)); +#ifdef DBG_XMIT_BUF_EXT + pxmitbuf->no = i; +#endif + pxmitbuf++; + + } + + pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf; + +exit: + ; +} +#endif + +u8 +mpt_to_mgnt_rate( + u32 MptRateIdx +) +{ + /* Mapped to MGN_XXX defined in MgntGen.h */ + switch (MptRateIdx) { + /* CCK rate. */ + case MPT_RATE_1M: + return MGN_1M; + case MPT_RATE_2M: + return MGN_2M; + case MPT_RATE_55M: + return MGN_5_5M; + case MPT_RATE_11M: + return MGN_11M; + + /* OFDM rate. */ + case MPT_RATE_6M: + return MGN_6M; + case MPT_RATE_9M: + return MGN_9M; + case MPT_RATE_12M: + return MGN_12M; + case MPT_RATE_18M: + return MGN_18M; + case MPT_RATE_24M: + return MGN_24M; + case MPT_RATE_36M: + return MGN_36M; + case MPT_RATE_48M: + return MGN_48M; + case MPT_RATE_54M: + return MGN_54M; + + /* HT rate. */ + case MPT_RATE_MCS0: + return MGN_MCS0; + case MPT_RATE_MCS1: + return MGN_MCS1; + case MPT_RATE_MCS2: + return MGN_MCS2; + case MPT_RATE_MCS3: + return MGN_MCS3; + case MPT_RATE_MCS4: + return MGN_MCS4; + case MPT_RATE_MCS5: + return MGN_MCS5; + case MPT_RATE_MCS6: + return MGN_MCS6; + case MPT_RATE_MCS7: + return MGN_MCS7; + case MPT_RATE_MCS8: + return MGN_MCS8; + case MPT_RATE_MCS9: + return MGN_MCS9; + case MPT_RATE_MCS10: + return MGN_MCS10; + case MPT_RATE_MCS11: + return MGN_MCS11; + case MPT_RATE_MCS12: + return MGN_MCS12; + case MPT_RATE_MCS13: + return MGN_MCS13; + case MPT_RATE_MCS14: + return MGN_MCS14; + case MPT_RATE_MCS15: + return MGN_MCS15; + case MPT_RATE_MCS16: + return MGN_MCS16; + case MPT_RATE_MCS17: + return MGN_MCS17; + case MPT_RATE_MCS18: + return MGN_MCS18; + case MPT_RATE_MCS19: + return MGN_MCS19; + case MPT_RATE_MCS20: + return MGN_MCS20; + case MPT_RATE_MCS21: + return MGN_MCS21; + case MPT_RATE_MCS22: + return MGN_MCS22; + case MPT_RATE_MCS23: + return MGN_MCS23; + case MPT_RATE_MCS24: + return MGN_MCS24; + case MPT_RATE_MCS25: + return MGN_MCS25; + case MPT_RATE_MCS26: + return MGN_MCS26; + case MPT_RATE_MCS27: + return MGN_MCS27; + case MPT_RATE_MCS28: + return MGN_MCS28; + case MPT_RATE_MCS29: + return MGN_MCS29; + case MPT_RATE_MCS30: + return MGN_MCS30; + case MPT_RATE_MCS31: + return MGN_MCS31; + + /* VHT rate. */ + case MPT_RATE_VHT1SS_MCS0: + return MGN_VHT1SS_MCS0; + case MPT_RATE_VHT1SS_MCS1: + return MGN_VHT1SS_MCS1; + case MPT_RATE_VHT1SS_MCS2: + return MGN_VHT1SS_MCS2; + case MPT_RATE_VHT1SS_MCS3: + return MGN_VHT1SS_MCS3; + case MPT_RATE_VHT1SS_MCS4: + return MGN_VHT1SS_MCS4; + case MPT_RATE_VHT1SS_MCS5: + return MGN_VHT1SS_MCS5; + case MPT_RATE_VHT1SS_MCS6: + return MGN_VHT1SS_MCS6; + case MPT_RATE_VHT1SS_MCS7: + return MGN_VHT1SS_MCS7; + case MPT_RATE_VHT1SS_MCS8: + return MGN_VHT1SS_MCS8; + case MPT_RATE_VHT1SS_MCS9: + return MGN_VHT1SS_MCS9; + case MPT_RATE_VHT2SS_MCS0: + return MGN_VHT2SS_MCS0; + case MPT_RATE_VHT2SS_MCS1: + return MGN_VHT2SS_MCS1; + case MPT_RATE_VHT2SS_MCS2: + return MGN_VHT2SS_MCS2; + case MPT_RATE_VHT2SS_MCS3: + return MGN_VHT2SS_MCS3; + case MPT_RATE_VHT2SS_MCS4: + return MGN_VHT2SS_MCS4; + case MPT_RATE_VHT2SS_MCS5: + return MGN_VHT2SS_MCS5; + case MPT_RATE_VHT2SS_MCS6: + return MGN_VHT2SS_MCS6; + case MPT_RATE_VHT2SS_MCS7: + return MGN_VHT2SS_MCS7; + case MPT_RATE_VHT2SS_MCS8: + return MGN_VHT2SS_MCS8; + case MPT_RATE_VHT2SS_MCS9: + return MGN_VHT2SS_MCS9; + case MPT_RATE_VHT3SS_MCS0: + return MGN_VHT3SS_MCS0; + case MPT_RATE_VHT3SS_MCS1: + return MGN_VHT3SS_MCS1; + case MPT_RATE_VHT3SS_MCS2: + return MGN_VHT3SS_MCS2; + case MPT_RATE_VHT3SS_MCS3: + return MGN_VHT3SS_MCS3; + case MPT_RATE_VHT3SS_MCS4: + return MGN_VHT3SS_MCS4; + case MPT_RATE_VHT3SS_MCS5: + return MGN_VHT3SS_MCS5; + case MPT_RATE_VHT3SS_MCS6: + return MGN_VHT3SS_MCS6; + case MPT_RATE_VHT3SS_MCS7: + return MGN_VHT3SS_MCS7; + case MPT_RATE_VHT3SS_MCS8: + return MGN_VHT3SS_MCS8; + case MPT_RATE_VHT3SS_MCS9: + return MGN_VHT3SS_MCS9; + case MPT_RATE_VHT4SS_MCS0: + return MGN_VHT4SS_MCS0; + case MPT_RATE_VHT4SS_MCS1: + return MGN_VHT4SS_MCS1; + case MPT_RATE_VHT4SS_MCS2: + return MGN_VHT4SS_MCS2; + case MPT_RATE_VHT4SS_MCS3: + return MGN_VHT4SS_MCS3; + case MPT_RATE_VHT4SS_MCS4: + return MGN_VHT4SS_MCS4; + case MPT_RATE_VHT4SS_MCS5: + return MGN_VHT4SS_MCS5; + case MPT_RATE_VHT4SS_MCS6: + return MGN_VHT4SS_MCS6; + case MPT_RATE_VHT4SS_MCS7: + return MGN_VHT4SS_MCS7; + case MPT_RATE_VHT4SS_MCS8: + return MGN_VHT4SS_MCS8; + case MPT_RATE_VHT4SS_MCS9: + return MGN_VHT4SS_MCS9; + + case MPT_RATE_LAST: /* fully automatiMGN_VHT2SS_MCS1; */ + default: + RTW_INFO("<===mpt_to_mgnt_rate(), Invalid Rate: %d!!\n", MptRateIdx); + return 0x0; + } +} + + +u8 HwRateToMPTRate(u8 rate) +{ + u8 ret_rate = MGN_1M; + + switch (rate) { + case DESC_RATE1M: + ret_rate = MPT_RATE_1M; + break; + case DESC_RATE2M: + ret_rate = MPT_RATE_2M; + break; + case DESC_RATE5_5M: + ret_rate = MPT_RATE_55M; + break; + case DESC_RATE11M: + ret_rate = MPT_RATE_11M; + break; + case DESC_RATE6M: + ret_rate = MPT_RATE_6M; + break; + case DESC_RATE9M: + ret_rate = MPT_RATE_9M; + break; + case DESC_RATE12M: + ret_rate = MPT_RATE_12M; + break; + case DESC_RATE18M: + ret_rate = MPT_RATE_18M; + break; + case DESC_RATE24M: + ret_rate = MPT_RATE_24M; + break; + case DESC_RATE36M: + ret_rate = MPT_RATE_36M; + break; + case DESC_RATE48M: + ret_rate = MPT_RATE_48M; + break; + case DESC_RATE54M: + ret_rate = MPT_RATE_54M; + break; + case DESC_RATEMCS0: + ret_rate = MPT_RATE_MCS0; + break; + case DESC_RATEMCS1: + ret_rate = MPT_RATE_MCS1; + break; + case DESC_RATEMCS2: + ret_rate = MPT_RATE_MCS2; + break; + case DESC_RATEMCS3: + ret_rate = MPT_RATE_MCS3; + break; + case DESC_RATEMCS4: + ret_rate = MPT_RATE_MCS4; + break; + case DESC_RATEMCS5: + ret_rate = MPT_RATE_MCS5; + break; + case DESC_RATEMCS6: + ret_rate = MPT_RATE_MCS6; + break; + case DESC_RATEMCS7: + ret_rate = MPT_RATE_MCS7; + break; + case DESC_RATEMCS8: + ret_rate = MPT_RATE_MCS8; + break; + case DESC_RATEMCS9: + ret_rate = MPT_RATE_MCS9; + break; + case DESC_RATEMCS10: + ret_rate = MPT_RATE_MCS10; + break; + case DESC_RATEMCS11: + ret_rate = MPT_RATE_MCS11; + break; + case DESC_RATEMCS12: + ret_rate = MPT_RATE_MCS12; + break; + case DESC_RATEMCS13: + ret_rate = MPT_RATE_MCS13; + break; + case DESC_RATEMCS14: + ret_rate = MPT_RATE_MCS14; + break; + case DESC_RATEMCS15: + ret_rate = MPT_RATE_MCS15; + break; + case DESC_RATEMCS16: + ret_rate = MPT_RATE_MCS16; + break; + case DESC_RATEMCS17: + ret_rate = MPT_RATE_MCS17; + break; + case DESC_RATEMCS18: + ret_rate = MPT_RATE_MCS18; + break; + case DESC_RATEMCS19: + ret_rate = MPT_RATE_MCS19; + break; + case DESC_RATEMCS20: + ret_rate = MPT_RATE_MCS20; + break; + case DESC_RATEMCS21: + ret_rate = MPT_RATE_MCS21; + break; + case DESC_RATEMCS22: + ret_rate = MPT_RATE_MCS22; + break; + case DESC_RATEMCS23: + ret_rate = MPT_RATE_MCS23; + break; + case DESC_RATEMCS24: + ret_rate = MPT_RATE_MCS24; + break; + case DESC_RATEMCS25: + ret_rate = MPT_RATE_MCS25; + break; + case DESC_RATEMCS26: + ret_rate = MPT_RATE_MCS26; + break; + case DESC_RATEMCS27: + ret_rate = MPT_RATE_MCS27; + break; + case DESC_RATEMCS28: + ret_rate = MPT_RATE_MCS28; + break; + case DESC_RATEMCS29: + ret_rate = MPT_RATE_MCS29; + break; + case DESC_RATEMCS30: + ret_rate = MPT_RATE_MCS30; + break; + case DESC_RATEMCS31: + ret_rate = MPT_RATE_MCS31; + break; + case DESC_RATEVHTSS1MCS0: + ret_rate = MPT_RATE_VHT1SS_MCS0; + break; + case DESC_RATEVHTSS1MCS1: + ret_rate = MPT_RATE_VHT1SS_MCS1; + break; + case DESC_RATEVHTSS1MCS2: + ret_rate = MPT_RATE_VHT1SS_MCS2; + break; + case DESC_RATEVHTSS1MCS3: + ret_rate = MPT_RATE_VHT1SS_MCS3; + break; + case DESC_RATEVHTSS1MCS4: + ret_rate = MPT_RATE_VHT1SS_MCS4; + break; + case DESC_RATEVHTSS1MCS5: + ret_rate = MPT_RATE_VHT1SS_MCS5; + break; + case DESC_RATEVHTSS1MCS6: + ret_rate = MPT_RATE_VHT1SS_MCS6; + break; + case DESC_RATEVHTSS1MCS7: + ret_rate = MPT_RATE_VHT1SS_MCS7; + break; + case DESC_RATEVHTSS1MCS8: + ret_rate = MPT_RATE_VHT1SS_MCS8; + break; + case DESC_RATEVHTSS1MCS9: + ret_rate = MPT_RATE_VHT1SS_MCS9; + break; + case DESC_RATEVHTSS2MCS0: + ret_rate = MPT_RATE_VHT2SS_MCS0; + break; + case DESC_RATEVHTSS2MCS1: + ret_rate = MPT_RATE_VHT2SS_MCS1; + break; + case DESC_RATEVHTSS2MCS2: + ret_rate = MPT_RATE_VHT2SS_MCS2; + break; + case DESC_RATEVHTSS2MCS3: + ret_rate = MPT_RATE_VHT2SS_MCS3; + break; + case DESC_RATEVHTSS2MCS4: + ret_rate = MPT_RATE_VHT2SS_MCS4; + break; + case DESC_RATEVHTSS2MCS5: + ret_rate = MPT_RATE_VHT2SS_MCS5; + break; + case DESC_RATEVHTSS2MCS6: + ret_rate = MPT_RATE_VHT2SS_MCS6; + break; + case DESC_RATEVHTSS2MCS7: + ret_rate = MPT_RATE_VHT2SS_MCS7; + break; + case DESC_RATEVHTSS2MCS8: + ret_rate = MPT_RATE_VHT2SS_MCS8; + break; + case DESC_RATEVHTSS2MCS9: + ret_rate = MPT_RATE_VHT2SS_MCS9; + break; + case DESC_RATEVHTSS3MCS0: + ret_rate = MPT_RATE_VHT3SS_MCS0; + break; + case DESC_RATEVHTSS3MCS1: + ret_rate = MPT_RATE_VHT3SS_MCS1; + break; + case DESC_RATEVHTSS3MCS2: + ret_rate = MPT_RATE_VHT3SS_MCS2; + break; + case DESC_RATEVHTSS3MCS3: + ret_rate = MPT_RATE_VHT3SS_MCS3; + break; + case DESC_RATEVHTSS3MCS4: + ret_rate = MPT_RATE_VHT3SS_MCS4; + break; + case DESC_RATEVHTSS3MCS5: + ret_rate = MPT_RATE_VHT3SS_MCS5; + break; + case DESC_RATEVHTSS3MCS6: + ret_rate = MPT_RATE_VHT3SS_MCS6; + break; + case DESC_RATEVHTSS3MCS7: + ret_rate = MPT_RATE_VHT3SS_MCS7; + break; + case DESC_RATEVHTSS3MCS8: + ret_rate = MPT_RATE_VHT3SS_MCS8; + break; + case DESC_RATEVHTSS3MCS9: + ret_rate = MPT_RATE_VHT3SS_MCS9; + break; + case DESC_RATEVHTSS4MCS0: + ret_rate = MPT_RATE_VHT4SS_MCS0; + break; + case DESC_RATEVHTSS4MCS1: + ret_rate = MPT_RATE_VHT4SS_MCS1; + break; + case DESC_RATEVHTSS4MCS2: + ret_rate = MPT_RATE_VHT4SS_MCS2; + break; + case DESC_RATEVHTSS4MCS3: + ret_rate = MPT_RATE_VHT4SS_MCS3; + break; + case DESC_RATEVHTSS4MCS4: + ret_rate = MPT_RATE_VHT4SS_MCS4; + break; + case DESC_RATEVHTSS4MCS5: + ret_rate = MPT_RATE_VHT4SS_MCS5; + break; + case DESC_RATEVHTSS4MCS6: + ret_rate = MPT_RATE_VHT4SS_MCS6; + break; + case DESC_RATEVHTSS4MCS7: + ret_rate = MPT_RATE_VHT4SS_MCS7; + break; + case DESC_RATEVHTSS4MCS8: + ret_rate = MPT_RATE_VHT4SS_MCS8; + break; + case DESC_RATEVHTSS4MCS9: + ret_rate = MPT_RATE_VHT4SS_MCS9; + break; + + default: + RTW_INFO("hw_rate_to_m_rate(): Non supported Rate [%x]!!!\n", rate); + break; + } + return ret_rate; +} + +u8 rtw_mpRateParseFunc(PADAPTER pAdapter, u8 *targetStr) +{ + u16 i = 0; + u8 *rateindex_Array[] = { "1M", "2M", "5.5M", "11M", "6M", "9M", "12M", "18M", "24M", "36M", "48M", "54M", + "HTMCS0", "HTMCS1", "HTMCS2", "HTMCS3", "HTMCS4", "HTMCS5", "HTMCS6", "HTMCS7", + "HTMCS8", "HTMCS9", "HTMCS10", "HTMCS11", "HTMCS12", "HTMCS13", "HTMCS14", "HTMCS15", + "HTMCS16", "HTMCS17", "HTMCS18", "HTMCS19", "HTMCS20", "HTMCS21", "HTMCS22", "HTMCS23", + "HTMCS24", "HTMCS25", "HTMCS26", "HTMCS27", "HTMCS28", "HTMCS29", "HTMCS30", "HTMCS31", + "VHT1MCS0", "VHT1MCS1", "VHT1MCS2", "VHT1MCS3", "VHT1MCS4", "VHT1MCS5", "VHT1MCS6", "VHT1MCS7", "VHT1MCS8", "VHT1MCS9", + "VHT2MCS0", "VHT2MCS1", "VHT2MCS2", "VHT2MCS3", "VHT2MCS4", "VHT2MCS5", "VHT2MCS6", "VHT2MCS7", "VHT2MCS8", "VHT2MCS9", + "VHT3MCS0", "VHT3MCS1", "VHT3MCS2", "VHT3MCS3", "VHT3MCS4", "VHT3MCS5", "VHT3MCS6", "VHT3MCS7", "VHT3MCS8", "VHT3MCS9", + "VHT4MCS0", "VHT4MCS1", "VHT4MCS2", "VHT4MCS3", "VHT4MCS4", "VHT4MCS5", "VHT4MCS6", "VHT4MCS7", "VHT4MCS8", "VHT4MCS9" + }; + + for (i = 0; i <= 83; i++) { + if (strcmp(targetStr, rateindex_Array[i]) == 0) { + RTW_INFO("%s , index = %d\n", __func__ , i); + return i; + } + } + + printk("%s ,please input a Data RATE String as:", __func__); + for (i = 0; i <= 83; i++) { + printk("%s ", rateindex_Array[i]); + if (i % 10 == 0) + printk("\n"); + } + return _FAIL; +} + +u8 rtw_mp_mode_check(PADAPTER pAdapter) +{ + PADAPTER primary_adapter = GET_PRIMARY_ADAPTER(pAdapter); + + if (primary_adapter->registrypriv.mp_mode == 1 || primary_adapter->mppriv.bprocess_mp_mode == _TRUE) + return _TRUE; + else + return _FALSE; +} + +bool rtw_is_mp_tssitrk_on(_adapter *adapter) +{ + _adapter *primary_adapter = GET_PRIMARY_ADAPTER(adapter); + + return primary_adapter->mppriv.tssitrk_on; +} + +u32 mpt_ProQueryCalTxPower( + PADAPTER pAdapter, + u8 RfPath +) +{ + + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(pAdapter); + PMPT_CONTEXT pMptCtx = &(pAdapter->mppriv.mpt_ctx); + + u32 TxPower = 1; + struct txpwr_idx_comp tic; + u8 mgn_rate = mpt_to_mgnt_rate(pMptCtx->mpt_rate_index); + RATE_SECTION rs = mgn_rate_to_rs(mgn_rate); + + TxPower = rtw_hal_get_tx_power_index(pAdapter, RfPath, rs, mgn_rate + , pHalData->current_channel_bw, pHalData->current_band_type, pHalData->current_channel, 0, &tic); + + dump_tx_power_index_inline(RTW_DBGDUMP, pAdapter, RfPath + , pHalData->current_channel_bw, pHalData->current_channel + , mgn_rate, TxPower, &tic); + + pAdapter->mppriv.txpoweridx = (u8)TxPower; + if (RfPath == RF_PATH_A) + pMptCtx->TxPwrLevel[RF_PATH_A] = (u8)TxPower; + else if (RfPath == RF_PATH_B) + pMptCtx->TxPwrLevel[RF_PATH_B] = (u8)TxPower; + else if (RfPath == RF_PATH_C) + pMptCtx->TxPwrLevel[RF_PATH_C] = (u8)TxPower; + else if (RfPath == RF_PATH_D) + pMptCtx->TxPwrLevel[RF_PATH_D] = (u8)TxPower; + hal_mpt_SetTxPower(pAdapter); + + return TxPower; +} + +u32 mpt_get_tx_power_finalabs_val(PADAPTER padapter, u8 rf_path) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + PMPT_CONTEXT pMptCtx = &(padapter->mppriv.mpt_ctx); + + u8 mgn_rate = mpt_to_mgnt_rate(pMptCtx->mpt_rate_index); + u32 powerdbm = 0; + + powerdbm = phy_get_tx_power_final_absolute_value(padapter, rf_path, mgn_rate, pHalData->current_channel_bw, pHalData->current_channel); + + RTW_INFO("bw=%d, ch=%d, rateid=%d, TSSI Power(dBm):%d\n", + pHalData->current_channel_bw, pHalData->current_channel, mgn_rate ,powerdbm); + + return powerdbm; +} + +#ifdef CONFIG_MP_VHT_HW_TX_MODE +static inline void dump_buf(u8 *buf, u32 len) +{ + u32 i; + + RTW_INFO("-----------------Len %d----------------\n", len); + for (i = 0; i < len; i++) + RTW_INFO("%2.2x-", *(buf + i)); + RTW_INFO("\n"); +} + +void ByteToBit( + u8 *out, + bool *in, + u8 in_size) +{ + u8 i = 0, j = 0; + + for (i = 0; i < in_size; i++) { + for (j = 0; j < 8; j++) { + if (in[8 * i + j]) + out[i] |= (1 << j); + } + } +} + + +void CRC16_generator( + bool *out, + bool *in, + u8 in_size +) +{ + u8 i = 0; + bool temp = 0, reg[] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; + + for (i = 0; i < in_size; i++) {/* take one's complement and bit reverse*/ + temp = in[i] ^ reg[15]; + reg[15] = reg[14]; + reg[14] = reg[13]; + reg[13] = reg[12]; + reg[12] = reg[11]; + reg[11] = reg[10]; + reg[10] = reg[9]; + reg[9] = reg[8]; + reg[8] = reg[7]; + + reg[7] = reg[6]; + reg[6] = reg[5]; + reg[5] = reg[4]; + reg[4] = reg[3]; + reg[3] = reg[2]; + reg[2] = reg[1]; + reg[1] = reg[0]; + reg[12] = reg[12] ^ temp; + reg[5] = reg[5] ^ temp; + reg[0] = temp; + } + for (i = 0; i < 16; i++) /* take one's complement and bit reverse*/ + out[i] = 1 - reg[15 - i]; +} + + + +/*======================================== + SFD SIGNAL SERVICE LENGTH CRC + 16 bit 8 bit 8 bit 16 bit 16 bit +========================================*/ +void CCK_generator( + PRT_PMAC_TX_INFO pPMacTxInfo, + PRT_PMAC_PKT_INFO pPMacPktInfo +) +{ + double ratio = 0; + bool crc16_in[32] = {0}, crc16_out[16] = {0}; + bool LengthExtBit; + double LengthExact; + double LengthPSDU; + u8 i; + u32 PacketLength = pPMacTxInfo->PacketLength; + + if (pPMacTxInfo->bSPreamble) + pPMacTxInfo->SFD = 0x05CF; + else + pPMacTxInfo->SFD = 0xF3A0; + + switch (pPMacPktInfo->MCS) { + case 0: + pPMacTxInfo->SignalField = 0xA; + ratio = 8; + /*CRC16_in(1,0:7)=[0 1 0 1 0 0 0 0]*/ + crc16_in[1] = crc16_in[3] = 1; + break; + case 1: + pPMacTxInfo->SignalField = 0x14; + ratio = 4; + /*CRC16_in(1,0:7)=[0 0 1 0 1 0 0 0];*/ + crc16_in[2] = crc16_in[4] = 1; + break; + case 2: + pPMacTxInfo->SignalField = 0x37; + ratio = 8.0 / 5.5; + /*CRC16_in(1,0:7)=[1 1 1 0 1 1 0 0];*/ + crc16_in[0] = crc16_in[1] = crc16_in[2] = crc16_in[4] = crc16_in[5] = 1; + break; + case 3: + pPMacTxInfo->SignalField = 0x6E; + ratio = 8.0 / 11.0; + /*CRC16_in(1,0:7)=[0 1 1 1 0 1 1 0];*/ + crc16_in[1] = crc16_in[2] = crc16_in[3] = crc16_in[5] = crc16_in[6] = 1; + break; + } + + LengthExact = PacketLength * ratio; + LengthPSDU = ceil(LengthExact); + + if ((pPMacPktInfo->MCS == 3) && + ((LengthPSDU - LengthExact) >= 0.727 || (LengthPSDU - LengthExact) <= -0.727)) + LengthExtBit = 1; + else + LengthExtBit = 0; + + + pPMacTxInfo->LENGTH = (u32)LengthPSDU; + /* CRC16_in(1,16:31) = LengthPSDU[0:15]*/ + for (i = 0; i < 16; i++) + crc16_in[i + 16] = (pPMacTxInfo->LENGTH >> i) & 0x1; + + if (LengthExtBit == 0) { + pPMacTxInfo->ServiceField = 0x0; + /* CRC16_in(1,8:15) = [0 0 0 0 0 0 0 0];*/ + } else { + pPMacTxInfo->ServiceField = 0x80; + /*CRC16_in(1,8:15)=[0 0 0 0 0 0 0 1];*/ + crc16_in[15] = 1; + } + + CRC16_generator(crc16_out, crc16_in, 32); + + _rtw_memset(pPMacTxInfo->CRC16, 0, 2); + ByteToBit(pPMacTxInfo->CRC16, crc16_out, 2); + +} + + +void PMAC_Get_Pkt_Param( + PRT_PMAC_TX_INFO pPMacTxInfo, + PRT_PMAC_PKT_INFO pPMacPktInfo) +{ + + u8 TX_RATE_HEX = 0, MCS = 0; + u8 TX_RATE = pPMacTxInfo->TX_RATE; + + /* TX_RATE & Nss */ + if (MPT_IS_2SS_RATE(TX_RATE)) + pPMacPktInfo->Nss = 2; + else if (MPT_IS_3SS_RATE(TX_RATE)) + pPMacPktInfo->Nss = 3; + else if (MPT_IS_4SS_RATE(TX_RATE)) + pPMacPktInfo->Nss = 4; + else + pPMacPktInfo->Nss = 1; + + RTW_INFO("PMacTxInfo.Nss =%d\n", pPMacPktInfo->Nss); + + /* MCS & TX_RATE_HEX*/ + if (MPT_IS_CCK_RATE(TX_RATE)) { + switch (TX_RATE) { + case MPT_RATE_1M: + TX_RATE_HEX = MCS = 0; + break; + case MPT_RATE_2M: + TX_RATE_HEX = MCS = 1; + break; + case MPT_RATE_55M: + TX_RATE_HEX = MCS = 2; + break; + case MPT_RATE_11M: + TX_RATE_HEX = MCS = 3; + break; + } + } else if (MPT_IS_OFDM_RATE(TX_RATE)) { + MCS = TX_RATE - MPT_RATE_6M; + TX_RATE_HEX = MCS + 4; + } else if (MPT_IS_HT_RATE(TX_RATE)) { + MCS = TX_RATE - MPT_RATE_MCS0; + TX_RATE_HEX = MCS + 12; + } else if (MPT_IS_VHT_RATE(TX_RATE)) { + TX_RATE_HEX = TX_RATE - MPT_RATE_VHT1SS_MCS0 + 44; + + if (MPT_IS_VHT_2S_RATE(TX_RATE)) + MCS = TX_RATE - MPT_RATE_VHT2SS_MCS0; + else if (MPT_IS_VHT_3S_RATE(TX_RATE)) + MCS = TX_RATE - MPT_RATE_VHT3SS_MCS0; + else if (MPT_IS_VHT_4S_RATE(TX_RATE)) + MCS = TX_RATE - MPT_RATE_VHT4SS_MCS0; + else + MCS = TX_RATE - MPT_RATE_VHT1SS_MCS0; + } + + pPMacPktInfo->MCS = MCS; + pPMacTxInfo->TX_RATE_HEX = TX_RATE_HEX; + + RTW_INFO(" MCS=%d, TX_RATE_HEX =0x%x\n", MCS, pPMacTxInfo->TX_RATE_HEX); + /* mSTBC & Nsts*/ + pPMacPktInfo->Nsts = pPMacPktInfo->Nss; + if (pPMacTxInfo->bSTBC) { + if (pPMacPktInfo->Nss == 1) { + pPMacTxInfo->m_STBC = 2; + pPMacPktInfo->Nsts = pPMacPktInfo->Nss * 2; + } else + pPMacTxInfo->m_STBC = 1; + } else + pPMacTxInfo->m_STBC = 1; +} + + +u32 LDPC_parameter_generator( + u32 N_pld_int, + u32 N_CBPSS, + u32 N_SS, + u32 R, + u32 m_STBC, + u32 N_TCB_int +) +{ + double CR = 0.; + double N_pld = (double)N_pld_int; + double N_TCB = (double)N_TCB_int; + double N_CW = 0., N_shrt = 0., N_spcw = 0., N_fshrt = 0.; + double L_LDPC = 0., K_LDPC = 0., L_LDPC_info = 0.; + double N_punc = 0., N_ppcw = 0., N_fpunc = 0., N_rep = 0., N_rpcw = 0., N_frep = 0.; + double R_eff = 0.; + u32 VHTSIGA2B3 = 0;/* extra symbol from VHT-SIG-A2 Bit 3*/ + + if (R == 0) + CR = 0.5; + else if (R == 1) + CR = 2. / 3.; + else if (R == 2) + CR = 3. / 4.; + else if (R == 3) + CR = 5. / 6.; + + if (N_TCB <= 648.) { + N_CW = 1.; + if (N_TCB >= N_pld + 912.*(1. - CR)) + L_LDPC = 1296.; + else + L_LDPC = 648.; + } else if (N_TCB <= 1296.) { + N_CW = 1.; + if (N_TCB >= (double)N_pld + 1464.*(1. - CR)) + L_LDPC = 1944.; + else + L_LDPC = 1296.; + } else if (N_TCB <= 1944.) { + N_CW = 1.; + L_LDPC = 1944.; + } else if (N_TCB <= 2592.) { + N_CW = 2.; + if (N_TCB >= N_pld + 2916.*(1. - CR)) + L_LDPC = 1944.; + else + L_LDPC = 1296.; + } else { + N_CW = ceil(N_pld / 1944. / CR); + L_LDPC = 1944.; + } + /* Number of information bits per CW*/ + K_LDPC = L_LDPC * CR; + /* Number of shortening bits max(0, (N_CW * L_LDPC * R) - N_pld)*/ + N_shrt = (N_CW * K_LDPC - N_pld) > 0. ? (N_CW * K_LDPC - N_pld) : 0.; + /* Number of shortening bits per CW N_spcw = rtfloor(N_shrt/N_CW)*/ + N_spcw = rtfloor(N_shrt / N_CW); + /* The first N_fshrt CWs shorten 1 bit more*/ + N_fshrt = (double)((int)N_shrt % (int)N_CW); + /* Number of data bits for the last N_CW-N_fshrt CWs*/ + L_LDPC_info = K_LDPC - N_spcw; + /* Number of puncturing bits*/ + N_punc = (N_CW * L_LDPC - N_TCB - N_shrt) > 0. ? (N_CW * L_LDPC - N_TCB - N_shrt) : 0.; + if (((N_punc > .1 * N_CW * L_LDPC * (1. - CR)) && (N_shrt < 1.2 * N_punc * CR / (1. - CR))) || + (N_punc > 0.3 * N_CW * L_LDPC * (1. - CR))) { + /*cout << "*** N_TCB and N_punc are Recomputed ***" << endl;*/ + VHTSIGA2B3 = 1; + N_TCB += (double)N_CBPSS * N_SS * m_STBC; + N_punc = (N_CW * L_LDPC - N_TCB - N_shrt) > 0. ? (N_CW * L_LDPC - N_TCB - N_shrt) : 0.; + } else + VHTSIGA2B3 = 0; + + return VHTSIGA2B3; +} /* function end of LDPC_parameter_generator */ + +/*======================================== + Data field of PPDU + Get N_sym and SIGA2BB3 +========================================*/ +void PMAC_Nsym_generator( + PRT_PMAC_TX_INFO pPMacTxInfo, + PRT_PMAC_PKT_INFO pPMacPktInfo) +{ + u32 SIGA2B3 = 0; + u8 TX_RATE = pPMacTxInfo->TX_RATE; + + u32 R, R_list[10] = {0, 0, 2, 0, 2, 1, 2, 3, 2, 3}; + double CR = 0; + u32 N_SD, N_BPSC_list[10] = {1, 2, 2, 4, 4, 6, 6, 6, 8, 8}; + u32 N_BPSC = 0, N_CBPS = 0, N_DBPS = 0, N_ES = 0, N_SYM = 0, N_pld = 0, N_TCB = 0; + int D_R = 0; + + RTW_INFO("TX_RATE = %d\n", TX_RATE); + /* N_SD*/ + if (pPMacTxInfo->BandWidth == 0) + N_SD = 52; + else if (pPMacTxInfo->BandWidth == 1) + N_SD = 108; + else + N_SD = 234; + + if (MPT_IS_HT_RATE(TX_RATE)) { + u8 MCS_temp; + + if (pPMacPktInfo->MCS > 23) + MCS_temp = pPMacPktInfo->MCS - 24; + else if (pPMacPktInfo->MCS > 15) + MCS_temp = pPMacPktInfo->MCS - 16; + else if (pPMacPktInfo->MCS > 7) + MCS_temp = pPMacPktInfo->MCS - 8; + else + MCS_temp = pPMacPktInfo->MCS; + + R = R_list[MCS_temp]; + + switch (R) { + case 0: + CR = .5; + break; + case 1: + CR = 2. / 3.; + break; + case 2: + CR = 3. / 4.; + break; + case 3: + CR = 5. / 6.; + break; + } + + N_BPSC = N_BPSC_list[MCS_temp]; + N_CBPS = N_BPSC * N_SD * pPMacPktInfo->Nss; + N_DBPS = (u32)((double)N_CBPS * CR); + + if (pPMacTxInfo->bLDPC == FALSE) { + N_ES = (u32)ceil((double)(N_DBPS * pPMacPktInfo->Nss) / 4. / 300.); + RTW_INFO("N_ES = %d\n", N_ES); + + /* N_SYM = m_STBC* (8*length+16+6*N_ES) / (m_STBC*N_DBPS)*/ + N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(pPMacTxInfo->PacketLength * 8 + 16 + N_ES * 6) / + (double)(N_DBPS * pPMacTxInfo->m_STBC)); + + } else { + N_ES = 1; + /* N_pld = length * 8 + 16*/ + N_pld = pPMacTxInfo->PacketLength * 8 + 16; + RTW_INFO("N_pld = %d\n", N_pld); + N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(N_pld) / + (double)(N_DBPS * pPMacTxInfo->m_STBC)); + RTW_INFO("N_SYM = %d\n", N_SYM); + /* N_avbits = N_CBPS *m_STBC *(N_pld/N_CBPS*R*m_STBC)*/ + N_TCB = N_CBPS * N_SYM; + RTW_INFO("N_TCB = %d\n", N_TCB); + SIGA2B3 = LDPC_parameter_generator(N_pld, N_CBPS, pPMacPktInfo->Nss, R, pPMacTxInfo->m_STBC, N_TCB); + RTW_INFO("SIGA2B3 = %d\n", SIGA2B3); + N_SYM = N_SYM + SIGA2B3 * pPMacTxInfo->m_STBC; + RTW_INFO("N_SYM = %d\n", N_SYM); + } + } else if (MPT_IS_VHT_RATE(TX_RATE)) { + R = R_list[pPMacPktInfo->MCS]; + + switch (R) { + case 0: + CR = .5; + break; + case 1: + CR = 2. / 3.; + break; + case 2: + CR = 3. / 4.; + break; + case 3: + CR = 5. / 6.; + break; + } + N_BPSC = N_BPSC_list[pPMacPktInfo->MCS]; + N_CBPS = N_BPSC * N_SD * pPMacPktInfo->Nss; + N_DBPS = (u32)((double)N_CBPS * CR); + if (pPMacTxInfo->bLDPC == FALSE) { + if (pPMacTxInfo->bSGI) + N_ES = (u32)ceil((double)(N_DBPS) / 3.6 / 600.); + else + N_ES = (u32)ceil((double)(N_DBPS) / 4. / 600.); + /* N_SYM = m_STBC* (8*length+16+6*N_ES) / (m_STBC*N_DBPS)*/ + N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(pPMacTxInfo->PacketLength * 8 + 16 + N_ES * 6) / (double)(N_DBPS * pPMacTxInfo->m_STBC)); + SIGA2B3 = 0; + } else { + N_ES = 1; + /* N_SYM = m_STBC* (8*length+N_service) / (m_STBC*N_DBPS)*/ + N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(pPMacTxInfo->PacketLength * 8 + 16) / (double)(N_DBPS * pPMacTxInfo->m_STBC)); + /* N_avbits = N_sys_init * N_CBPS*/ + N_TCB = N_CBPS * N_SYM; + /* N_pld = N_sys_init * N_DBPS*/ + N_pld = N_SYM * N_DBPS; + SIGA2B3 = LDPC_parameter_generator(N_pld, N_CBPS, pPMacPktInfo->Nss, R, pPMacTxInfo->m_STBC, N_TCB); + N_SYM = N_SYM + SIGA2B3 * pPMacTxInfo->m_STBC; + } + + switch (R) { + case 0: + D_R = 2; + break; + case 1: + D_R = 3; + break; + case 2: + D_R = 4; + break; + case 3: + D_R = 6; + break; + } + + if (((N_CBPS / N_ES) % D_R) != 0) { + RTW_INFO("MCS= %d is not supported when Nss=%d and BW= %d !!\n", pPMacPktInfo->MCS, pPMacPktInfo->Nss, pPMacTxInfo->BandWidth); + return; + } + + RTW_INFO("MCS= %d Nss=%d and BW= %d !!\n", pPMacPktInfo->MCS, pPMacPktInfo->Nss, pPMacTxInfo->BandWidth); + } + + pPMacPktInfo->N_sym = N_SYM; + pPMacPktInfo->SIGA2B3 = SIGA2B3; +} + +/*======================================== + L-SIG Rate R Length P Tail + 4b 1b 12b 1b 6b +========================================*/ + +void L_SIG_generator( + u32 N_SYM, /* Max: 750*/ + PRT_PMAC_TX_INFO pPMacTxInfo, + PRT_PMAC_PKT_INFO pPMacPktInfo) +{ + u8 sig_bi[24] = {0}; /* 24 BIT*/ + u32 mode, LENGTH; + int i; + + if (MPT_IS_OFDM_RATE(pPMacTxInfo->TX_RATE)) { + mode = pPMacPktInfo->MCS; + LENGTH = pPMacTxInfo->PacketLength; + } else { + u8 N_LTF; + double T_data; + u32 OFDM_symbol; + + mode = 0; + + /* Table 20-13 Num of HT-DLTFs request*/ + if (pPMacPktInfo->Nsts <= 2) + N_LTF = pPMacPktInfo->Nsts; + else + N_LTF = 4; + + if (pPMacTxInfo->bSGI) + T_data = 3.6; + else + T_data = 4.0; + + /*(L-SIG, HT-SIG, HT-STF, HT-LTF....HT-LTF, Data)*/ + if (MPT_IS_VHT_RATE(pPMacTxInfo->TX_RATE)) + OFDM_symbol = (u32)ceil((double)(8 + 4 + N_LTF * 4 + N_SYM * T_data + 4) / 4.); + else + OFDM_symbol = (u32)ceil((double)(8 + 4 + N_LTF * 4 + N_SYM * T_data) / 4.); + + RTW_INFO("%s , OFDM_symbol =%d\n", __func__, OFDM_symbol); + LENGTH = OFDM_symbol * 3 - 3; + RTW_INFO("%s , LENGTH =%d\n", __func__, LENGTH); + + } + /* Rate Field*/ + switch (mode) { + case 0: + sig_bi[0] = 1; + sig_bi[1] = 1; + sig_bi[2] = 0; + sig_bi[3] = 1; + break; + case 1: + sig_bi[0] = 1; + sig_bi[1] = 1; + sig_bi[2] = 1; + sig_bi[3] = 1; + break; + case 2: + sig_bi[0] = 0; + sig_bi[1] = 1; + sig_bi[2] = 0; + sig_bi[3] = 1; + break; + case 3: + sig_bi[0] = 0; + sig_bi[1] = 1; + sig_bi[2] = 1; + sig_bi[3] = 1; + break; + case 4: + sig_bi[0] = 1; + sig_bi[1] = 0; + sig_bi[2] = 0; + sig_bi[3] = 1; + break; + case 5: + sig_bi[0] = 1; + sig_bi[1] = 0; + sig_bi[2] = 1; + sig_bi[3] = 1; + break; + case 6: + sig_bi[0] = 0; + sig_bi[1] = 0; + sig_bi[2] = 0; + sig_bi[3] = 1; + break; + case 7: + sig_bi[0] = 0; + sig_bi[1] = 0; + sig_bi[2] = 1; + sig_bi[3] = 1; + break; + } + /*Reserved bit*/ + sig_bi[4] = 0; + + /* Length Field*/ + for (i = 0; i < 12; i++) + sig_bi[i + 5] = (LENGTH >> i) & 1; + + /* Parity Bit*/ + sig_bi[17] = 0; + for (i = 0; i < 17; i++) + sig_bi[17] = sig_bi[17] + sig_bi[i]; + + sig_bi[17] %= 2; + + /* Tail Field*/ + for (i = 18; i < 24; i++) + sig_bi[i] = 0; + + /* dump_buf(sig_bi,24);*/ + _rtw_memset(pPMacTxInfo->LSIG, 0, 3); + ByteToBit(pPMacTxInfo->LSIG, (bool *)sig_bi, 3); +} + + +void CRC8_generator( + bool *out, + bool *in, + u8 in_size +) +{ + u8 i = 0; + bool temp = 0, reg[] = {1, 1, 1, 1, 1, 1, 1, 1}; + + for (i = 0; i < in_size; i++) { /* take one's complement and bit reverse*/ + temp = in[i] ^ reg[7]; + reg[7] = reg[6]; + reg[6] = reg[5]; + reg[5] = reg[4]; + reg[4] = reg[3]; + reg[3] = reg[2]; + reg[2] = reg[1] ^ temp; + reg[1] = reg[0] ^ temp; + reg[0] = temp; + } + for (i = 0; i < 8; i++)/* take one's complement and bit reverse*/ + out[i] = reg[7 - i] ^ 1; +} + +/*/================================================================================ + HT-SIG1 MCS CW Length 24BIT + 24BIT + 7b 1b 16b + HT-SIG2 Smoothing Not sounding Rsvd AGG STBC FEC SGI N_ELTF CRC Tail + 1b 1b 1b 1b 2b 1b 1b 2b 8b 6b +================================================================================*/ +void HT_SIG_generator( + PRT_PMAC_TX_INFO pPMacTxInfo, + PRT_PMAC_PKT_INFO pPMacPktInfo +) +{ + u32 i; + bool sig_bi[48] = {0}, crc8[8] = {0}; + /* MCS Field*/ + for (i = 0; i < 7; i++) + sig_bi[i] = (pPMacPktInfo->MCS >> i) & 0x1; + /* Packet BW Setting*/ + sig_bi[7] = pPMacTxInfo->BandWidth; + /* HT-Length Field*/ + for (i = 0; i < 16; i++) + sig_bi[i + 8] = (pPMacTxInfo->PacketLength >> i) & 0x1; + /* Smoothing; 1->allow smoothing*/ + sig_bi[24] = 1; + /*Not Sounding*/ + sig_bi[25] = 1 - pPMacTxInfo->NDP_sound; + /*Reserved bit*/ + sig_bi[26] = 1; + /*/Aggregate*/ + sig_bi[27] = 0; + /*STBC Field*/ + if (pPMacTxInfo->bSTBC) { + sig_bi[28] = 1; + sig_bi[29] = 0; + } else { + sig_bi[28] = 0; + sig_bi[29] = 0; + } + /*Advance Coding, 0: BCC, 1: LDPC*/ + sig_bi[30] = pPMacTxInfo->bLDPC; + /* Short GI*/ + sig_bi[31] = pPMacTxInfo->bSGI; + /* N_ELTFs*/ + if (pPMacTxInfo->NDP_sound == FALSE) { + sig_bi[32] = 0; + sig_bi[33] = 0; + } else { + int N_ELTF = pPMacTxInfo->Ntx - pPMacPktInfo->Nss; + + for (i = 0; i < 2; i++) + sig_bi[32 + i] = (N_ELTF >> i) % 2; + } + /* CRC-8*/ + CRC8_generator(crc8, sig_bi, 34); + + for (i = 0; i < 8; i++) + sig_bi[34 + i] = crc8[i]; + + /*Tail*/ + for (i = 42; i < 48; i++) + sig_bi[i] = 0; + + _rtw_memset(pPMacTxInfo->HT_SIG, 0, 6); + ByteToBit(pPMacTxInfo->HT_SIG, sig_bi, 6); +} + + +/*====================================================================================== + VHT-SIG-A1 + BW Reserved STBC G_ID SU_Nsts P_AID TXOP_PS_NOT_ALLOW Reserved + 2b 1b 1b 6b 3b 9b 1b 2b 1b + VHT-SIG-A2 + SGI SGI_Nsym SU/MU coding LDPC_Extra SU_NCS Beamformed Reserved CRC Tail + 1b 1b 1b 1b 4b 1b 1b 8b 6b +======================================================================================*/ +void VHT_SIG_A_generator( + PRT_PMAC_TX_INFO pPMacTxInfo, + PRT_PMAC_PKT_INFO pPMacPktInfo) +{ + u32 i; + bool sig_bi[48], crc8[8]; + + _rtw_memset(sig_bi, 0, 48); + _rtw_memset(crc8, 0, 8); + + /* BW Setting*/ + for (i = 0; i < 2; i++) + sig_bi[i] = (pPMacTxInfo->BandWidth >> i) & 0x1; + /* Reserved Bit*/ + sig_bi[2] = 1; + /*STBC Field*/ + sig_bi[3] = pPMacTxInfo->bSTBC; + /*Group ID: Single User->A value of 0 or 63 indicates an SU PPDU. */ + for (i = 0; i < 6; i++) + sig_bi[4 + i] = 0; + /* N_STS/Partial AID*/ + for (i = 0; i < 12; i++) { + if (i < 3) + sig_bi[10 + i] = ((pPMacPktInfo->Nsts - 1) >> i) & 0x1; + else + sig_bi[10 + i] = 0; + } + /*TXOP_PS_NOT_ALLPWED*/ + sig_bi[22] = 0; + /*Reserved Bits*/ + sig_bi[23] = 1; + /*Short GI*/ + sig_bi[24] = pPMacTxInfo->bSGI; + if (pPMacTxInfo->bSGI > 0 && (pPMacPktInfo->N_sym % 10) == 9) + sig_bi[25] = 1; + else + sig_bi[25] = 0; + /* SU/MU[0] Coding*/ + sig_bi[26] = pPMacTxInfo->bLDPC; /* 0:BCC, 1:LDPC */ + sig_bi[27] = pPMacPktInfo->SIGA2B3; /*/ Record Extra OFDM Symols is added or not when LDPC is used*/ + /*SU MCS/MU[1-3] Coding*/ + for (i = 0; i < 4; i++) + sig_bi[28 + i] = (pPMacPktInfo->MCS >> i) & 0x1; + /*SU Beamform */ + sig_bi[32] = 0; /*packet.TXBF_en;*/ + /*Reserved Bit*/ + sig_bi[33] = 1; + /*CRC-8*/ + CRC8_generator(crc8, sig_bi, 34); + for (i = 0; i < 8; i++) + sig_bi[34 + i] = crc8[i]; + /*Tail*/ + for (i = 42; i < 48; i++) + sig_bi[i] = 0; + + _rtw_memset(pPMacTxInfo->VHT_SIG_A, 0, 6); + ByteToBit(pPMacTxInfo->VHT_SIG_A, sig_bi, 6); +} + +/*====================================================================================== + VHT-SIG-B + Length Resesrved Trail + 17/19/21 BIT 3/2/2 BIT 6b +======================================================================================*/ +void VHT_SIG_B_generator( + PRT_PMAC_TX_INFO pPMacTxInfo) +{ + bool sig_bi[32], crc8_bi[8]; + u32 i, len, res, tail = 6, total_len, crc8_in_len; + u32 sigb_len; + + _rtw_memset(sig_bi, 0, 32); + _rtw_memset(crc8_bi, 0, 8); + + /*Sounding Packet*/ + if (pPMacTxInfo->NDP_sound == 1) { + if (pPMacTxInfo->BandWidth == 0) { + bool sigb_temp[26] = {0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}; + + _rtw_memcpy(sig_bi, sigb_temp, 26); + } else if (pPMacTxInfo->BandWidth == 1) { + bool sigb_temp[27] = {1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0}; + + _rtw_memcpy(sig_bi, sigb_temp, 27); + } else if (pPMacTxInfo->BandWidth == 2) { + bool sigb_temp[29] = {0, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}; + + _rtw_memcpy(sig_bi, sigb_temp, 29); + } + } else { /* Not NDP Sounding*/ + bool *sigb_temp[29] = {0}; + + if (pPMacTxInfo->BandWidth == 0) { + len = 17; + res = 3; + } else if (pPMacTxInfo->BandWidth == 1) { + len = 19; + res = 2; + } else if (pPMacTxInfo->BandWidth == 2) { + len = 21; + res = 2; + } else { + len = 21; + res = 2; + } + total_len = len + res + tail; + crc8_in_len = len + res; + + /*Length Field*/ + sigb_len = (pPMacTxInfo->PacketLength + 3) >> 2; + + for (i = 0; i < len; i++) + sig_bi[i] = (sigb_len >> i) & 0x1; + /*Reserved Field*/ + for (i = 0; i < res; i++) + sig_bi[len + i] = 1; + /* CRC-8*/ + CRC8_generator(crc8_bi, sig_bi, crc8_in_len); + + /* Tail */ + for (i = 0; i < tail; i++) + sig_bi[len + res + i] = 0; + } + + _rtw_memset(pPMacTxInfo->VHT_SIG_B, 0, 4); + ByteToBit(pPMacTxInfo->VHT_SIG_B, sig_bi, 4); + + pPMacTxInfo->VHT_SIG_B_CRC = 0; + ByteToBit(&(pPMacTxInfo->VHT_SIG_B_CRC), crc8_bi, 1); +} + +/*======================= + VHT Delimiter +=======================*/ +void VHT_Delimiter_generator( + PRT_PMAC_TX_INFO pPMacTxInfo +) +{ + bool sig_bi[32] = {0}, crc8[8] = {0}; + u32 crc8_in_len = 16; + u32 PacketLength = pPMacTxInfo->PacketLength; + int j; + + /* Delimiter[0]: EOF*/ + sig_bi[0] = 1; + /* Delimiter[1]: Reserved*/ + sig_bi[1] = 0; + /* Delimiter[3:2]: MPDU Length High*/ + sig_bi[2] = ((PacketLength - 4) >> 12) % 2; + sig_bi[3] = ((PacketLength - 4) >> 13) % 2; + /* Delimiter[15:4]: MPDU Length Low*/ + for (j = 4; j < 16; j++) + sig_bi[j] = ((PacketLength - 4) >> (j - 4)) % 2; + CRC8_generator(crc8, sig_bi, crc8_in_len); + for (j = 16; j < 24; j++) /* Delimiter[23:16]: CRC 8*/ + sig_bi[j] = crc8[j - 16]; + for (j = 24; j < 32; j++) /* Delimiter[31:24]: Signature ('4E' in Hex, 78 in Dec)*/ + sig_bi[j] = (78 >> (j - 24)) % 2; + + _rtw_memset(pPMacTxInfo->VHT_Delimiter, 0, 4); + ByteToBit(pPMacTxInfo->VHT_Delimiter, sig_bi, 4); +} + +#endif +#endif diff --git a/core/rtw_odm.c b/core/rtw_odm.c new file mode 100644 index 0000000..9651d4f --- /dev/null +++ b/core/rtw_odm.c @@ -0,0 +1,600 @@ +/****************************************************************************** + * + * Copyright(c) 2013 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include + +u32 rtw_phydm_ability_ops(_adapter *adapter, HAL_PHYDM_OPS ops, u32 ability) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter); + struct dm_struct *podmpriv = &pHalData->odmpriv; + u32 result = 0; + + switch (ops) { + case HAL_PHYDM_DIS_ALL_FUNC: + podmpriv->support_ability = DYNAMIC_FUNC_DISABLE; + halrf_cmn_info_set(podmpriv, HALRF_CMNINFO_ABILITY, DYNAMIC_FUNC_DISABLE); + break; + case HAL_PHYDM_FUNC_SET: + podmpriv->support_ability |= ability; + break; + case HAL_PHYDM_FUNC_CLR: + podmpriv->support_ability &= ~(ability); + break; + case HAL_PHYDM_ABILITY_BK: + /* dm flag backup*/ + podmpriv->bk_support_ability = podmpriv->support_ability; + pHalData->bk_rf_ability = halrf_cmn_info_get(podmpriv, HALRF_CMNINFO_ABILITY); + break; + case HAL_PHYDM_ABILITY_RESTORE: + /* restore dm flag */ + podmpriv->support_ability = podmpriv->bk_support_ability; + halrf_cmn_info_set(podmpriv, HALRF_CMNINFO_ABILITY, pHalData->bk_rf_ability); + break; + case HAL_PHYDM_ABILITY_SET: + podmpriv->support_ability = ability; + break; + case HAL_PHYDM_ABILITY_GET: + result = podmpriv->support_ability; + break; + } + return result; +} + +/* set ODM_CMNINFO_IC_TYPE based on chip_type */ +void rtw_odm_init_ic_type(_adapter *adapter) +{ + struct dm_struct *odm = adapter_to_phydm(adapter); + u32 ic_type = chip_type_to_odm_ic_type(rtw_get_chip_type(adapter)); + + rtw_warn_on(!ic_type); + + odm_cmn_info_init(odm, ODM_CMNINFO_IC_TYPE, ic_type); +} + +void rtw_odm_adaptivity_ver_msg(void *sel, _adapter *adapter) +{ + RTW_PRINT_SEL(sel, "ADAPTIVITY_VERSION "ADAPTIVITY_VERSION"\n"); +} + +#define RTW_ADAPTIVITY_EN_DISABLE 0 +#define RTW_ADAPTIVITY_EN_ENABLE 1 + +void rtw_odm_adaptivity_en_msg(void *sel, _adapter *adapter) +{ + struct registry_priv *regsty = &adapter->registrypriv; + + RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_EN_"); + + if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_DISABLE) + _RTW_PRINT_SEL(sel, "DISABLE\n"); + else if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_ENABLE) + _RTW_PRINT_SEL(sel, "ENABLE\n"); + else + _RTW_PRINT_SEL(sel, "INVALID\n"); +} + +#define RTW_ADAPTIVITY_MODE_NORMAL 0 +#define RTW_ADAPTIVITY_MODE_CARRIER_SENSE 1 + +void rtw_odm_adaptivity_mode_msg(void *sel, _adapter *adapter) +{ + struct registry_priv *regsty = &adapter->registrypriv; + + RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_MODE_"); + + if (regsty->adaptivity_mode == RTW_ADAPTIVITY_MODE_NORMAL) + _RTW_PRINT_SEL(sel, "NORMAL\n"); + else if (regsty->adaptivity_mode == RTW_ADAPTIVITY_MODE_CARRIER_SENSE) + _RTW_PRINT_SEL(sel, "CARRIER_SENSE\n"); + else + _RTW_PRINT_SEL(sel, "INVALID\n"); +} + +void rtw_odm_adaptivity_config_msg(void *sel, _adapter *adapter) +{ + rtw_odm_adaptivity_ver_msg(sel, adapter); + rtw_odm_adaptivity_en_msg(sel, adapter); + rtw_odm_adaptivity_mode_msg(sel, adapter); +} + +bool rtw_odm_adaptivity_needed(_adapter *adapter) +{ + struct registry_priv *regsty = &adapter->registrypriv; + bool ret = _FALSE; + + if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_ENABLE) + ret = _TRUE; + + return ret; +} + +void rtw_odm_adaptivity_parm_msg(void *sel, _adapter *adapter) +{ + struct dm_struct *odm = adapter_to_phydm(adapter); + + rtw_odm_adaptivity_config_msg(sel, adapter); + + RTW_PRINT_SEL(sel, "%10s %16s\n" + , "th_l2h_ini", "th_edcca_hl_diff"); + RTW_PRINT_SEL(sel, "0x%-8x %-16d\n" + , (u8)odm->th_l2h_ini + , odm->th_edcca_hl_diff + ); +} + +void rtw_odm_adaptivity_parm_set(_adapter *adapter, s8 th_l2h_ini, s8 th_edcca_hl_diff) +{ + struct dm_struct *odm = adapter_to_phydm(adapter); + + odm->th_l2h_ini = th_l2h_ini; + odm->th_edcca_hl_diff = th_edcca_hl_diff; +} + +void rtw_odm_get_perpkt_rssi(void *sel, _adapter *adapter) +{ + struct dm_struct *odm = adapter_to_phydm(adapter); + + RTW_PRINT_SEL(sel, "rx_rate = %s, rssi_a = %d(%%), rssi_b = %d(%%)\n", + HDATA_RATE(odm->rx_rate), odm->rssi_a, odm->rssi_b); +} + + +void rtw_odm_acquirespinlock(_adapter *adapter, enum rt_spinlock_type type) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(adapter); + _irqL irqL; + + switch (type) { + case RT_IQK_SPINLOCK: + _enter_critical_bh(&pHalData->IQKSpinLock, &irqL); + default: + break; + } +} + +void rtw_odm_releasespinlock(_adapter *adapter, enum rt_spinlock_type type) +{ + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(adapter); + _irqL irqL; + + switch (type) { + case RT_IQK_SPINLOCK: + _exit_critical_bh(&pHalData->IQKSpinLock, &irqL); + default: + break; + } +} + +s16 rtw_odm_get_tx_power_mbm(struct dm_struct *dm, u8 rfpath, u8 rate, u8 bw, u8 cch) +{ + return phy_get_txpwr_single_mbm(dm->adapter, rfpath, mgn_rate_to_rs(rate), rate, bw, cch, 0, 0, 0, NULL); +} + +#ifdef CONFIG_DFS_MASTER +inline void rtw_odm_radar_detect_reset(_adapter *adapter) +{ + phydm_radar_detect_reset(adapter_to_phydm(adapter)); +} + +inline void rtw_odm_radar_detect_disable(_adapter *adapter) +{ + phydm_radar_detect_disable(adapter_to_phydm(adapter)); +} + +/* called after ch, bw is set */ +inline void rtw_odm_radar_detect_enable(_adapter *adapter) +{ + phydm_radar_detect_enable(adapter_to_phydm(adapter)); +} + +inline BOOLEAN rtw_odm_radar_detect(_adapter *adapter) +{ + return phydm_radar_detect(adapter_to_phydm(adapter)); +} + +static enum phydm_dfs_region_domain _rtw_dfs_regd_to_phydm[] = { + [RTW_DFS_REGD_NONE] = PHYDM_DFS_DOMAIN_UNKNOWN, + [RTW_DFS_REGD_FCC] = PHYDM_DFS_DOMAIN_FCC, + [RTW_DFS_REGD_MKK] = PHYDM_DFS_DOMAIN_MKK, + [RTW_DFS_REGD_ETSI] = PHYDM_DFS_DOMAIN_ETSI, +}; + +#define rtw_dfs_regd_to_phydm(region) (((region) >= RTW_DFS_REGD_NUM) ? _rtw_dfs_regd_to_phydm[RTW_DFS_REGD_NONE] : _rtw_dfs_regd_to_phydm[(region)]) + +void rtw_odm_update_dfs_region(struct dvobj_priv *dvobj) +{ + odm_cmn_info_init(dvobj_to_phydm(dvobj), ODM_CMNINFO_DFS_REGION_DOMAIN, rtw_dfs_regd_to_phydm(rtw_rfctl_get_dfs_domain(dvobj_to_rfctl(dvobj)))); +} + +inline u8 rtw_odm_radar_detect_polling_int_ms(struct dvobj_priv *dvobj) +{ + return phydm_dfs_polling_time(dvobj_to_phydm(dvobj)); +} +#endif /* CONFIG_DFS_MASTER */ + +void rtw_odm_parse_rx_phy_status_chinfo(union recv_frame *rframe, u8 *phys) +{ +#ifndef DBG_RX_PHYSTATUS_CHINFO +#define DBG_RX_PHYSTATUS_CHINFO 0 +#endif + +#if (ODM_PHY_STATUS_NEW_TYPE_SUPPORT == 1) + _adapter *adapter = rframe->u.hdr.adapter; + struct dm_struct *phydm = adapter_to_phydm(adapter); + struct rx_pkt_attrib *attrib = &rframe->u.hdr.attrib; + u8 *wlanhdr = get_recvframe_data(rframe); + + if (phydm->support_ic_type & PHYSTS_2ND_TYPE_IC) { + /* + * 8723D: + * type_0(CCK) + * l_rxsc + * is filled with primary channel SC, not real rxsc. + * 0:LSC, 1:USC + * type_1(OFDM) + * rf_mode + * RF bandwidth when RX + * l_rxsc(legacy), ht_rxsc + * see below RXSC N-series + * type_2(Not used) + */ + /* + * 8821C, 8822B: + * type_0(CCK) + * l_rxsc + * is filled with primary channel SC, not real rxsc. + * 0:LSC, 1:USC + * type_1(OFDM) + * rf_mode + * RF bandwidth when RX + * l_rxsc(legacy), ht_rxsc + * see below RXSC AC-series + * type_2(Not used) + */ + + if ((*phys & 0xf) == 0) { + struct phy_sts_rpt_jgr2_type0 *phys_t0 = (struct phy_sts_rpt_jgr2_type0 *)phys; + + if (DBG_RX_PHYSTATUS_CHINFO) { + RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, l_rxsc:%u)\n" + , *phys & 0xf + , MAC_ARG(get_ta(wlanhdr)) + , is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC" + , HDATA_RATE(attrib->data_rate) + , phys_t0->band, phys_t0->channel, phys_t0->rxsc + ); + } + + } else if ((*phys & 0xf) == 1) { + struct phy_sts_rpt_jgr2_type1 *phys_t1 = (struct phy_sts_rpt_jgr2_type1 *)phys; + u8 rxsc = (attrib->data_rate > DESC_RATE11M && attrib->data_rate < DESC_RATEMCS0) ? phys_t1->l_rxsc : phys_t1->ht_rxsc; + u8 pkt_cch = 0; + u8 pkt_bw = CHANNEL_WIDTH_20; + + #if ODM_IC_11N_SERIES_SUPPORT + if (phydm->support_ic_type & ODM_IC_11N_SERIES) { + /* RXSC N-series */ + #define RXSC_DUP 0 + #define RXSC_LSC 1 + #define RXSC_USC 2 + #define RXSC_40M 3 + + static const s8 cch_offset_by_rxsc[4] = {0, -2, 2, 0}; + + if (phys_t1->rf_mode == 0) { + pkt_cch = phys_t1->channel; + pkt_bw = CHANNEL_WIDTH_20; + } else if (phys_t1->rf_mode == 1) { + if (rxsc == RXSC_LSC || rxsc == RXSC_USC) { + pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc]; + pkt_bw = CHANNEL_WIDTH_20; + } else if (rxsc == RXSC_40M) { + pkt_cch = phys_t1->channel; + pkt_bw = CHANNEL_WIDTH_40; + } + } else + rtw_warn_on(1); + + goto type1_end; + } + #endif /* ODM_IC_11N_SERIES_SUPPORT */ + + #if ODM_IC_11AC_SERIES_SUPPORT + if (phydm->support_ic_type & ODM_IC_11AC_SERIES) { + /* RXSC AC-series */ + #define RXSC_DUP 0 /* 0: RX from all SC of current rf_mode */ + + #define RXSC_LL20M_OF_160M 8 /* 1~8: RX from 20MHz SC */ + #define RXSC_L20M_OF_160M 6 + #define RXSC_L20M_OF_80M 4 + #define RXSC_L20M_OF_40M 2 + #define RXSC_U20M_OF_40M 1 + #define RXSC_U20M_OF_80M 3 + #define RXSC_U20M_OF_160M 5 + #define RXSC_UU20M_OF_160M 7 + + #define RXSC_L40M_OF_160M 12 /* 9~12: RX from 40MHz SC */ + #define RXSC_L40M_OF_80M 10 + #define RXSC_U40M_OF_80M 9 + #define RXSC_U40M_OF_160M 11 + + #define RXSC_L80M_OF_160M 14 /* 13~14: RX from 80MHz SC */ + #define RXSC_U80M_OF_160M 13 + + static const s8 cch_offset_by_rxsc[15] = {0, 2, -2, 6, -6, 10, -10, 14, -14, 4, -4, 12, -12, 8, -8}; + + if (phys_t1->rf_mode == 0) { + /* RF 20MHz */ + pkt_cch = phys_t1->channel; + pkt_bw = CHANNEL_WIDTH_20; + goto type1_end; + } + + if (rxsc == 0) { + /* RF and RX with same BW */ + if (attrib->data_rate >= DESC_RATEMCS0) { + pkt_cch = phys_t1->channel; + pkt_bw = phys_t1->rf_mode; + } + goto type1_end; + } + + if ((phys_t1->rf_mode == 1 && rxsc >= 1 && rxsc <= 2) /* RF 40MHz, RX 20MHz */ + || (phys_t1->rf_mode == 2 && rxsc >= 1 && rxsc <= 4) /* RF 80MHz, RX 20MHz */ + || (phys_t1->rf_mode == 3 && rxsc >= 1 && rxsc <= 8) /* RF 160MHz, RX 20MHz */ + ) { + pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc]; + pkt_bw = CHANNEL_WIDTH_20; + } else if ((phys_t1->rf_mode == 2 && rxsc >= 9 && rxsc <= 10) /* RF 80MHz, RX 40MHz */ + || (phys_t1->rf_mode == 3 && rxsc >= 9 && rxsc <= 12) /* RF 160MHz, RX 40MHz */ + ) { + if (attrib->data_rate >= DESC_RATEMCS0) { + pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc]; + pkt_bw = CHANNEL_WIDTH_40; + } + } else if ((phys_t1->rf_mode == 3 && rxsc >= 13 && rxsc <= 14) /* RF 160MHz, RX 80MHz */ + ) { + if (attrib->data_rate >= DESC_RATEMCS0) { + pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc]; + pkt_bw = CHANNEL_WIDTH_80; + } + } else + rtw_warn_on(1); + + } + #endif /* ODM_IC_11AC_SERIES_SUPPORT */ + +type1_end: + if (DBG_RX_PHYSTATUS_CHINFO) { + RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, rf_mode:%u, l_rxsc:%u, ht_rxsc:%u) => %u,%u\n" + , *phys & 0xf + , MAC_ARG(get_ta(wlanhdr)) + , is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC" + , HDATA_RATE(attrib->data_rate) + , phys_t1->band, phys_t1->channel, phys_t1->rf_mode, phys_t1->l_rxsc, phys_t1->ht_rxsc + , pkt_cch, pkt_bw + ); + } + + /* for now, only return cneter channel of 20MHz packet */ + if (pkt_cch && pkt_bw == CHANNEL_WIDTH_20) + attrib->ch = pkt_cch; + + } else { + struct phy_sts_rpt_jgr2_type2 *phys_t2 = (struct phy_sts_rpt_jgr2_type2 *)phys; + + if (DBG_RX_PHYSTATUS_CHINFO) { + RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, l_rxsc:%u, ht_rxsc:%u)\n" + , *phys & 0xf + , MAC_ARG(get_ta(wlanhdr)) + , is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC" + , HDATA_RATE(attrib->data_rate) + , phys_t2->band, phys_t2->channel, phys_t2->l_rxsc, phys_t2->ht_rxsc + ); + } + } + } +#endif /* (ODM_PHY_STATUS_NEW_TYPE_SUPPORT == 1) */ + +} + +#if defined(CONFIG_RTL8822C) && defined(CONFIG_LPS_PG) +void +debug_DACK( + struct dm_struct *dm +) +{ + //P_PHYDM_FUNC dm; + //dm = &(SysMib.ODM.Phydm); + //PIQK_OFFLOAD_PARM pIQK_info; + //pIQK_info= &(SysMib.ODM.IQKParm); + u8 i; + u32 temp1, temp2, temp3; + + temp1 = odm_get_bb_reg(dm, 0x1860, bMaskDWord); + temp2 = odm_get_bb_reg(dm, 0x4160, bMaskDWord); + temp3 = odm_get_bb_reg(dm, 0x9b4, bMaskDWord); + + odm_set_bb_reg(dm, 0x9b4, bMaskDWord, 0xdb66db00); + + //pathA + odm_set_bb_reg(dm, 0x1830, BIT(30), 0x0); + odm_set_bb_reg(dm, 0x1860, 0xfc000000, 0x3c); + + RTW_INFO("path A i\n"); + //i + for (i = 0; i < 0xf; i++) { + odm_set_bb_reg(dm, 0x18b0, 0xf0000000, i); + RTW_INFO("[0][0][%d] = 0x%08x\n", i, (u16)odm_get_bb_reg(dm,0x2810,0x7fc0000)); + //pIQK_info->msbk_d[0][0][i] = (u16)odm_get_bb_reg(dm,0x2810,0x7fc0000); + } + RTW_INFO("path A q\n"); + //q + for (i = 0; i < 0xf; i++) { + odm_set_bb_reg(dm, 0x18cc, 0xf0000000, i); + RTW_INFO("[0][1][%d] = 0x%08x\n", i, (u16)odm_get_bb_reg(dm,0x283c,0x7fc0000)); + //pIQK_info->msbk_d[0][1][i] = (u16)odm_get_bb_reg(dm,0x283c,0x7fc0000); + } + //pathB + odm_set_bb_reg(dm, 0x4130, BIT(30), 0x0); + odm_set_bb_reg(dm, 0x4160, 0xfc000000, 0x3c); + + RTW_INFO("\npath B i\n"); + //i + for (i = 0; i < 0xf; i++) { + odm_set_bb_reg(dm, 0x41b0, 0xf0000000, i); + RTW_INFO("[1][0][%d] = 0x%08x\n", i, (u16)odm_get_bb_reg(dm,0x4510,0x7fc0000)); + //pIQK_info->msbk_d[1][0][i] = (u16)odm_get_bb_reg(dm,0x2810,0x7fc0000); + } + RTW_INFO("path B q\n"); + //q + for (i = 0; i < 0xf; i++) { + odm_set_bb_reg(dm, 0x41cc, 0xf0000000, i); + RTW_INFO("[1][1][%d] = 0x%08x\n", i, (u16)odm_get_bb_reg(dm,0x453c,0x7fc0000)); + //pIQK_info->msbk_d[1][1][i] = (u16)odm_get_bb_reg(dm,0x283c,0x7fc0000); + } + + //restore to normal + odm_set_bb_reg(dm, 0x1830, BIT(30), 0x1); + odm_set_bb_reg(dm, 0x4130, BIT(30), 0x1); + odm_set_bb_reg(dm, 0x1860, bMaskDWord, temp1); + odm_set_bb_reg(dm, 0x4160, bMaskDWord, temp2); + odm_set_bb_reg(dm, 0x9b4, bMaskDWord, temp3); + + +} + +void +debug_IQK( + struct dm_struct *dm, + IN u8 idx, + IN u8 path +) +{ + u8 i, ch; + u32 tmp; + u32 bit_mask_20_16 = BIT(20) | BIT(19) | BIT(18) | BIT(17) | BIT(16); + + RTW_INFO("idx = %d, path = %d\n", idx, path); + + odm_set_bb_reg(dm, 0x1b00, MASKDWORD, 0x8 | path << 1); + + if (idx == TX_IQK) {//TXCFIR + odm_set_bb_reg(dm, R_0x1b20, BIT(31) | BIT(30), 0x3); + } else {//RXCFIR + odm_set_bb_reg(dm, R_0x1b20, BIT(31) | BIT(30), 0x1); + } + odm_set_bb_reg(dm, R_0x1bd4, BIT(21), 0x1); + odm_set_bb_reg(dm, R_0x1bd4, bit_mask_20_16, 0x10); + for (i = 0; i <= 16; i++) { + odm_set_bb_reg(dm, R_0x1bd8, MASKDWORD, 0xe0000001 | i << 2); + tmp = odm_get_bb_reg(dm, R_0x1bfc, MASKDWORD); + RTW_INFO("iqk_cfir_real[%d][%d][%d] = 0x%x\n", path, idx, i, ((tmp & 0x0fff0000) >> 16)); + //iqk_info->iqk_cfir_real[ch][path][idx][i] = + // (tmp & 0x0fff0000) >> 16; + RTW_INFO("iqk_cfir_imag[%d][%d][%d] = 0x%x\n", path, idx, i, (tmp & 0x0fff)); + //iqk_info->iqk_cfir_imag[ch][path][idx][i] = tmp & 0x0fff; + } + odm_set_bb_reg(dm, R_0x1b20, BIT(31) | BIT(30), 0x0); + //odm_set_bb_reg(dm, R_0x1bd8, MASKDWORD, 0x0); +} + +__odm_func__ void +debug_information_8822c( + struct dm_struct *dm) +{ + struct dm_dpk_info *dpk_info = &dm->dpk_info; + + u32 reg_rf18; + + if (odm_get_bb_reg(dm, R_0x1e7c, BIT(30))) + dpk_info->is_tssi_mode = true; + else + dpk_info->is_tssi_mode = false; + + reg_rf18 = odm_get_rf_reg(dm, RF_PATH_A, RF_0x18, RFREG_MASK); + + dpk_info->dpk_band = (u8)((reg_rf18 & BIT(16)) >> 16); /*0/1:G/A*/ + dpk_info->dpk_ch = (u8)reg_rf18 & 0xff; + dpk_info->dpk_bw = (u8)((reg_rf18 & 0x3000) >> 12); /*3/2/1:20/40/80*/ + + RTW_INFO("[DPK] TSSI/ Band/ CH/ BW = %d / %s / %d / %s\n", + dpk_info->is_tssi_mode, dpk_info->dpk_band == 0 ? "2G" : "5G", + dpk_info->dpk_ch, + dpk_info->dpk_bw == 3 ? "20M" : (dpk_info->dpk_bw == 2 ? "40M" : "80M")); +} + +extern void _dpk_get_coef_8822c(void *dm_void, u8 path); + +__odm_func__ void +debug_reload_data_8822c( + void *dm_void) +{ + struct dm_struct *dm = (struct dm_struct *)dm_void; + struct dm_dpk_info *dpk_info = &dm->dpk_info; + + u8 path; + u32 u32tmp; + + debug_information_8822c(dm); + + for (path = 0; path < DPK_RF_PATH_NUM_8822C; path++) { + + RTW_INFO("[DPK] Reload path: 0x%x\n", path); + + odm_set_bb_reg(dm, R_0x1b00, MASKDWORD, 0x8 | (path << 1)); + + /*txagc bnd*/ + if (dpk_info->dpk_band == 0x0) + u32tmp = odm_get_bb_reg(dm, R_0x1b60, MASKDWORD); + else + u32tmp = odm_get_bb_reg(dm, R_0x1b60, MASKDWORD); + + RTW_INFO("[DPK] txagc bnd = 0x%08x\n", u32tmp); + + u32tmp = odm_get_bb_reg(dm, R_0x1b64, MASKBYTE3); + RTW_INFO("[DPK] dpk_txagc = 0x%08x\n", u32tmp); + + //debug_coef_write_8822c(dm, path, dpk_info->dpk_path_ok & BIT(path) >> path); + _dpk_get_coef_8822c(dm, path); + + //debug_one_shot_8822c(dm, path, DPK_ON); + + odm_set_bb_reg(dm, R_0x1b00, 0x0000000f, 0xc); + + if (path == RF_PATH_A) + u32tmp = odm_get_bb_reg(dm, R_0x1b04, 0x0fffffff); + else + u32tmp = odm_get_bb_reg(dm, R_0x1b5c, 0x0fffffff); + + RTW_INFO("[DPK] dpk_gs = 0x%08x\n", u32tmp); + + } +} + +void odm_lps_pg_debug_8822c(void *dm_void) +{ + struct dm_struct *dm = (struct dm_struct *)dm_void; + + debug_DACK(dm); + debug_IQK(dm, TX_IQK, RF_PATH_A); + debug_IQK(dm, RX_IQK, RF_PATH_A); + debug_IQK(dm, TX_IQK, RF_PATH_B); + debug_IQK(dm, RX_IQK, RF_PATH_B); + debug_reload_data_8822c(dm); +} +#endif /* defined(CONFIG_RTL8822C) && defined(CONFIG_LPS_PG) */ + diff --git a/core/rtw_p2p.c b/core/rtw_p2p.c new file mode 100644 index 0000000..24372de --- /dev/null +++ b/core/rtw_p2p.c @@ -0,0 +1,5017 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_P2P_C_ + +#include + +#ifdef CONFIG_P2P + +int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8 *ch_list, u8 ch_cnt) +{ + int found = 0, i = 0; + + for (i = 0; i < ch_cnt; i++) { + if (ch_list[i] == desired_ch) { + found = 1; + break; + } + } + return found ; +} + +int is_any_client_associated(_adapter *padapter) +{ + return padapter->stapriv.asoc_list_cnt ? _TRUE : _FALSE; +} + +static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + _irqL irqL; + _list *phead, *plist; + u32 len = 0; + u16 attr_len = 0; + u8 tmplen, *pdata_attr, *pstart, *pcur; + struct sta_info *psta = NULL; + _adapter *padapter = pwdinfo->padapter; + struct sta_priv *pstapriv = &padapter->stapriv; + + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN); + + if (NULL == pdata_attr) { + RTW_INFO("%s pdata_attr malloc failed\n", __FUNCTION__); + goto _exit; + } + + pstart = pdata_attr; + pcur = pdata_attr; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* look up sta asoc_queue */ + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + + plist = get_next(plist); + + + if (psta->is_p2p_device) { + tmplen = 0; + + pcur++; + + /* P2P device address */ + _rtw_memcpy(pcur, psta->dev_addr, ETH_ALEN); + pcur += ETH_ALEN; + + /* P2P interface address */ + _rtw_memcpy(pcur, psta->cmn.mac_addr, ETH_ALEN); + pcur += ETH_ALEN; + + *pcur = psta->dev_cap; + pcur++; + + /* *(u16*)(pcur) = cpu_to_be16(psta->config_methods); */ + RTW_PUT_BE16(pcur, psta->config_methods); + pcur += 2; + + _rtw_memcpy(pcur, psta->primary_dev_type, 8); + pcur += 8; + + *pcur = psta->num_of_secdev_type; + pcur++; + + _rtw_memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type * 8); + pcur += psta->num_of_secdev_type * 8; + + if (psta->dev_name_len > 0) { + /* *(u16*)(pcur) = cpu_to_be16( WPS_ATTR_DEVICE_NAME ); */ + RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME); + pcur += 2; + + /* *(u16*)(pcur) = cpu_to_be16( psta->dev_name_len ); */ + RTW_PUT_BE16(pcur, psta->dev_name_len); + pcur += 2; + + _rtw_memcpy(pcur, psta->dev_name, psta->dev_name_len); + pcur += psta->dev_name_len; + } + + + tmplen = (u8)(pcur - pstart); + + *pstart = (tmplen - 1); + + attr_len += tmplen; + + /* pstart += tmplen; */ + pstart = pcur; + + } + + + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + if (attr_len > 0) + len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr); + + rtw_mfree(pdata_attr, MAX_P2P_IE_LEN); + +_exit: + return len; + +} + +static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + _adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_GO_DISC_REQUEST; + u8 dialogToken = 0; + + RTW_INFO("[%s]\n", __FUNCTION__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* Build P2P action frame header */ + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + /* there is no IE in this P2P action frame */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + +} + +static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + _adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_DEVDISC_RESP; + u8 p2pie[8] = { 0x00 }; + u32 p2pielen = 0; + + RTW_INFO("[%s]\n", __FUNCTION__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* Build P2P public action frame header */ + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + + /* Build P2P IE */ + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* P2P_ATTR_STATUS */ + p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status); + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + +} + +static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8 *raddr, u8 *frame_body, u16 config_method) +{ + _adapter *padapter = pwdinfo->padapter; + unsigned char category = RTW_WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u8 dialogToken = frame_body[7]; /* The Dialog Token of provisioning discovery request frame. */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PROVISION_DISC_RESP; + u8 wpsie[100] = { 0x00 }; + u8 wpsielen = 0; +#ifdef CONFIG_WFD + u32 wfdielen = 0; +#endif + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + wpsielen = 0; + /* WPS OUI */ + /* *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI ); */ + RTW_PUT_BE32(wpsie, WPSOUI); + wpsielen += 4; + +#if 0 + /* WPS version */ + /* Type: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1); + wpsielen += 2; + + /* Length: */ + *(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001); + wpsielen += 2; + + /* Value: */ + wpsie[wpsielen++] = WPS_VERSION_1; /* Version 1.0 */ +#endif + + /* Config Method */ + /* Type: */ + /* *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD ); */ + RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD); + wpsielen += 2; + + /* Length: */ + /* *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 ); */ + RTW_PUT_BE16(wpsie + wpsielen, 0x0002); + wpsielen += 2; + + /* Value: */ + /* *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( config_method ); */ + RTW_PUT_BE16(wpsie + wpsielen, config_method); + wpsielen += 2; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + +#ifdef CONFIG_WFD + wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + + return; + +} + +static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + _adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PRESENCE_RESPONSE; + u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 }; + u8 noa_attr_content[32] = { 0x00 }; + u32 p2pielen = 0; + + RTW_INFO("[%s]\n", __FUNCTION__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* Build P2P action frame header */ + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen)); + + + /* Add P2P IE header */ + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Add Status attribute in P2P IE */ + p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status); + + /* Add NoA attribute in P2P IE */ + noa_attr_content[0] = 0x1;/* index */ + noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */ + + /* todo: Notice of Absence Descriptor(s) */ + + p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content); + + + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &(pattrib->pktlen)); + + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + +} + +u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 }; + u16 capability = 0; + u32 len = 0, p2pielen = 0; + + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + + /* According to the P2P Specification, the beacon frame should contain 3 P2P attributes */ + /* 1. P2P Capability */ + /* 2. P2P Device ID */ + /* 3. Notice of Absence ( NOA ) */ + + /* P2P Capability ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + /* Be able to participate in additional P2P Groups and */ + /* support the P2P Invitation Procedure */ + /* Group Capability Bitmap, 1 byte */ + capability = P2P_DEVCAP_INVITATION_PROC | P2P_DEVCAP_CLIENT_DISCOVERABILITY; + capability |= ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8); + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING)) + capability |= (P2P_GRPCAP_GROUP_FORMATION << 8); + + capability = cpu_to_le16(capability); + + p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8 *)&capability); + + + /* P2P Device ID ATTR */ + p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr); + + + /* Notice of Absence ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + + /* go_add_noa_attr(pwdinfo); */ + + + pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + + return len; + +} + +#ifdef CONFIG_WFD +u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u16 val16 = 0; + u32 len = 0, wfdielen = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the beacon frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + + if (P2P_ROLE_GO == pwdinfo->role) { + if (is_any_client_associated(pwdinfo->padapter)) { + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + } else { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + } + + } else { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u16 val16 = 0; + u32 len = 0, wfdielen = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + + if (1 == pwdinfo->wfd_tdls_enable) { + /* WFD primary sink + available for WFD session + WiFi TDLS mode + WSC ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD | + WFD_DEVINFO_PC_TDLS; + RTW_PUT_BE16(wfdie + wfdielen, val16); + } else { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSC ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + u16 v16 = 0; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe response frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + /* 4. WFD Session Information */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode */ + + if (_TRUE == pwdinfo->session_available) { + if (P2P_ROLE_GO == pwdinfo->role) { + if (is_any_client_associated(pwdinfo->padapter)) { + if (pwdinfo->wfd_tdls_enable) { + /* TDLS mode + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } else { + /* WiFi Direct mode + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } + } else { + if (pwdinfo->wfd_tdls_enable) { + /* available for WFD session + TDLS mode + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } else { + /* available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } + } + } else { + if (pwdinfo->wfd_tdls_enable) { + /* available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } else { + /* available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } + } + } else { + if (pwdinfo->wfd_tdls_enable) { + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } else { + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT; + RTW_PUT_BE16(wfdie + wfdielen, v16); + } + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0000); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } +#ifdef CONFIG_CONCURRENT_MODE +#ifdef CONFIG_TDLS + { + int i; + _adapter *iface = NULL; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + if (iface == padapter) + continue; + + if ((tunneled == 0) && (iface->wdinfo.wfd_tdls_enable == 1)) { + /* Alternative MAC Address ATTR + Type: */ + wfdie[wfdielen++] = WFD_ATTR_ALTER_MAC; + + /* Length: + Note: In the WFD specification, the size of length field is 2.*/ + RTW_PUT_BE16(wfdie + wfdielen, ETH_ALEN); + wfdielen += 2; + + /* Value: + Alternative MAC Address*/ + _rtw_memcpy(wfdie + wfdielen, adapter_mac_addr(iface), ETH_ALEN); + wfdielen += ETH_ALEN; + } + } + } + } + +#endif /* CONFIG_TDLS*/ +#endif /* CONFIG_CONCURRENT_MODE */ + + pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u16 val16 = 0; + u32 len = 0, wfdielen = 0; + _adapter *padapter = NULL; + struct mlme_priv *pmlmepriv = NULL; + struct wifi_display_info *pwfd_info = NULL; + + padapter = pwdinfo->padapter; + pmlmepriv = &padapter->mlmepriv; + pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + + pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + if (P2P_ROLE_GO == pwdinfo->role) { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0000); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u16 val16 = 0; + u32 len = 0, wfdielen = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + if (P2P_ROLE_GO == pwdinfo->role) { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0000); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} + +u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + u16 val16 = 0; + _adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->wdinfo.wfd_info; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + goto exit; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID ( Optional ) */ + /* 3. Local IP Adress ( Optional ) */ + + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */ + val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, val16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0007); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[wfdielen++] = 0; + /* MAC Addr. */ + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + wfdie[wfdielen++] = 0; + + rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + +exit: + return len; +} +#endif /* CONFIG_WFD */ + +u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 }; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20100907 */ + /* According to the P2P Specification, the probe response frame should contain 5 P2P attributes */ + /* 1. P2P Capability */ + /* 2. Extended Listen Timing */ + /* 3. Notice of Absence ( NOA ) ( Only GO needs this ) */ + /* 4. Device Info */ + /* 5. Group Info ( Only GO need this ) */ + + /* P2P Capability ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 ); */ + RTW_PUT_LE16(p2pie + p2pielen, 0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + p2pie[p2pielen] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING)) + p2pie[p2pielen] |= P2P_GRPCAP_GROUP_FORMATION; + + p2pielen++; + } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) { + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT; + else + p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT; + } + + /* Extended Listen Timing ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING; + + /* Length: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 ); */ + RTW_PUT_LE16(p2pie + p2pielen, 0x0004); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF ); */ + RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF); + p2pielen += 2; + + /* Availability Interval */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF ); */ + RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF); + p2pielen += 2; + + + /* Notice of Absence ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* go_add_noa_attr(pwdinfo); */ + } + + /* Device Info ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len ); */ + RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm ); */ + RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm); + p2pielen += 2; + + { + /* Primary Device Type */ + /* Category ID */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + /* *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI ); */ + RTW_PUT_BE32(p2pie + p2pielen, WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + } + + /* Number of Secondary Device Types */ + p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len ); */ + RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + /* Group Info ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen); + + + pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + + return len; + +} + +u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 *pssid, u8 ussidlen, u8 *pdev_raddr) +{ + u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 }; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20110301 */ + /* According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */ + /* 1. P2P Capability */ + /* 2. Device Info */ + /* 3. Group ID ( When joining an operating P2P Group ) */ + + /* P2P Capability ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_CAPABILITY; + + /* Length: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 ); */ + RTW_PUT_LE16(p2pie + p2pielen, 0x0002); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT; + else + p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT; + + + /* Device Info ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len ); */ + RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) { + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC); + } else { + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY); + } + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA); + p2pielen += 2; + + /* OUI */ + /* *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI ); */ + RTW_PUT_BE32(p2pie + p2pielen, WPSOUI); + p2pielen += 4; + + /* Sub Category ID */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[p2pielen++] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME ); */ + RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME); + p2pielen += 2; + + /* Length: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len ); */ + RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) { + /* Added by Albert 2011/05/19 */ + /* In this case, the pdev_raddr is the device address of the group owner. */ + + /* P2P Group ID ATTR */ + /* Type: */ + p2pie[p2pielen++] = P2P_ATTR_GROUP_ID; + + /* Length: */ + /* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + ussidlen ); */ + RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen); + p2pielen += 2; + + /* Value: */ + _rtw_memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN); + p2pielen += ETH_ALEN; + + _rtw_memcpy(p2pie + p2pielen, pssid, ussidlen); + p2pielen += ussidlen; + + } + + pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + + return len; + +} + + +u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code) +{ + u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 }; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[p2pielen++] = 0x50; + p2pie[p2pielen++] = 0x6F; + p2pie[p2pielen++] = 0x9A; + p2pie[p2pielen++] = 0x09; /* WFA P2P v1.0 */ + + /* According to the P2P Specification, the Association response frame should contain 2 P2P attributes */ + /* 1. Status */ + /* 2. Extended Listen Timing (optional) */ + + + /* Status ATTR */ + p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code); + + + /* Extended Listen Timing ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + + + pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; + +} + +u32 build_deauth_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u32 len = 0; + + return len; +} + +u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *p; + u32 ret = _FALSE; + u8 *p2pie; + u32 p2pielen = 0; + int ssid_len = 0, rate_cnt = 0; + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + if (rate_cnt <= 4) { + int i, g_rate = 0; + + for (i = 0; i < rate_cnt; i++) { + if (((*(p + 2 + i) & 0xff) != 0x02) && + ((*(p + 2 + i) & 0xff) != 0x04) && + ((*(p + 2 + i) & 0xff) != 0x0B) && + ((*(p + 2 + i) & 0xff) != 0x16)) + g_rate = 1; + } + + if (g_rate == 0) { + /* There is no OFDM rate included in SupportedRates IE of this probe request frame */ + /* The driver should response this probe request. */ + return ret; + } + } else { + /* rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */ + /* We should proceed the following check for this probe request. */ + } + + /* Added comments by Albert 20100906 */ + /* There are several items we should check here. */ + /* 1. This probe request frame must contain the P2P IE. (Done) */ + /* 2. This probe request frame must contain the wildcard SSID. (Done) */ + /* 3. Wildcard BSSID. (Todo) */ + /* 4. Destination Address. ( Done in mgt_dispatcher function ) */ + /* 5. Requested Device Type in WSC IE. (Todo) */ + /* 6. Device ID attribute in P2P IE. (Todo) */ + + p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len, + len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_); + + ssid_len &= 0xff; /* Just last 1 byte is valid for ssid len of the probe request */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , NULL, &p2pielen); + if (p2pie) { + if ((p != NULL) && _rtw_memcmp((void *)(p + 2), (void *) pwdinfo->p2p_wildcard_ssid , 7)) { + /* todo: */ + /* Check Requested Device Type attributes in WSC IE. */ + /* Check Device ID attribute in P2P IE */ + + ret = _TRUE; + } else if ((p != NULL) && (ssid_len == 0)) + ret = _TRUE; + } else { + /* non -p2p device */ + } + + } + + + return ret; + +} + +u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta) +{ + u8 status_code = P2P_STATUS_SUCCESS; + u8 *pbuf, *pattr_content = NULL; + u32 attr_contentlen = 0; + u16 cap_attr = 0; + unsigned short frame_type, ie_offset = 0; + u8 *ies; + u32 ies_len; + u8 *p2p_ie; + u32 p2p_ielen = 0; + + if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + return P2P_STATUS_FAIL_REQUEST_UNABLE; + + frame_type = get_frame_sub_type(pframe); + if (frame_type == WIFI_ASSOCREQ) + ie_offset = _ASOCREQ_IE_OFFSET_; + else /* WIFI_REASSOCREQ */ + ie_offset = _REASOCREQ_IE_OFFSET_; + + ies = pframe + WLAN_HDR_A3_LEN + ie_offset; + ies_len = len - WLAN_HDR_A3_LEN - ie_offset; + + p2p_ie = rtw_get_p2p_ie(ies , ies_len , NULL, &p2p_ielen); + + if (!p2p_ie) { + RTW_INFO("[%s] P2P IE not Found!!\n", __FUNCTION__); + status_code = P2P_STATUS_FAIL_INVALID_PARAM; + } else + RTW_INFO("[%s] P2P IE Found!!\n", __FUNCTION__); + + while (p2p_ie) { + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&cap_attr, (uint *) &attr_contentlen)) { + RTW_INFO("[%s] Got P2P Capability Attr!!\n", __FUNCTION__); + cap_attr = le16_to_cpu(cap_attr); + psta->dev_cap = cap_attr & 0xff; + } + + /* Check Extended Listen Timing ATTR */ + + + /* Check P2P Device Info ATTR */ + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint *)&attr_contentlen)) { + RTW_INFO("[%s] Got P2P DEVICE INFO Attr!!\n", __FUNCTION__); + pattr_content = pbuf = rtw_zmalloc(attr_contentlen); + if (pattr_content) { + u8 num_of_secdev_type; + u16 dev_name_len; + + + rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint *)&attr_contentlen); + + _rtw_memcpy(psta->dev_addr, pattr_content, ETH_ALEN);/* P2P Device Address */ + + pattr_content += ETH_ALEN; + + _rtw_memcpy(&psta->config_methods, pattr_content, 2);/* Config Methods */ + psta->config_methods = be16_to_cpu(psta->config_methods); + + pattr_content += 2; + + _rtw_memcpy(psta->primary_dev_type, pattr_content, 8); + + pattr_content += 8; + + num_of_secdev_type = *pattr_content; + pattr_content += 1; + + if (num_of_secdev_type == 0) + psta->num_of_secdev_type = 0; + else { + u32 len; + + psta->num_of_secdev_type = num_of_secdev_type; + + len = (sizeof(psta->secdev_types_list) < (num_of_secdev_type * 8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type * 8); + + _rtw_memcpy(psta->secdev_types_list, pattr_content, len); + + pattr_content += (num_of_secdev_type * 8); + } + + + /* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */ + psta->dev_name_len = 0; + if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16 *)pattr_content)) { + dev_name_len = be16_to_cpu(*(u16 *)(pattr_content + 2)); + + psta->dev_name_len = (sizeof(psta->dev_name) < dev_name_len) ? sizeof(psta->dev_name) : dev_name_len; + + _rtw_memcpy(psta->dev_name, pattr_content + 4, psta->dev_name_len); + } + + rtw_mfree(pbuf, attr_contentlen); + + } + + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + + } + + return status_code; + +} + +u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *frame_body; + u8 status, dialogToken; + struct sta_info *psta = NULL; + _adapter *padapter = pwdinfo->padapter; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *p2p_ie; + u32 p2p_ielen = 0; + + frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + + dialogToken = frame_body[7]; + status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP; + + p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen); + if (p2p_ie) { + u8 groupid[38] = { 0x00 }; + u8 dev_addr[ETH_ALEN] = { 0x00 }; + u32 attr_contentlen = 0; + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) { + if (_rtw_memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) && + _rtw_memcmp(pwdinfo->p2p_group_ssid, groupid + ETH_ALEN, pwdinfo->p2p_group_ssid_len)) { + attr_contentlen = 0; + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen)) { + _irqL irqL; + _list *phead, *plist; + + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); + phead = &pstapriv->asoc_list; + plist = get_next(phead); + + /* look up sta asoc_queue */ + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list); + + plist = get_next(plist); + + if (psta->is_p2p_device && (psta->dev_cap & P2P_DEVCAP_CLIENT_DISCOVERABILITY) && + _rtw_memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) { + + /* _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); */ + /* issue GO Discoverability Request */ + issue_group_disc_req(pwdinfo, psta->cmn.mac_addr); + /* _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); */ + + status = P2P_STATUS_SUCCESS; + + break; + } else + status = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + + } + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); + + } else + status = P2P_STATUS_FAIL_INVALID_PARAM; + + } else + status = P2P_STATUS_FAIL_INVALID_PARAM; + + } + + } + + + /* issue Device Discoverability Response */ + issue_p2p_devdisc_resp(pwdinfo, get_addr2_ptr(pframe), status, dialogToken); + + + return (status == P2P_STATUS_SUCCESS) ? _TRUE : _FALSE; + +} + +u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + return _TRUE; +} + +u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *frame_body; + u8 *wpsie; + uint wps_ielen = 0, attr_contentlen = 0; + u16 uconfig_method = 0; + + + frame_body = (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + + wpsie = rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen); + if (wpsie) { + if (rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD , (u8 *) &uconfig_method, &attr_contentlen)) { + uconfig_method = be16_to_cpu(uconfig_method); + switch (uconfig_method) { + case WPS_CM_DISPLYA: { + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3); + break; + } + case WPS_CM_LABEL: { + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3); + break; + } + case WPS_CM_PUSH_BUTTON: { + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3); + break; + } + case WPS_CM_KEYPAD: { + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3); + break; + } + } + issue_p2p_provision_resp(pwdinfo, get_addr2_ptr(pframe), frame_body, uconfig_method); + } + } + RTW_INFO("[%s] config method = %s\n", __FUNCTION__, pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req); + return _TRUE; + +} + +u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe) +{ + + return _TRUE; +} + +u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list) +{ + u8 i = 0, j = 0; + u8 temp = 0; + u8 ch_no = 0; + ch_content += 3; + ch_cnt -= 3; + + while (ch_cnt > 0) { + ch_content += 1; + ch_cnt -= 1; + temp = *ch_content; + for (i = 0 ; i < temp ; i++, j++) + peer_ch_list[j] = *(ch_content + 1 + i); + ch_content += (temp + 1); + ch_cnt -= (temp + 1); + ch_no += temp ; + } + + return ch_no; +} + +u8 rtw_p2p_ch_inclusion(_adapter *adapter, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + int i = 0, j = 0, temp = 0; + u8 ch_no = 0; + + for (i = 0; i < peer_ch_num; i++) { + for (j = temp; j < rfctl->max_chan_nums; j++) { + if (*(peer_ch_list + i) == rfctl->channel_set[j].ChannelNum) { + ch_list_inclusioned[ch_no++] = *(peer_ch_list + i); + temp = j; + break; + } + } + } + + return ch_no; +} + +u8 process_p2p_group_negotation_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + _adapter *padapter = pwdinfo->padapter; + u8 result = P2P_STATUS_SUCCESS; + u32 p2p_ielen = 0, wps_ielen = 0; + u8 *ies; + u32 ies_len; + u8 *p2p_ie; + u8 *wpsie; + u16 wps_devicepassword_id = 0x0000; + uint wps_devicepassword_id_len = 0; +#ifdef CONFIG_WFD +#ifdef CONFIG_TDLS + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; +#endif /* CONFIG_TDLS */ +#endif /* CONFIG_WFD */ + wpsie = rtw_get_wps_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen); + if (wpsie) { + /* Commented by Kurt 20120113 */ + /* If some device wants to do p2p handshake without sending prov_disc_req */ + /* We have to get peer_req_cm from here. */ + if (_rtw_memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) { + rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8 *) &wps_devicepassword_id, &wps_devicepassword_id_len); + wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id); + + if (wps_devicepassword_id == WPS_DPID_USER_SPEC) + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3); + else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3); + else + _rtw_memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3); + } + } else { + RTW_INFO("[%s] WPS IE not Found!!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + return result ; + } + + ies = pframe + _PUBLIC_ACTION_IE_OFFSET_; + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + if (!p2p_ie) { + RTW_INFO("[%s] P2P IE not Found!!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } + + while (p2p_ie) { + u8 attr_content = 0x00; + u32 attr_contentlen = 0; + u8 ch_content[100] = { 0x00 }; + uint ch_cnt = 0; + u8 peer_ch_list[100] = { 0x00 }; + u8 peer_ch_num = 0; + u8 ch_list_inclusioned[100] = { 0x00 }; + u8 ch_num_inclusioned = 0; + u16 cap_attr; + u8 listen_ch_attr[5] = { 0x00 }; + + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING); + + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&cap_attr, (uint *)&attr_contentlen)) { + cap_attr = le16_to_cpu(cap_attr); + +#if defined(CONFIG_WFD) && defined(CONFIG_TDLS) + if (!(cap_attr & P2P_GRPCAP_INTRABSS)) + ptdlsinfo->ap_prohibited = _TRUE; +#endif /* defined(CONFIG_WFD) && defined(CONFIG_TDLS) */ + } + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) { + RTW_INFO("[%s] GO Intent = %d, tie = %d\n", __FUNCTION__, attr_content >> 1, attr_content & 0x01); + pwdinfo->peer_intent = attr_content; /* include both intent and tie breaker values. */ + + if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) { + /* Try to match the tie breaker value */ + if (pwdinfo->intent == P2P_MAX_INTENT) { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + result = P2P_STATUS_FAIL_BOTH_GOINTENT_15; + } else { + if (attr_content & 0x01) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + else + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + } else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + else + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* Store the group id information. */ + _rtw_memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN); + _rtw_memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen); + } + } + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, (u8 *)listen_ch_attr, (uint *) &attr_contentlen) && attr_contentlen == 5) + pwdinfo->nego_req_info.peer_ch = listen_ch_attr[4]; + + RTW_INFO(FUNC_ADPT_FMT" listen channel :%u\n", FUNC_ADPT_ARG(padapter), pwdinfo->nego_req_info.peer_ch); + + attr_contentlen = 0; + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) { + if (attr_contentlen != ETH_ALEN) + _rtw_memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN); + } + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt)) { + peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list); + ch_num_inclusioned = rtw_p2p_ch_inclusion(padapter, peer_ch_list, peer_ch_num, ch_list_inclusioned); + + if (ch_num_inclusioned == 0) { + RTW_INFO("[%s] No common channel in channel list!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_NO_COMMON_CH; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel, + ch_list_inclusioned, ch_num_inclusioned)) { +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) + && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + RTW_INFO("[%s] desired channel NOT Found!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_NO_COMMON_CH; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } else +#endif /* CONFIG_CONCURRENT_MODE */ + { + u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0; + attr_contentlen = 0; + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + peer_operating_ch = operatingch_info[4]; + + if (rtw_p2p_is_channel_list_ok(peer_operating_ch, + ch_list_inclusioned, ch_num_inclusioned)) { + /** + * Change our operating channel as peer's for compatibility. + */ + pwdinfo->operating_channel = peer_operating_ch; + RTW_INFO("[%s] Change op ch to %02x as peer's\n", __FUNCTION__, pwdinfo->operating_channel); + } else { + /* Take first channel of ch_list_inclusioned as operating channel */ + pwdinfo->operating_channel = ch_list_inclusioned[0]; + RTW_INFO("[%s] Change op ch to %02x\n", __FUNCTION__, pwdinfo->operating_channel); + } + } + + } + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } + + if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO) { + result = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY); + return result; + } + +#ifdef CONFIG_WFD + rtw_process_wfd_ies(padapter, pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, __func__); +#endif + + return result ; +} + +u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + _adapter *padapter = pwdinfo->padapter; + u8 result = P2P_STATUS_SUCCESS; + u32 p2p_ielen, wps_ielen; + u8 *ies; + u32 ies_len; + u8 *p2p_ie; +#ifdef CONFIG_WFD +#ifdef CONFIG_TDLS + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; +#endif /* CONFIG_TDLS */ +#endif /* CONFIG_WFD */ + + ies = pframe + _PUBLIC_ACTION_IE_OFFSET_; + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + /* Be able to know which one is the P2P GO and which one is P2P client. */ + + if (rtw_get_wps_ie(ies, ies_len, NULL, &wps_ielen)) { + + } else { + RTW_INFO("[%s] WPS IE not Found!!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + if (!p2p_ie) { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + } else { + + u8 attr_content = 0x00; + u32 attr_contentlen = 0; + u8 operatingch_info[5] = { 0x00 }; + u8 groupid[38]; + u16 cap_attr; + u8 peer_ch_list[100] = { 0x00 }; + u8 peer_ch_num = 0; + u8 ch_list_inclusioned[100] = { 0x00 }; + u8 ch_num_inclusioned = 0; + + while (p2p_ie) { /* Found the P2P IE. */ + + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&cap_attr, (uint *)&attr_contentlen)) { + cap_attr = le16_to_cpu(cap_attr); +#ifdef CONFIG_TDLS + if (!(cap_attr & P2P_GRPCAP_INTRABSS)) + ptdlsinfo->ap_prohibited = _TRUE; +#endif /* CONFIG_TDLS */ + } + + rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + if (attr_contentlen == 1) { + RTW_INFO("[%s] Status = %d\n", __FUNCTION__, attr_content); + if (attr_content == P2P_STATUS_SUCCESS) { + /* Do nothing. */ + } else { + if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY); + else + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + result = attr_content; + break; + } + } + + /* Try to get the peer's interface address */ + attr_contentlen = 0; + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) { + if (attr_contentlen != ETH_ALEN) + _rtw_memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN); + } + + /* Try to get the peer's intent and tie breaker value. */ + attr_content = 0x00; + attr_contentlen = 0; + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) { + RTW_INFO("[%s] GO Intent = %d, tie = %d\n", __FUNCTION__, attr_content >> 1, attr_content & 0x01); + pwdinfo->peer_intent = attr_content; /* include both intent and tie breaker values. */ + + if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) { + /* Try to match the tie breaker value */ + if (pwdinfo->intent == P2P_MAX_INTENT) { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + result = P2P_STATUS_FAIL_BOTH_GOINTENT_15; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } else { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + if (attr_content & 0x01) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + else + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + } else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } else { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + /* Store the group id information. */ + _rtw_memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN); + _rtw_memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen); + + } + } + + /* Try to get the operation channel information */ + + attr_contentlen = 0; + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) { + RTW_INFO("[%s] Peer's operating channel = %d\n", __FUNCTION__, operatingch_info[4]); + pwdinfo->peer_operating_ch = operatingch_info[4]; + } + + /* Try to get the channel list information */ + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len)) { + RTW_INFO("[%s] channel list attribute found, len = %d\n", __FUNCTION__, pwdinfo->channel_list_attr_len); + + peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list); + ch_num_inclusioned = rtw_p2p_ch_inclusion(padapter, peer_ch_list, peer_ch_num, ch_list_inclusioned); + + if (ch_num_inclusioned == 0) { + RTW_INFO("[%s] No common channel in channel list!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_NO_COMMON_CH; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) { + if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel, + ch_list_inclusioned, ch_num_inclusioned)) { +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) + && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + RTW_INFO("[%s] desired channel NOT Found!\n", __FUNCTION__); + result = P2P_STATUS_FAIL_NO_COMMON_CH; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } else +#endif /* CONFIG_CONCURRENT_MODE */ + { + u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0; + attr_contentlen = 0; + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + peer_operating_ch = operatingch_info[4]; + + if (rtw_p2p_is_channel_list_ok(peer_operating_ch, + ch_list_inclusioned, ch_num_inclusioned)) { + /** + * Change our operating channel as peer's for compatibility. + */ + pwdinfo->operating_channel = peer_operating_ch; + RTW_INFO("[%s] Change op ch to %02x as peer's\n", __FUNCTION__, pwdinfo->operating_channel); + } else { + /* Take first channel of ch_list_inclusioned as operating channel */ + pwdinfo->operating_channel = ch_list_inclusioned[0]; + RTW_INFO("[%s] Change op ch to %02x\n", __FUNCTION__, pwdinfo->operating_channel); + } + } + + } + } + + } else + RTW_INFO("[%s] channel list attribute not found!\n", __FUNCTION__); + + /* Try to get the group id information if peer is GO */ + attr_contentlen = 0; + _rtw_memset(groupid, 0x00, 38); + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) { + _rtw_memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN); + _rtw_memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN); + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } + + } + +#ifdef CONFIG_WFD + rtw_process_wfd_ies(padapter, pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, __func__); +#endif + + return result ; + +} + +u8 process_p2p_group_negotation_confirm(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ +#ifdef CONFIG_CONCURRENT_MODE + _adapter *padapter = pwdinfo->padapter; + struct roch_info *prochinfo = &padapter->rochinfo; +#endif + u8 *ies; + u32 ies_len; + u8 *p2p_ie; + u32 p2p_ielen = 0; + u8 result = P2P_STATUS_SUCCESS; + ies = pframe + _PUBLIC_ACTION_IE_OFFSET_; + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + while (p2p_ie) { /* Found the P2P IE. */ + u8 attr_content = 0x00, operatingch_info[5] = { 0x00 }; + u8 groupid[38] = { 0x00 }; + u32 attr_contentlen = 0; + + pwdinfo->negotiation_dialog_token = 1; + rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + if (attr_contentlen == 1) { + RTW_INFO("[%s] Status = %d\n", __FUNCTION__, attr_content); + result = attr_content; + + if (attr_content == P2P_STATUS_SUCCESS) { + + _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer); + + /* Commented by Albert 20100911 */ + /* Todo: Need to handle the case which both Intents are the same. */ + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1)) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1)) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + else { + /* Have to compare the Tie Breaker */ + if (pwdinfo->peer_intent & 0x01) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + else + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED) + && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + /* Switch back to the AP channel soon. */ + _set_timer(&prochinfo->ap_roch_ch_switch_timer, 100); + } +#endif + } else { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } + } + + /* Try to get the group id information */ + attr_contentlen = 0; + _rtw_memset(groupid, 0x00, 38); + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) { + RTW_INFO("[%s] Ssid = %s, ssidlen = %zu\n", __FUNCTION__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN])); + _rtw_memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN); + _rtw_memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN); + } + + attr_contentlen = 0; + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) { + RTW_INFO("[%s] Peer's operating channel = %d\n", __FUNCTION__, operatingch_info[4]); + pwdinfo->peer_operating_ch = operatingch_info[4]; + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + + } + + return result ; +} + +u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *frame_body; + u8 dialogToken = 0; + u8 status = P2P_STATUS_SUCCESS; + + frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + + dialogToken = frame_body[6]; + + /* todo: check NoA attribute */ + + issue_p2p_presence_resp(pwdinfo, get_addr2_ptr(pframe), status, dialogToken); + + return _TRUE; +} + +void find_phase_handler(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct sitesurvey_parm parm; + _irqL irqL; + u8 _status = 0; + + + rtw_init_sitesurvey_parm(padapter, &parm); + _rtw_memcpy(&parm.ssid[0].Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN); + parm.ssid[0].SsidLength = P2P_WILDCARD_SSID_LEN; + parm.ssid_num = 1; + + rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH); + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + _status = rtw_sitesurvey_cmd(padapter, &parm); + _exit_critical_bh(&pmlmepriv->lock, &irqL); + + +} + +void restore_p2p_state_handler(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED)) { + u8 union_ch = rtw_mi_get_union_chan(padapter); + u8 union_bw = rtw_mi_get_union_bw(padapter); + u8 union_offset = rtw_mi_get_union_offset(padapter); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP)) { + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + rtw_back_opch(padapter); + } + } +#endif + + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) { +#ifdef CONFIG_CONCURRENT_MODE + rtw_concurrent_handler(padapter); +#else + /* In the P2P client mode, the driver should not switch back to its listen channel */ + /* because this P2P client should stay at the operating channel of P2P GO. */ + set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); +#endif + } +} + +void pre_tx_invitereq_handler(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq_p2p(padapter, NULL); + _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); + +} + +void pre_tx_provdisc_handler(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq_p2p(padapter, NULL); + _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); + +} + +void pre_tx_negoreq_handler(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq_p2p(padapter , NULL); + /* WIN Phone only accept unicast probe request when nego back */ + issue_probereq_p2p(padapter , pwdinfo->nego_req_info.peerDevAddr); + _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); + +} + +#ifdef CONFIG_IOCTL_CFG80211 + +#if 0 +static void rtw_change_p2pie_op_ch(_adapter *padapter, const u8 *frame_body, u32 len, u8 ch) +{ + u8 *ies, *p2p_ie; + u32 ies_len, p2p_ielen; + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) + return; +#endif /* CONFIG_MCC_MODE */ + + ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_); + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + u32 attr_contentlen = 0; + u8 *pattr = NULL; + + /* Check P2P_ATTR_OPERATING_CH */ + attr_contentlen = 0; + pattr = NULL; + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint *)&attr_contentlen); + if (pattr != NULL) + *(pattr + 4) = ch; + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } +} +#endif + +#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) +static void rtw_change_p2pie_ch_list(_adapter *padapter, const u8 *frame_body, u32 len, u8 ch) +{ + u8 *ies, *p2p_ie; + u32 ies_len, p2p_ielen; + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) + return; +#endif /* CONFIG_MCC_MODE */ + + ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_); + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + u32 attr_contentlen = 0; + u8 *pattr = NULL; + + /* Check P2P_ATTR_CH_LIST */ + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint *)&attr_contentlen); + if (pattr != NULL) { + int i; + u32 num_of_ch; + u8 *pattr_temp = pattr + 3 ; + + attr_contentlen -= 3; + + while (attr_contentlen > 0) { + num_of_ch = *(pattr_temp + 1); + + for (i = 0; i < num_of_ch; i++) + *(pattr_temp + 2 + i) = ch; + + pattr_temp += (2 + num_of_ch); + attr_contentlen -= (2 + num_of_ch); + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } +} +#endif + +#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) +static bool rtw_chk_p2pie_ch_list_with_buddy(_adapter *padapter, const u8 *frame_body, u32 len) +{ + bool fit = _FALSE; + u8 *ies, *p2p_ie; + u32 ies_len, p2p_ielen; + u8 union_ch = rtw_mi_get_union_chan(padapter); + + ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_); + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + u32 attr_contentlen = 0; + u8 *pattr = NULL; + + /* Check P2P_ATTR_CH_LIST */ + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint *)&attr_contentlen); + if (pattr != NULL) { + int i; + u32 num_of_ch; + u8 *pattr_temp = pattr + 3 ; + + attr_contentlen -= 3; + + while (attr_contentlen > 0) { + num_of_ch = *(pattr_temp + 1); + + for (i = 0; i < num_of_ch; i++) { + if (*(pattr_temp + 2 + i) == union_ch) { + RTW_INFO(FUNC_ADPT_FMT" ch_list fit buddy_ch:%u\n", FUNC_ADPT_ARG(padapter), union_ch); + fit = _TRUE; + break; + } + } + + pattr_temp += (2 + num_of_ch); + attr_contentlen -= (2 + num_of_ch); + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } + + return fit; +} + +#if defined(CONFIG_P2P_INVITE_IOT) +static bool rtw_chk_p2pie_op_ch_with_buddy(_adapter *padapter, const u8 *frame_body, u32 len) +{ + bool fit = _FALSE; + u8 *ies, *p2p_ie; + u32 ies_len, p2p_ielen; + u8 union_ch = rtw_mi_get_union_chan(padapter); + + ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_); + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + u32 attr_contentlen = 0; + u8 *pattr = NULL; + + /* Check P2P_ATTR_OPERATING_CH */ + attr_contentlen = 0; + pattr = NULL; + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint *)&attr_contentlen); + if (pattr != NULL) { + if (*(pattr + 4) == union_ch) { + RTW_INFO(FUNC_ADPT_FMT" op_ch fit buddy_ch:%u\n", FUNC_ADPT_ARG(padapter), union_ch); + fit = _TRUE; + break; + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } + + return fit; +} +#endif + +static void rtw_cfg80211_adjust_p2pie_channel(_adapter *padapter, const u8 *frame_body, u32 len) +{ + u8 *ies, *p2p_ie; + u32 ies_len, p2p_ielen; + u8 union_ch = rtw_mi_get_union_chan(padapter); + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) + return; +#endif /* CONFIG_MCC_MODE */ + + ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_); + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + u32 attr_contentlen = 0; + u8 *pattr = NULL; + + /* Check P2P_ATTR_CH_LIST */ + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint *)&attr_contentlen); + if (pattr != NULL) { + int i; + u32 num_of_ch; + u8 *pattr_temp = pattr + 3 ; + + attr_contentlen -= 3; + + while (attr_contentlen > 0) { + num_of_ch = *(pattr_temp + 1); + + for (i = 0; i < num_of_ch; i++) { + if (*(pattr_temp + 2 + i) && *(pattr_temp + 2 + i) != union_ch) { + #ifdef RTW_SINGLE_WIPHY + RTW_ERR("replace ch_list:%u with:%u\n", *(pattr_temp + 2 + i), union_ch); + #endif + *(pattr_temp + 2 + i) = union_ch; /*forcing to the same channel*/ + } + } + + pattr_temp += (2 + num_of_ch); + attr_contentlen -= (2 + num_of_ch); + } + } + + /* Check P2P_ATTR_OPERATING_CH */ + attr_contentlen = 0; + pattr = NULL; + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint *)&attr_contentlen); + if (pattr != NULL) { + if (*(pattr + 4) && *(pattr + 4) != union_ch) { + #ifdef RTW_SINGLE_WIPHY + RTW_ERR("replace op_ch:%u with:%u\n", *(pattr + 4), union_ch); + #endif + *(pattr + 4) = union_ch; /*forcing to the same channel */ + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + + } + +} +#endif + +#ifdef CONFIG_WFD +u32 rtw_xframe_build_wfd_ie(struct xmit_frame *xframe) +{ + _adapter *adapter = xframe->padapter; + struct wifidirect_info *wdinfo = &adapter->wdinfo; + u8 *frame = xframe->buf_addr + TXDESC_OFFSET; + u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr); + u8 *frame_tail = frame + xframe->attrib.pktlen; + u8 category, action, OUI_Subtype, dialogToken = 0; + u32 wfdielen = 0; + + category = frame_body[0]; + if (category == RTW_WLAN_CATEGORY_PUBLIC) { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR + && _rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE + ) { + OUI_Subtype = frame_body[6]; + dialogToken = frame_body[7]; + + switch (OUI_Subtype) { + case P2P_GO_NEGO_REQ: + wfdielen = build_nego_req_wfd_ie(wdinfo, frame_tail); + break; + case P2P_GO_NEGO_RESP: + wfdielen = build_nego_resp_wfd_ie(wdinfo, frame_tail); + break; + case P2P_GO_NEGO_CONF: + wfdielen = build_nego_confirm_wfd_ie(wdinfo, frame_tail); + break; + case P2P_INVIT_REQ: + wfdielen = build_invitation_req_wfd_ie(wdinfo, frame_tail); + break; + case P2P_INVIT_RESP: + wfdielen = build_invitation_resp_wfd_ie(wdinfo, frame_tail); + break; + case P2P_PROVISION_DISC_REQ: + wfdielen = build_provdisc_req_wfd_ie(wdinfo, frame_tail); + break; + case P2P_PROVISION_DISC_RESP: + wfdielen = build_provdisc_resp_wfd_ie(wdinfo, frame_tail); + break; + case P2P_DEVDISC_REQ: + case P2P_DEVDISC_RESP: + default: + break; + } + + } + } else if (category == RTW_WLAN_CATEGORY_P2P) { + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + +#ifdef CONFIG_DEBUG_CFG80211 + RTW_INFO("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n" + , cpu_to_be32(*((u32 *)(frame_body + 1))), OUI_Subtype, dialogToken); +#endif + + switch (OUI_Subtype) { + case P2P_NOTICE_OF_ABSENCE: + break; + case P2P_PRESENCE_REQUEST: + break; + case P2P_PRESENCE_RESPONSE: + break; + case P2P_GO_DISC_REQUEST: + break; + default: + break; + } + } else + RTW_INFO("%s, action frame category=%d\n", __func__, category); + + xframe->attrib.pktlen += wfdielen; + + return wfdielen; +} +#endif /* CONFIG_WFD */ + +bool rtw_xframe_del_wfd_ie(struct xmit_frame *xframe) +{ +#define DBG_XFRAME_DEL_WFD_IE 0 + u8 *frame = xframe->buf_addr + TXDESC_OFFSET; + u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr); + u8 *frame_tail = frame + xframe->attrib.pktlen; + u8 category, action, OUI_Subtype; + u8 *ies = NULL; + uint ies_len_ori = 0; + uint ies_len = 0; + + category = frame_body[0]; + if (category == RTW_WLAN_CATEGORY_PUBLIC) { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR + && _rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE + ) { + OUI_Subtype = frame_body[6]; + + switch (OUI_Subtype) { + case P2P_GO_NEGO_REQ: + case P2P_GO_NEGO_RESP: + case P2P_GO_NEGO_CONF: + case P2P_INVIT_REQ: + case P2P_INVIT_RESP: + case P2P_PROVISION_DISC_REQ: + case P2P_PROVISION_DISC_RESP: + ies = frame_body + 8; + ies_len_ori = frame_tail - (frame_body + 8); + break; + } + } + } + + if (ies && ies_len_ori) { + ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_XFRAME_DEL_WFD_IE ? __func__ : NULL); + xframe->attrib.pktlen -= (ies_len_ori - ies_len); + } + + return ies_len_ori != ies_len; +} + +/* +* rtw_xframe_chk_wfd_ie - +* +*/ +void rtw_xframe_chk_wfd_ie(struct xmit_frame *xframe) +{ + _adapter *adapter = xframe->padapter; +#ifdef CONFIG_IOCTL_CFG80211 + struct wifidirect_info *wdinfo = &adapter->wdinfo; +#endif + u8 build = 0; + u8 del = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + del = 1; + +#ifdef CONFIG_IOCTL_CFG80211 + if (wdinfo->wfd_info->wfd_enable == _TRUE) +#endif + del = build = 1; + + if (del) + rtw_xframe_del_wfd_ie(xframe); + +#ifdef CONFIG_WFD + if (build) + rtw_xframe_build_wfd_ie(xframe); +#endif +} + +u8 *dump_p2p_attr_ch_list(u8 *p2p_ie, uint p2p_ielen, u8 *buf, u32 buf_len) +{ + uint attr_contentlen = 0; + u8 *pattr = NULL; + int w_sz = 0; + u8 ch_cnt = 0; + u8 ch_list[40]; + + pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, &attr_contentlen); + if (pattr != NULL) { + int i, j; + u32 num_of_ch; + u8 *pattr_temp = pattr + 3 ; + + attr_contentlen -= 3; + + _rtw_memset(ch_list, 0, 40); + + while (attr_contentlen > 0) { + num_of_ch = *(pattr_temp + 1); + + for (i = 0; i < num_of_ch; i++) { + for (j = 0; j < ch_cnt; j++) { + if (ch_list[j] == *(pattr_temp + 2 + i)) + break; + } + if (j >= ch_cnt) + ch_list[ch_cnt++] = *(pattr_temp + 2 + i); + + } + + pattr_temp += (2 + num_of_ch); + attr_contentlen -= (2 + num_of_ch); + } + + for (j = 0; j < ch_cnt; j++) { + if (j == 0) + w_sz += snprintf(buf + w_sz, buf_len - w_sz, "%u", ch_list[j]); + else if (ch_list[j] - ch_list[j - 1] != 1) + w_sz += snprintf(buf + w_sz, buf_len - w_sz, ", %u", ch_list[j]); + else if (j != ch_cnt - 1 && ch_list[j + 1] - ch_list[j] == 1) { + /* empty */ + } else + w_sz += snprintf(buf + w_sz, buf_len - w_sz, "-%u", ch_list[j]); + } + } + return buf; +} + +/* + * return _TRUE if requester is GO, _FALSE if responder is GO + */ +bool rtw_p2p_nego_intent_compare(u8 req, u8 resp) +{ + if (req >> 1 == resp >> 1) + return req & 0x01 ? _TRUE : _FALSE; + else if (req >> 1 > resp >> 1) + return _TRUE; + else + return _FALSE; +} + +int rtw_p2p_check_frames(_adapter *padapter, const u8 *buf, u32 len, u8 tx) +{ + int is_p2p_frame = (-1); + unsigned char *frame_body; + u8 category, action, OUI_Subtype, dialogToken = 0; + u8 *p2p_ie = NULL; + uint p2p_ielen = 0; + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); + int status = -1; + u8 ch_list_buf[128] = {'\0'}; + int op_ch = -1; + int listen_ch = -1; + u8 intent = 0; + u8 *iaddr = NULL; + u8 *gbssid = NULL; + + frame_body = (unsigned char *)(buf + sizeof(struct rtw_ieee80211_hdr_3addr)); + category = frame_body[0]; + /* just for check */ + if (category == RTW_WLAN_CATEGORY_PUBLIC) { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR + && _rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE + ) { + OUI_Subtype = frame_body[6]; + dialogToken = frame_body[7]; + is_p2p_frame = OUI_Subtype; + + #ifdef CONFIG_DEBUG_CFG80211 + RTW_INFO("ACTION_CATEGORY_PUBLIC: ACT_PUBLIC_VENDOR, OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n", + cpu_to_be32(*((u32 *)(frame_body + 2))), OUI_Subtype, dialogToken); + #endif + + p2p_ie = rtw_get_p2p_ie( + (u8 *)buf + sizeof(struct rtw_ieee80211_hdr_3addr) + _PUBLIC_ACTION_IE_OFFSET_ + , len - sizeof(struct rtw_ieee80211_hdr_3addr) - _PUBLIC_ACTION_IE_OFFSET_ + , NULL, &p2p_ielen); + + switch (OUI_Subtype) { /* OUI Subtype */ + u8 *cont; + uint cont_len; + case P2P_GO_NEGO_REQ: { + struct rtw_wdev_nego_info *nego_info = &pwdev_priv->nego_info; + + if (tx) { + #ifdef CONFIG_DRV_ISSUE_PROV_REQ /* IOT FOR S2 */ + if (pwdev_priv->provdisc_req_issued == _FALSE) + rtw_cfg80211_issue_p2p_provision_request(padapter, buf, len); + #endif /* CONFIG_DRV_ISSUE_PROV_REQ */ + + /* pwdev_priv->provdisc_req_issued = _FALSE; */ + + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)); + #endif + } + + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len); + if (cont) + op_ch = *(cont + 4); + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, NULL, &cont_len); + if (cont) + listen_ch = *(cont + 4); + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, NULL, &cont_len); + if (cont) + intent = *cont; + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, NULL, &cont_len); + if (cont && cont_len == 6) + iaddr = cont; + + if (nego_info->token != dialogToken) + rtw_wdev_nego_info_init(nego_info); + + _rtw_memcpy(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN); + if (iaddr) + _rtw_memcpy(tx ? nego_info->iface_addr : nego_info->peer_iface_addr, iaddr, ETH_ALEN); + nego_info->active = tx ? 1 : 0; + nego_info->token = dialogToken; + nego_info->req_op_ch = op_ch; + nego_info->req_listen_ch = listen_ch; + nego_info->req_intent = intent; + nego_info->state = 0; + + dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128); + RTW_INFO("RTW_%s:P2P_GO_NEGO_REQ, dialogToken=%d, intent:%u%s, listen_ch:%d, op_ch:%d, ch_list:%s" + , (tx == _TRUE) ? "Tx" : "Rx" , dialogToken , (intent >> 1) , intent & 0x1 ? "+" : "-" , listen_ch , op_ch , ch_list_buf); + if (iaddr) + _RTW_INFO(", iaddr:"MAC_FMT, MAC_ARG(iaddr)); + _RTW_INFO("\n"); + + if (!tx) { + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) + && rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE + && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + RTW_INFO(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter)); + rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0); + } + #endif + } + + break; + } + case P2P_GO_NEGO_RESP: { + struct rtw_wdev_nego_info *nego_info = &pwdev_priv->nego_info; + + if (tx) { + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)); + #endif + } + + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len); + if (cont) + op_ch = *(cont + 4); + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, NULL, &cont_len); + if (cont) + intent = *cont; + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + if (cont) + status = *cont; + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, NULL, &cont_len); + if (cont && cont_len == 6) + iaddr = cont; + + if (nego_info->token == dialogToken && nego_info->state == 0 + && _rtw_memcmp(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN) == _TRUE + ) { + if (iaddr) + _rtw_memcpy(tx ? nego_info->iface_addr : nego_info->peer_iface_addr, iaddr, ETH_ALEN); + nego_info->status = (status == -1) ? 0xff : status; + nego_info->rsp_op_ch = op_ch; + nego_info->rsp_intent = intent; + nego_info->state = 1; + if (status != 0) + nego_info->token = 0; /* init */ + } + + dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128); + RTW_INFO("RTW_%s:P2P_GO_NEGO_RESP, dialogToken=%d, intent:%u%s, status:%d, op_ch:%d, ch_list:%s" + , (tx == _TRUE) ? "Tx" : "Rx", dialogToken, (intent >> 1), intent & 0x1 ? "+" : "-", status, op_ch, ch_list_buf); + if (iaddr) + _RTW_INFO(", iaddr:"MAC_FMT, MAC_ARG(iaddr)); + _RTW_INFO("\n"); + + if (!tx) { + pwdev_priv->provdisc_req_issued = _FALSE; + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) + && rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE + && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + RTW_INFO(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter)); + rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0); + } + #endif + } + + break; + } + case P2P_GO_NEGO_CONF: { + struct rtw_wdev_nego_info *nego_info = &pwdev_priv->nego_info; + bool is_go = _FALSE; + + if (tx) { + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)); + #endif + } + + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len); + if (cont) + op_ch = *(cont + 4); + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + if (cont) + status = *cont; + + if (nego_info->token == dialogToken && nego_info->state == 1 + && _rtw_memcmp(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN) == _TRUE + ) { + nego_info->status = (status == -1) ? 0xff : status; + nego_info->conf_op_ch = (op_ch == -1) ? 0 : op_ch; + nego_info->state = 2; + + if (status == 0) { + if (rtw_p2p_nego_intent_compare(nego_info->req_intent, nego_info->rsp_intent) ^ !tx) + is_go = _TRUE; + } + + nego_info->token = 0; /* init */ + } + + dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128); + RTW_INFO("RTW_%s:P2P_GO_NEGO_CONF, dialogToken=%d, status:%d, op_ch:%d, ch_list:%s\n" + , (tx == _TRUE) ? "Tx" : "Rx", dialogToken, status, op_ch, ch_list_buf); + + if (!tx) { + } + + break; + } + case P2P_INVIT_REQ: { + struct rtw_wdev_invit_info *invit_info = &pwdev_priv->invit_info; + int flags = -1; + + if (tx) { + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) + && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)); + #endif + } + + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len); + if (cont) + flags = *cont; + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len); + if (cont) + op_ch = *(cont + 4); + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, NULL, &cont_len); + if (cont && cont_len == 6) + gbssid = cont; + + if (invit_info->token != dialogToken) + rtw_wdev_invit_info_init(invit_info); + + _rtw_memcpy(invit_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN); + if (gbssid) + _rtw_memcpy(invit_info->group_bssid, gbssid, ETH_ALEN); + invit_info->active = tx ? 1 : 0; + invit_info->token = dialogToken; + invit_info->flags = (flags == -1) ? 0x0 : flags; + invit_info->req_op_ch = op_ch; + invit_info->state = 0; + + dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128); + RTW_INFO("RTW_%s:P2P_INVIT_REQ, dialogToken=%d, flags:0x%02x, op_ch:%d, ch_list:%s" + , (tx == _TRUE) ? "Tx" : "Rx", dialogToken, flags, op_ch, ch_list_buf); + if (gbssid) + _RTW_INFO(", gbssid:"MAC_FMT, MAC_ARG(gbssid)); + _RTW_INFO("\n"); + + if (!tx) { + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) { + #if defined(CONFIG_P2P_INVITE_IOT) + if (op_ch != -1 && rtw_chk_p2pie_op_ch_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE) { + RTW_INFO(FUNC_ADPT_FMT" op_ch:%u has no intersect with buddy\n", FUNC_ADPT_ARG(padapter), op_ch); + rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0); + } else + #endif + if (rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE) { + RTW_INFO(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter)); + rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0); + } + } + #endif + } + + break; + } + case P2P_INVIT_RESP: { + struct rtw_wdev_invit_info *invit_info = &pwdev_priv->invit_info; + + if (tx) { + #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT) + if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) + rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)); + #endif + } + + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + if (cont) { + #ifdef CONFIG_P2P_INVITE_IOT + if (tx && *cont == 7) { + RTW_INFO("TX_P2P_INVITE_RESP, status is no common channel, change to unknown group\n"); + *cont = 8; /* unknow group status */ + } + #endif /* CONFIG_P2P_INVITE_IOT */ + status = *cont; + } + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len); + if (cont) + op_ch = *(cont + 4); + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, NULL, &cont_len); + if (cont && cont_len == 6) + gbssid = cont; + + if (invit_info->token == dialogToken && invit_info->state == 0 + && _rtw_memcmp(invit_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN) == _TRUE + ) { + invit_info->status = (status == -1) ? 0xff : status; + invit_info->rsp_op_ch = op_ch; + invit_info->state = 1; + invit_info->token = 0; /* init */ + } + + dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128); + RTW_INFO("RTW_%s:P2P_INVIT_RESP, dialogToken=%d, status:%d, op_ch:%d, ch_list:%s" + , (tx == _TRUE) ? "Tx" : "Rx", dialogToken, status, op_ch, ch_list_buf); + if (gbssid) + _RTW_INFO(", gbssid:"MAC_FMT, MAC_ARG(gbssid)); + _RTW_INFO("\n"); + + if (!tx) { + } + + break; + } + case P2P_DEVDISC_REQ: + RTW_INFO("RTW_%s:P2P_DEVDISC_REQ, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + break; + case P2P_DEVDISC_RESP: + cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + RTW_INFO("RTW_%s:P2P_DEVDISC_RESP, dialogToken=%d, status:%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken, cont ? *cont : -1); + break; + case P2P_PROVISION_DISC_REQ: { + size_t frame_body_len = len - sizeof(struct rtw_ieee80211_hdr_3addr); + u8 *p2p_ie; + uint p2p_ielen = 0; + uint contentlen = 0; + + RTW_INFO("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + + /* if(tx) */ + { + pwdev_priv->provdisc_req_issued = _FALSE; + + p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen); + if (p2p_ie) { + + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen)) { + pwdev_priv->provdisc_req_issued = _FALSE;/* case: p2p_client join p2p GO */ + } else { + #ifdef CONFIG_DEBUG_CFG80211 + RTW_INFO("provdisc_req_issued is _TRUE\n"); + #endif /*CONFIG_DEBUG_CFG80211*/ + pwdev_priv->provdisc_req_issued = _TRUE;/* case: p2p_devices connection before Nego req. */ + } + + } + } + } + break; + case P2P_PROVISION_DISC_RESP: + RTW_INFO("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + break; + default: + RTW_INFO("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", OUI_Subtype, dialogToken); + break; + } + + } + + } else if (category == RTW_WLAN_CATEGORY_P2P) { + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + + #ifdef CONFIG_DEBUG_CFG80211 + RTW_INFO("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n", + cpu_to_be32(*((u32 *)(frame_body + 1))), OUI_Subtype, dialogToken); + #endif + + is_p2p_frame = OUI_Subtype; + + switch (OUI_Subtype) { + case P2P_NOTICE_OF_ABSENCE: + RTW_INFO("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + break; + case P2P_PRESENCE_REQUEST: + RTW_INFO("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + break; + case P2P_PRESENCE_RESPONSE: + RTW_INFO("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + break; + case P2P_GO_DISC_REQUEST: + RTW_INFO("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", dialogToken); + break; + default: + RTW_INFO("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx == _TRUE) ? "Tx" : "Rx", OUI_Subtype, dialogToken); + break; + } + + } + + return is_p2p_frame; +} +#endif /* CONFIG_IOCTL_CFG80211 */ + +s32 p2p_protocol_wk_hdl(_adapter *padapter, int intCmdType, u8 *buf) +{ + int ret = H2C_SUCCESS; + + switch (intCmdType) { + case P2P_FIND_PHASE_WK: + find_phase_handler(padapter); + break; + + case P2P_RESTORE_STATE_WK: + restore_p2p_state_handler(padapter); + break; + + case P2P_PRE_TX_PROVDISC_PROCESS_WK: +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED)) + rtw_concurrent_handler(padapter); + else + pre_tx_provdisc_handler(padapter); +#else + pre_tx_provdisc_handler(padapter); +#endif + break; + + case P2P_PRE_TX_INVITEREQ_PROCESS_WK: +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED)) + rtw_concurrent_handler(padapter); + else + pre_tx_invitereq_handler(padapter); +#else + pre_tx_invitereq_handler(padapter); +#endif + break; + + case P2P_PRE_TX_NEGOREQ_PROCESS_WK: +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED)) + rtw_concurrent_handler(padapter); + else + pre_tx_negoreq_handler(padapter); +#else + pre_tx_negoreq_handler(padapter); +#endif + break; + + default: + rtw_warn_on(1); + break; + } + + return ret; +} + +int process_p2p_cross_connect_ie(PADAPTER padapter, u8 *IEs, u32 IELength) +{ + int ret = _TRUE; + u8 *ies; + u32 ies_len; + u8 *p2p_ie; + u32 p2p_ielen = 0; + u8 p2p_attr[MAX_P2P_IE_LEN] = { 0x00 };/* NoA length should be n*(13) + 2 */ + u32 attr_contentlen = 0; + + + + if (IELength <= _BEACON_IE_OFFSET_) + return ret; + + ies = IEs + _BEACON_IE_OFFSET_; + ies_len = IELength - _BEACON_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + /* Get P2P Manageability IE. */ + if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_MANAGEABILITY, p2p_attr, &attr_contentlen)) { + if ((p2p_attr[0] & (BIT(0) | BIT(1))) == 0x01) + ret = _FALSE; + break; + } + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + } + + return ret; +} + +#ifdef CONFIG_P2P_PS +void process_p2p_ps_ie(PADAPTER padapter, u8 *IEs, u32 IELength) +{ + u8 *ies; + u32 ies_len; + u8 *p2p_ie; + u32 p2p_ielen = 0; + u8 *noa_attr; /* NoA length should be n*(13) + 2 */ + u32 attr_contentlen = 0; + + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u8 find_p2p = _FALSE, find_p2p_ps = _FALSE; + u8 noa_offset, noa_num, noa_index; + + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; +#ifdef CONFIG_CONCURRENT_MODE +#ifndef CONFIG_FW_MULTI_PORT_SUPPORT + if (padapter->hw_port != HW_PORT0) + return; +#endif +#endif + if (IELength <= _BEACON_IE_OFFSET_) + return; + + ies = IEs + _BEACON_IE_OFFSET_; + ies_len = IELength - _BEACON_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen); + + while (p2p_ie) { + find_p2p = _TRUE; + /* Get Notice of Absence IE. */ + noa_attr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, NULL, &attr_contentlen); + if (noa_attr) { + find_p2p_ps = _TRUE; + noa_index = noa_attr[0]; + + if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) || + (noa_index != pwdinfo->noa_index)) { /* if index change, driver should reconfigure related setting. */ + pwdinfo->noa_index = noa_index; + pwdinfo->opp_ps = noa_attr[1] >> 7; + if (pwdinfo->opp_ps != 1) + pwdinfo->ctwindow = 0; + else + pwdinfo->ctwindow = noa_attr[1] & 0x7F; + noa_offset = 2; + noa_num = 0; + /* NoA length should be n*(13) + 2 */ + if (attr_contentlen > 2 && (attr_contentlen - 2) % 13 == 0) { + while (noa_offset < attr_contentlen && noa_num < P2P_MAX_NOA_NUM) { + /* _rtw_memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */ + pwdinfo->noa_count[noa_num] = noa_attr[noa_offset]; + noa_offset += 1; + + _rtw_memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4); + noa_offset += 4; + + _rtw_memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4); + noa_offset += 4; + + _rtw_memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4); + noa_offset += 4; + + noa_num++; + } + } + pwdinfo->noa_num = noa_num; + + if (pwdinfo->opp_ps == 1) { + pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW; + /* driver should wait LPS for entering CTWindow */ + if (adapter_to_pwrctl(padapter)->bFwCurrentInPSMode == _TRUE) + p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1); + } else if (pwdinfo->noa_num > 0) { + pwdinfo->p2p_ps_mode = P2P_PS_NOA; + p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1); + } else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) + p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1); + } + + break; /* find target, just break. */ + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen); + + } + + if (find_p2p == _TRUE) { + if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == _FALSE)) + p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1); + } + +} + +void p2p_ps_wk_hdl(_adapter *padapter, u8 p2p_ps_state) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + u32 ps_deny = 0; + + /* Pre action for p2p state */ + switch (p2p_ps_state) { + case P2P_PS_DISABLE: + pwdinfo->p2p_ps_state = p2p_ps_state; + + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + +#ifdef CONFIG_LPS + if (pwdinfo->opp_ps == 1) { + if (pwrpriv->smart_ps == 0) { + pwrpriv->smart_ps = 2; + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) + rtw_exec_lps(padapter, pwrpriv->pwr_mode); + } + } +#endif /* CONFIG_LPS */ + + pwdinfo->noa_index = 0; + pwdinfo->ctwindow = 0; + pwdinfo->opp_ps = 0; + pwdinfo->noa_num = 0; + pwdinfo->p2p_ps_mode = P2P_PS_NONE; + + break; + case P2P_PS_ENABLE: + _enter_pwrlock(&adapter_to_pwrctl(padapter)->lock); + ps_deny = rtw_ps_deny_get(padapter); + _exit_pwrlock(&adapter_to_pwrctl(padapter)->lock); + + if ((ps_deny & (PS_DENY_SCAN | PS_DENY_JOIN)) + || rtw_mi_check_fwstate(padapter, (WIFI_UNDER_SURVEY | WIFI_UNDER_LINKING))) { + pwdinfo->p2p_ps_mode = P2P_PS_NONE; + RTW_DBG(FUNC_ADPT_FMT" Block P2P PS under site survey or LINKING\n", FUNC_ADPT_ARG(padapter)); + return; + } + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) { +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) { + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) { + RTW_INFO("P2P PS enble under MCC\n"); + rtw_warn_on(1); + } + + } +#endif /* CONFIG_MCC_MODE */ + pwdinfo->p2p_ps_state = p2p_ps_state; + +#ifdef CONFIG_LPS + if (pwdinfo->ctwindow > 0) { + if (pwrpriv->smart_ps != 0) { + pwrpriv->smart_ps = 0; + RTW_INFO("%s(): Enter CTW, change SmartPS\n", __FUNCTION__); + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) + rtw_exec_lps(padapter, pwrpriv->pwr_mode); + } + } +#endif /* CONFIG_LPS */ + + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + } + break; + case P2P_PS_SCAN: + case P2P_PS_SCAN_DONE: + case P2P_PS_ALLSTASLEEP: + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) { + pwdinfo->p2p_ps_state = p2p_ps_state; + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + } + break; + default: + break; + } + +#ifdef CONFIG_MCC_MODE + rtw_hal_mcc_process_noa(padapter); +#endif /* CONFIG_MCC_MODE */ +} + +u8 p2p_ps_wk_cmd(_adapter *padapter, u8 p2p_ps_state, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) +#ifdef CONFIG_CONCURRENT_MODE +#ifndef CONFIG_FW_MULTI_PORT_SUPPORT + || (padapter->hw_port != HW_PORT0) +#endif +#endif + ) + return res; + + if (enqueue) { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID; + pdrvextra_cmd_parm->type = p2p_ps_state; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + } else + p2p_ps_wk_hdl(padapter, p2p_ps_state); + +exit: + + + return res; + +} +#endif /* CONFIG_P2P_PS */ + +static void reset_ch_sitesurvey_timer_process(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + RTW_INFO("[%s] In\n", __FUNCTION__); + /* Reset the operation channel information */ + pwdinfo->rx_invitereq_info.operation_ch[0] = 0; +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->rx_invitereq_info.operation_ch[1] = 0; + pwdinfo->rx_invitereq_info.operation_ch[2] = 0; + pwdinfo->rx_invitereq_info.operation_ch[3] = 0; +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->rx_invitereq_info.scan_op_ch_only = 0; +} + +static void reset_ch_sitesurvey_timer_process2(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + RTW_INFO("[%s] In\n", __FUNCTION__); + /* Reset the operation channel information */ + pwdinfo->p2p_info.operation_ch[0] = 0; +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->p2p_info.operation_ch[1] = 0; + pwdinfo->p2p_info.operation_ch[2] = 0; + pwdinfo->p2p_info.operation_ch[3] = 0; +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->p2p_info.scan_op_ch_only = 0; +} + +static void restore_p2p_state_timer_process(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + p2p_protocol_wk_cmd(adapter, P2P_RESTORE_STATE_WK); +} + +static void pre_tx_scan_timer_process(void *FunctionContext) +{ + _adapter *adapter = (_adapter *) FunctionContext; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + _irqL irqL; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + _enter_critical_bh(&pmlmepriv->lock, &irqL); + + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) { + if (_TRUE == pwdinfo->tx_prov_disc_info.benable) { /* the provision discovery request frame is trigger to send or not */ + p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK); + /* issue_probereq_p2p(adapter, NULL); */ + /* _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT ); */ + } + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) { + if (_TRUE == pwdinfo->nego_req_info.benable) + p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK); + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) { + if (_TRUE == pwdinfo->invitereq_info.benable) + p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK); + } else + RTW_INFO("[%s] p2p_state is %d, ignore!!\n", __FUNCTION__, rtw_p2p_state(pwdinfo)); + + _exit_critical_bh(&pmlmepriv->lock, &irqL); +} + +static void find_phase_timer_process(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + adapter->wdinfo.find_phase_state_exchange_cnt++; + + p2p_protocol_wk_cmd(adapter, P2P_FIND_PHASE_WK); +} + +void reset_global_wifidirect_info(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo; + + pwdinfo = &padapter->wdinfo; + pwdinfo->persistent_supported = 0; + pwdinfo->session_available = _TRUE; + rtw_tdls_wfd_enable(padapter, 0); + pwdinfo->wfd_tdls_weaksec = _TRUE; +} + +#ifdef CONFIG_WFD +int rtw_init_wifi_display_info(_adapter *padapter) +{ + int res = _SUCCESS; + struct wifi_display_info *pwfd_info = &padapter->wfd_info; + + /* Used in P2P and TDLS */ + pwfd_info->init_rtsp_ctrlport = 554; +#ifdef CONFIG_IOCTL_CFG80211 + pwfd_info->rtsp_ctrlport = 0; +#else + pwfd_info->rtsp_ctrlport = pwfd_info->init_rtsp_ctrlport; /* set non-zero value for legacy wfd */ +#endif + pwfd_info->tdls_rtsp_ctrlport = 0; + pwfd_info->peer_rtsp_ctrlport = 0; /* Reset to 0 */ + pwfd_info->wfd_enable = _FALSE; + pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK; + pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY; + + /* Used in P2P */ + pwfd_info->peer_session_avail = _TRUE; + pwfd_info->wfd_pc = _FALSE; + + /* Used in TDLS */ + _rtw_memset(pwfd_info->ip_address, 0x00, 4); + _rtw_memset(pwfd_info->peer_ip_address, 0x00, 4); + return res; + +} + +inline void rtw_wfd_enable(_adapter *adapter, bool on) +{ + struct wifi_display_info *wfdinfo = &adapter->wfd_info; + + if (on) { + wfdinfo->rtsp_ctrlport = wfdinfo->init_rtsp_ctrlport; + wfdinfo->wfd_enable = _TRUE; + + } else { + wfdinfo->wfd_enable = _FALSE; + wfdinfo->rtsp_ctrlport = 0; + } +} + +inline void rtw_wfd_set_ctrl_port(_adapter *adapter, u16 port) +{ + struct wifi_display_info *wfdinfo = &adapter->wfd_info; + + wfdinfo->init_rtsp_ctrlport = port; + if (wfdinfo->wfd_enable == _TRUE) + wfdinfo->rtsp_ctrlport = port; + if (adapter->wdinfo.wfd_tdls_enable == 1) + wfdinfo->tdls_rtsp_ctrlport = port; +} + +inline void rtw_tdls_wfd_enable(_adapter *adapter, bool on) +{ + struct wifi_display_info *wfdinfo = &adapter->wfd_info; + + if (on) { + wfdinfo->tdls_rtsp_ctrlport = wfdinfo->init_rtsp_ctrlport; + adapter->wdinfo.wfd_tdls_enable = 1; + + } else { + adapter->wdinfo.wfd_tdls_enable = 0; + wfdinfo->tdls_rtsp_ctrlport = 0; + } +} + +u32 rtw_append_beacon_wfd_ie(_adapter *adapter, u8 *pbuf) +{ + struct wifidirect_info *wdinfo = &adapter->wdinfo; + struct mlme_priv *mlme = &adapter->mlmepriv; + u8 build_ie_by_self = 0; + u32 len = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + goto exit; + +#ifdef CONFIG_IOCTL_CFG80211 + if (_TRUE == wdinfo->wfd_info->wfd_enable) +#endif + build_ie_by_self = 1; + + if (build_ie_by_self) + len = build_beacon_wfd_ie(wdinfo, pbuf); +#ifdef CONFIG_IOCTL_CFG80211 + else if (mlme->wfd_beacon_ie && mlme->wfd_beacon_ie_len > 0) { + len = mlme->wfd_beacon_ie_len; + _rtw_memcpy(pbuf, mlme->wfd_beacon_ie, len); + } +#endif + +exit: + return len; +} + +u32 rtw_append_probe_req_wfd_ie(_adapter *adapter, u8 *pbuf) +{ + struct wifidirect_info *wdinfo = &adapter->wdinfo; + struct mlme_priv *mlme = &adapter->mlmepriv; + u8 build_ie_by_self = 0; + u32 len = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + goto exit; + +#ifdef CONFIG_IOCTL_CFG80211 + if (_TRUE == wdinfo->wfd_info->wfd_enable) +#endif + build_ie_by_self = 1; + + if (build_ie_by_self) + len = build_probe_req_wfd_ie(wdinfo, pbuf); +#ifdef CONFIG_IOCTL_CFG80211 + else if (mlme->wfd_probe_req_ie && mlme->wfd_probe_req_ie_len > 0) { + len = mlme->wfd_probe_req_ie_len; + _rtw_memcpy(pbuf, mlme->wfd_probe_req_ie, len); + } +#endif + +exit: + return len; +} + +u32 rtw_append_probe_resp_wfd_ie(_adapter *adapter, u8 *pbuf) +{ + struct wifidirect_info *wdinfo = &adapter->wdinfo; + struct mlme_priv *mlme = &adapter->mlmepriv; + u8 build_ie_by_self = 0; + u32 len = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + goto exit; + +#ifdef CONFIG_IOCTL_CFG80211 + if (_TRUE == wdinfo->wfd_info->wfd_enable) +#endif + build_ie_by_self = 1; + + if (build_ie_by_self) + len = build_probe_resp_wfd_ie(wdinfo, pbuf, 0); +#ifdef CONFIG_IOCTL_CFG80211 + else if (mlme->wfd_probe_resp_ie && mlme->wfd_probe_resp_ie_len > 0) { + len = mlme->wfd_probe_resp_ie_len; + _rtw_memcpy(pbuf, mlme->wfd_probe_resp_ie, len); + } +#endif + +exit: + return len; +} + +u32 rtw_append_assoc_req_wfd_ie(_adapter *adapter, u8 *pbuf) +{ + struct wifidirect_info *wdinfo = &adapter->wdinfo; + struct mlme_priv *mlme = &adapter->mlmepriv; + u8 build_ie_by_self = 0; + u32 len = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + goto exit; + +#ifdef CONFIG_IOCTL_CFG80211 + if (_TRUE == wdinfo->wfd_info->wfd_enable) +#endif + build_ie_by_self = 1; + + if (build_ie_by_self) + len = build_assoc_req_wfd_ie(wdinfo, pbuf); +#ifdef CONFIG_IOCTL_CFG80211 + else if (mlme->wfd_assoc_req_ie && mlme->wfd_assoc_req_ie_len > 0) { + len = mlme->wfd_assoc_req_ie_len; + _rtw_memcpy(pbuf, mlme->wfd_assoc_req_ie, len); + } +#endif + +exit: + return len; +} + +u32 rtw_append_assoc_resp_wfd_ie(_adapter *adapter, u8 *pbuf) +{ + struct wifidirect_info *wdinfo = &adapter->wdinfo; + struct mlme_priv *mlme = &adapter->mlmepriv; + u8 build_ie_by_self = 0; + u32 len = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + goto exit; + +#ifdef CONFIG_IOCTL_CFG80211 + if (_TRUE == wdinfo->wfd_info->wfd_enable) +#endif + build_ie_by_self = 1; + + if (build_ie_by_self) + len = build_assoc_resp_wfd_ie(wdinfo, pbuf); +#ifdef CONFIG_IOCTL_CFG80211 + else if (mlme->wfd_assoc_resp_ie && mlme->wfd_assoc_resp_ie_len > 0) { + len = mlme->wfd_assoc_resp_ie_len; + _rtw_memcpy(pbuf, mlme->wfd_assoc_resp_ie, len); + } +#endif + +exit: + return len; +} + +#endif /* CONFIG_WFD */ + +void rtw_init_wifidirect_timers(_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + rtw_init_timer(&pwdinfo->find_phase_timer, padapter, find_phase_timer_process, padapter); + rtw_init_timer(&pwdinfo->restore_p2p_state_timer, padapter, restore_p2p_state_timer_process, padapter); + rtw_init_timer(&pwdinfo->pre_tx_scan_timer, padapter, pre_tx_scan_timer_process, padapter); + rtw_init_timer(&pwdinfo->reset_ch_sitesurvey, padapter, reset_ch_sitesurvey_timer_process, padapter); + rtw_init_timer(&pwdinfo->reset_ch_sitesurvey2, padapter, reset_ch_sitesurvey_timer_process2, padapter); +} + +void rtw_init_wifidirect_addrs(_adapter *padapter, u8 *dev_addr, u8 *iface_addr) +{ +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + /*init device&interface address */ + if (dev_addr) + _rtw_memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN); + if (iface_addr) + _rtw_memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN); +#endif +} + +void init_wifidirect_info(_adapter *padapter, enum P2P_ROLE role) +{ + struct wifidirect_info *pwdinfo; +#ifdef CONFIG_WFD + struct wifi_display_info *pwfd_info = &padapter->wfd_info; +#endif + pwdinfo = &padapter->wdinfo; + + pwdinfo->padapter = padapter; + + /* 1, 6, 11 are the social channel defined in the WiFi Direct specification. */ + pwdinfo->social_chan[0] = 1; + pwdinfo->social_chan[1] = 6; + pwdinfo->social_chan[2] = 11; + pwdinfo->social_chan[3] = 0; /* channel 0 for scanning ending in site survey function. */ + + if (role != P2P_ROLE_DISABLE + && pwdinfo->driver_interface != DRIVER_CFG80211 + ) { + #ifdef CONFIG_CONCURRENT_MODE + u8 union_ch = 0; + + if (rtw_mi_check_status(padapter, MI_LINKED)) + union_ch = rtw_mi_get_union_chan(padapter); + + if (union_ch != 0 && + (union_ch == 1 || union_ch == 6 || union_ch == 11) + ) { + /* Use the AP's channel as the listen channel */ + /* This will avoid the channel switch between AP's channel and listen channel */ + pwdinfo->listen_channel = union_ch; + } else + #endif /* CONFIG_CONCURRENT_MODE */ + { + /* Use the channel 11 as the listen channel */ + pwdinfo->listen_channel = 11; + } + } + + if (role == P2P_ROLE_DEVICE) { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(padapter, MI_LINKED)) + rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE); + else +#endif + rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN); + + pwdinfo->intent = 1; + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN); + } else if (role == P2P_ROLE_CLIENT) { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + pwdinfo->intent = 1; + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + } else if (role == P2P_ROLE_GO) { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + pwdinfo->intent = 15; + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + } + + /* Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 ) */ + pwdinfo->support_rate[0] = 0x8c; /* 6(B) */ + pwdinfo->support_rate[1] = 0x92; /* 9(B) */ + pwdinfo->support_rate[2] = 0x18; /* 12 */ + pwdinfo->support_rate[3] = 0x24; /* 18 */ + pwdinfo->support_rate[4] = 0x30; /* 24 */ + pwdinfo->support_rate[5] = 0x48; /* 36 */ + pwdinfo->support_rate[6] = 0x60; /* 48 */ + pwdinfo->support_rate[7] = 0x6c; /* 54 */ + + _rtw_memcpy((void *) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7); + + _rtw_memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN); + pwdinfo->device_name_len = 0; + + _rtw_memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info)); + pwdinfo->invitereq_info.token = 3; /* Token used for P2P invitation request frame. */ + + _rtw_memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info)); + pwdinfo->inviteresp_info.token = 0; + + pwdinfo->profileindex = 0; + _rtw_memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM); + + rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE); + + pwdinfo->listen_dwell = (u8)((rtw_get_current_time() % 3) + 1); + /* RTW_INFO( "[%s] listen_dwell time is %d00ms\n", __FUNCTION__, pwdinfo->listen_dwell ); */ + + _rtw_memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info)); + pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE; + + _rtw_memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info)); + + pwdinfo->device_password_id_for_nego = WPS_DPID_PBC; + pwdinfo->negotiation_dialog_token = 1; + + _rtw_memset(pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN); + pwdinfo->nego_ssidlen = 0; + + pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO; +#ifdef CONFIG_WFD + pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC; + pwdinfo->wfd_info = pwfd_info; +#else + pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD; +#endif /* CONFIG_WFD */ + pwdinfo->channel_list_attr_len = 0; + _rtw_memset(pwdinfo->channel_list_attr, 0x00, 100); + + _rtw_memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4); + _rtw_memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3); + _rtw_memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info)); +#ifdef CONFIG_CONCURRENT_MODE +#ifdef CONFIG_IOCTL_CFG80211 + pwdinfo->ext_listen_interval = 1000; /* The interval to be available with legacy AP during p2p0-find/scan */ + pwdinfo->ext_listen_period = 3000; /* The time period to be available for P2P during nego */ +#else /* !CONFIG_IOCTL_CFG80211 */ + /* pwdinfo->ext_listen_interval = 3000; */ + /* pwdinfo->ext_listen_period = 400; */ + pwdinfo->ext_listen_interval = 1000; + pwdinfo->ext_listen_period = 1000; +#endif /* !CONFIG_IOCTL_CFG80211 */ +#endif + + /* Commented by Kurt 20130319 + * For WiDi purpose: Use CFG80211 interface but controled WFD/RDS frame by driver itself. */ +#ifdef CONFIG_IOCTL_CFG80211 + pwdinfo->driver_interface = DRIVER_CFG80211; +#else + pwdinfo->driver_interface = DRIVER_WEXT; +#endif /* CONFIG_IOCTL_CFG80211 */ + + pwdinfo->wfd_tdls_enable = 0; + _rtw_memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN); + _rtw_memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN); + + pwdinfo->rx_invitereq_info.operation_ch[0] = 0; + pwdinfo->rx_invitereq_info.operation_ch[1] = 0; /* Used to indicate the scan end in site survey function */ +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->rx_invitereq_info.operation_ch[2] = 0; + pwdinfo->rx_invitereq_info.operation_ch[3] = 0; + pwdinfo->rx_invitereq_info.operation_ch[4] = 0; +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->rx_invitereq_info.scan_op_ch_only = 0; + pwdinfo->p2p_info.operation_ch[0] = 0; + pwdinfo->p2p_info.operation_ch[1] = 0; /* Used to indicate the scan end in site survey function */ +#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH + pwdinfo->p2p_info.operation_ch[2] = 0; + pwdinfo->p2p_info.operation_ch[3] = 0; + pwdinfo->p2p_info.operation_ch[4] = 0; +#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */ + pwdinfo->p2p_info.scan_op_ch_only = 0; +} + +void _rtw_p2p_set_role(struct wifidirect_info *wdinfo, enum P2P_ROLE role) +{ + if (wdinfo->role != role) { + wdinfo->role = role; + rtw_mi_update_iface_status(&(wdinfo->padapter->mlmepriv), 0); + } +} + +#ifdef CONFIG_DBG_P2P + +/** + * rtw_p2p_role_txt - Get the p2p role name as a text string + * @role: P2P role + * Returns: The state name as a printable text string + */ +const char *rtw_p2p_role_txt(enum P2P_ROLE role) +{ + switch (role) { + case P2P_ROLE_DISABLE: + return "P2P_ROLE_DISABLE"; + case P2P_ROLE_DEVICE: + return "P2P_ROLE_DEVICE"; + case P2P_ROLE_CLIENT: + return "P2P_ROLE_CLIENT"; + case P2P_ROLE_GO: + return "P2P_ROLE_GO"; + default: + return "UNKNOWN"; + } +} + +/** + * rtw_p2p_state_txt - Get the p2p state name as a text string + * @state: P2P state + * Returns: The state name as a printable text string + */ +const char *rtw_p2p_state_txt(enum P2P_STATE state) +{ + switch (state) { + case P2P_STATE_NONE: + return "P2P_STATE_NONE"; + case P2P_STATE_IDLE: + return "P2P_STATE_IDLE"; + case P2P_STATE_LISTEN: + return "P2P_STATE_LISTEN"; + case P2P_STATE_SCAN: + return "P2P_STATE_SCAN"; + case P2P_STATE_FIND_PHASE_LISTEN: + return "P2P_STATE_FIND_PHASE_LISTEN"; + case P2P_STATE_FIND_PHASE_SEARCH: + return "P2P_STATE_FIND_PHASE_SEARCH"; + case P2P_STATE_TX_PROVISION_DIS_REQ: + return "P2P_STATE_TX_PROVISION_DIS_REQ"; + case P2P_STATE_RX_PROVISION_DIS_RSP: + return "P2P_STATE_RX_PROVISION_DIS_RSP"; + case P2P_STATE_RX_PROVISION_DIS_REQ: + return "P2P_STATE_RX_PROVISION_DIS_REQ"; + case P2P_STATE_GONEGO_ING: + return "P2P_STATE_GONEGO_ING"; + case P2P_STATE_GONEGO_OK: + return "P2P_STATE_GONEGO_OK"; + case P2P_STATE_GONEGO_FAIL: + return "P2P_STATE_GONEGO_FAIL"; + case P2P_STATE_RECV_INVITE_REQ_MATCH: + return "P2P_STATE_RECV_INVITE_REQ_MATCH"; + case P2P_STATE_PROVISIONING_ING: + return "P2P_STATE_PROVISIONING_ING"; + case P2P_STATE_PROVISIONING_DONE: + return "P2P_STATE_PROVISIONING_DONE"; + case P2P_STATE_TX_INVITE_REQ: + return "P2P_STATE_TX_INVITE_REQ"; + case P2P_STATE_RX_INVITE_RESP_OK: + return "P2P_STATE_RX_INVITE_RESP_OK"; + case P2P_STATE_RECV_INVITE_REQ_DISMATCH: + return "P2P_STATE_RECV_INVITE_REQ_DISMATCH"; + case P2P_STATE_RECV_INVITE_REQ_GO: + return "P2P_STATE_RECV_INVITE_REQ_GO"; + case P2P_STATE_RECV_INVITE_REQ_JOIN: + return "P2P_STATE_RECV_INVITE_REQ_JOIN"; + case P2P_STATE_RX_INVITE_RESP_FAIL: + return "P2P_STATE_RX_INVITE_RESP_FAIL"; + case P2P_STATE_RX_INFOR_NOREADY: + return "P2P_STATE_RX_INFOR_NOREADY"; + case P2P_STATE_TX_INFOR_NOREADY: + return "P2P_STATE_TX_INFOR_NOREADY"; + default: + return "UNKNOWN"; + } +} + +void dbg_rtw_p2p_set_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line) +{ + if (!_rtw_p2p_chk_state(wdinfo, state)) { + enum P2P_STATE old_state = _rtw_p2p_state(wdinfo); + _rtw_p2p_set_state(wdinfo, state); + RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_state from %s to %s\n", caller, line + , rtw_p2p_state_txt(old_state), rtw_p2p_state_txt(_rtw_p2p_state(wdinfo)) + ); + } else { + RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_state to same state %s\n", caller, line + , rtw_p2p_state_txt(_rtw_p2p_state(wdinfo)) + ); + } +} +void dbg_rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line) +{ + if (_rtw_p2p_pre_state(wdinfo) != state) { + enum P2P_STATE old_state = _rtw_p2p_pre_state(wdinfo); + _rtw_p2p_set_pre_state(wdinfo, state); + RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_pre_state from %s to %s\n", caller, line + , rtw_p2p_state_txt(old_state), rtw_p2p_state_txt(_rtw_p2p_pre_state(wdinfo)) + ); + } else { + RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_pre_state to same state %s\n", caller, line + , rtw_p2p_state_txt(_rtw_p2p_pre_state(wdinfo)) + ); + } +} +#if 0 +void dbg_rtw_p2p_restore_state(struct wifidirect_info *wdinfo, const char *caller, int line) +{ + if (wdinfo->pre_p2p_state != -1) { + RTW_INFO("[CONFIG_DBG_P2P]%s:%d restore from %s to %s\n", caller, line + , p2p_state_str[wdinfo->p2p_state], p2p_state_str[wdinfo->pre_p2p_state] + ); + _rtw_p2p_restore_state(wdinfo); + } else { + RTW_INFO("[CONFIG_DBG_P2P]%s:%d restore no pre state, cur state %s\n", caller, line + , p2p_state_str[wdinfo->p2p_state] + ); + } +} +#endif +void dbg_rtw_p2p_set_role(struct wifidirect_info *wdinfo, enum P2P_ROLE role, const char *caller, int line) +{ + if (wdinfo->role != role) { + enum P2P_ROLE old_role = wdinfo->role; + _rtw_p2p_set_role(wdinfo, role); + RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_role from %s to %s\n", caller, line + , rtw_p2p_role_txt(old_role), rtw_p2p_role_txt(wdinfo->role) + ); + } else { + RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_role to same role %s\n", caller, line + , rtw_p2p_role_txt(wdinfo->role) + ); + } +} +#endif /* CONFIG_DBG_P2P */ + + +int rtw_p2p_enable(_adapter *padapter, enum P2P_ROLE role) +{ + int ret = _SUCCESS; + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#ifdef CONFIG_CONCURRENT_MODE + struct roch_info *prochinfo = &padapter->rochinfo; +#endif + + if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || role == P2P_ROLE_GO) { +#if defined(CONFIG_CONCURRENT_MODE) && (!defined(RTW_P2P_GROUP_INTERFACE) || !RTW_P2P_GROUP_INTERFACE) + /* Commented by Albert 2011/12/30 */ + /* The driver just supports 1 P2P group operation. */ + /* So, this function will do nothing if the buddy adapter had enabled the P2P function. */ + /*if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE)) + return ret;*/ + /* Only selected interface can be P2P interface */ + if (padapter->iface_id != padapter->registrypriv.sel_p2p_iface) { + RTW_ERR("%s, iface_id:%d is not P2P interface!\n", __func__, padapter->iface_id); + ret = _FAIL; + return ret; + } +#endif /* CONFIG_CONCURRENT_MODE */ + + /* leave IPS/Autosuspend */ + if (_FAIL == rtw_pwr_wakeup(padapter)) { + ret = _FAIL; + goto exit; + } + + /* Added by Albert 2011/03/22 */ + /* In the P2P mode, the driver should not support the b mode. */ + /* So, the Tx packet shouldn't use the CCK rate */ + #ifdef CONFIG_IOCTL_CFG80211 + if (rtw_cfg80211_iface_has_p2p_group_cap(padapter)) + #endif + update_tx_basic_rate(padapter, WIRELESS_11AGN); + + /* Enable P2P function */ + init_wifidirect_info(padapter, role); + + #ifdef CONFIG_IOCTL_CFG80211 + if (padapter->wdinfo.driver_interface == DRIVER_CFG80211) + adapter_wdev_data(padapter)->p2p_enabled = _TRUE; + #endif + + rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, _TRUE); +#ifdef CONFIG_WFD + if (hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + rtw_hal_set_odm_var(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, _TRUE); +#endif + + } else if (role == P2P_ROLE_DISABLE) { + + #ifdef CONFIG_IOCTL_CFG80211 + if (padapter->wdinfo.driver_interface == DRIVER_CFG80211) + adapter_wdev_data(padapter)->p2p_enabled = _FALSE; + #endif + + pwdinfo->listen_channel = 0; + + /* Disable P2P function */ + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) { + _cancel_timer_ex(&pwdinfo->find_phase_timer); + _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer); + _cancel_timer_ex(&pwdinfo->pre_tx_scan_timer); + _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey); + _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey2); + reset_ch_sitesurvey_timer_process(padapter); + reset_ch_sitesurvey_timer_process2(padapter); +#ifdef CONFIG_CONCURRENT_MODE + _cancel_timer_ex(&prochinfo->ap_roch_ch_switch_timer); +#endif + rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_NONE); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE); + _rtw_memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info)); + + /* Remove profiles in wifidirect_info structure. */ + _rtw_memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM); + pwdinfo->profileindex = 0; + } + + rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, _FALSE); +#ifdef CONFIG_WFD + if (hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + rtw_hal_set_odm_var(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, _FALSE); +#endif + + if (_FAIL == rtw_pwr_wakeup(padapter)) { + ret = _FAIL; + goto exit; + } + + /* Restore to initial setting. */ + update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode); + + /* For WiDi purpose. */ +#ifdef CONFIG_IOCTL_CFG80211 + pwdinfo->driver_interface = DRIVER_CFG80211; +#else + pwdinfo->driver_interface = DRIVER_WEXT; +#endif /* CONFIG_IOCTL_CFG80211 */ + + } + +exit: + return ret; +} + +#endif /* CONFIG_P2P */ diff --git a/core/rtw_pwrctrl.c b/core/rtw_pwrctrl.c new file mode 100644 index 0000000..03dfaf9 --- /dev/null +++ b/core/rtw_pwrctrl.c @@ -0,0 +1,2955 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_PWRCTRL_C_ + +#include +#include +#include + +#ifdef DBG_CHECK_FW_PS_STATE +int rtw_fw_ps_state(PADAPTER padapter) +{ + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + int ret = _FAIL, dont_care = 0; + u16 fw_ps_state = 0; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct registry_priv *registry_par = &padapter->registrypriv; + + if (registry_par->check_fw_ps != 1) + return _SUCCESS; + + _enter_pwrlock(&pwrpriv->check_32k_lock); + + if (RTW_CANNOT_RUN(padapter)) { + RTW_INFO("%s: bSurpriseRemoved=%s , hw_init_completed=%d, bDriverStopped=%s\n", __func__ + , rtw_is_surprise_removed(padapter) ? "True" : "False" + , rtw_get_hw_init_completed(padapter) + , rtw_is_drv_stopped(padapter) ? "True" : "False"); + goto exit_fw_ps_state; + } + #if defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) + rtw_hal_get_hwreg(padapter, HW_VAR_FW_PS_STATE, (u8 *)&fw_ps_state); + if ((fw_ps_state & BIT_LPS_STATUS) == 0) + ret = _SUCCESS; + else { + pdbgpriv->dbg_poll_fail_cnt++; + RTW_INFO("%s: fw_ps_state=%04x\n", __FUNCTION__, fw_ps_state); + } + #else + rtw_hal_set_hwreg(padapter, HW_VAR_SET_REQ_FW_PS, (u8 *)&dont_care); + { + /* 4. if 0x88[7]=1, driver set cmd to leave LPS/IPS. */ + /* Else, hw will keep in active mode. */ + /* debug info: */ + /* 0x88[7] = 32kpermission, */ + /* 0x88[6:0] = current_ps_state */ + /* 0x89[7:0] = last_rpwm */ + + rtw_hal_get_hwreg(padapter, HW_VAR_FW_PS_STATE, (u8 *)&fw_ps_state); + + if ((fw_ps_state & 0x80) == 0) + ret = _SUCCESS; + else { + pdbgpriv->dbg_poll_fail_cnt++; + RTW_INFO("%s: fw_ps_state=%04x\n", __FUNCTION__, fw_ps_state); + } + } + #endif + +exit_fw_ps_state: + _exit_pwrlock(&pwrpriv->check_32k_lock); + return ret; +} +#endif /*DBG_CHECK_FW_PS_STATE*/ +#ifdef CONFIG_IPS +void _ips_enter(_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + pwrpriv->bips_processing = _TRUE; + + /* syn ips_mode with request */ + pwrpriv->ips_mode = pwrpriv->ips_mode_req; + + pwrpriv->ips_enter_cnts++; + RTW_INFO("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts); + + if (rf_off == pwrpriv->change_rfpwrstate) { + pwrpriv->bpower_saving = _TRUE; + RTW_PRINT("nolinked power save enter\n"); + + if (pwrpriv->ips_mode == IPS_LEVEL_2) + pwrpriv->bkeepfwalive = _TRUE; + +#ifdef CONFIG_RTW_CFGVENDOR_LLSTATS + pwrpriv->pwr_saving_start_time = rtw_get_current_time(); +#endif /* CONFIG_RTW_CFGVENDOR_LLSTATS */ + + rtw_ips_pwr_down(padapter); + pwrpriv->rf_pwrstate = rf_off; + } + pwrpriv->bips_processing = _FALSE; + +} + +void ips_enter(_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req); +#endif /* CONFIG_BT_COEXIST */ + + _enter_pwrlock(&pwrpriv->lock); + _ips_enter(padapter); + _exit_pwrlock(&pwrpriv->lock); + +#ifdef CONFIG_PCI_DYNAMIC_ASPM + rtw_pci_dynamic_aspm_set_mode(padapter, ASPM_MODE_PS); +#endif +} + +int _ips_leave(_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + int result = _SUCCESS; + + if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) { + pwrpriv->bips_processing = _TRUE; + pwrpriv->change_rfpwrstate = rf_on; + pwrpriv->ips_leave_cnts++; + RTW_INFO("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts); + + result = rtw_ips_pwr_up(padapter); + if (result == _SUCCESS) + pwrpriv->rf_pwrstate = rf_on; + +#ifdef CONFIG_RTW_CFGVENDOR_LLSTATS + pwrpriv->pwr_saving_time += rtw_get_passing_time_ms(pwrpriv->pwr_saving_start_time); +#endif /* CONFIG_RTW_CFGVENDOR_LLSTATS */ + + RTW_PRINT("nolinked power save leave\n"); + + RTW_INFO("==> ips_leave.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c)); + pwrpriv->bips_processing = _FALSE; + + pwrpriv->bkeepfwalive = _FALSE; + pwrpriv->bpower_saving = _FALSE; + } + + return result; +} + +int ips_leave(_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); +#ifdef DBG_CHECK_FW_PS_STATE + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; +#endif + int ret; + + if (!is_primary_adapter(padapter)) + return _SUCCESS; + + _enter_pwrlock(&pwrpriv->lock); + ret = _ips_leave(padapter); +#ifdef DBG_CHECK_FW_PS_STATE + if (rtw_fw_ps_state(padapter) == _FAIL) { + RTW_INFO("ips leave doesn't leave 32k\n"); + pdbgpriv->dbg_leave_ips_fail_cnt++; + } +#endif /* DBG_CHECK_FW_PS_STATE */ + _exit_pwrlock(&pwrpriv->lock); + +#ifdef CONFIG_PCI_DYNAMIC_ASPM + rtw_pci_dynamic_aspm_set_mode(padapter, ASPM_MODE_PERF); +#endif + + if (_SUCCESS == ret) + odm_dm_reset(&GET_HAL_DATA(padapter)->odmpriv); + +#ifdef CONFIG_BT_COEXIST + if (_SUCCESS == ret) + rtw_btcoex_IpsNotify(padapter, IPS_NONE); +#endif /* CONFIG_BT_COEXIST */ + + return ret; +} +#endif /* CONFIG_IPS */ + +#ifdef SUPPORT_HW_RFOFF_DETECTED + int rtw_hw_suspend(_adapter *padapter); + int rtw_hw_resume(_adapter *padapter); +#endif + +bool rtw_pwr_unassociated_idle(_adapter *adapter) +{ + u8 i; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct xmit_priv *pxmit_priv = &adapter->xmitpriv; + struct mlme_priv *pmlmepriv; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo; +#endif + + bool ret = _FALSE; + + if (adapter_to_pwrctl(adapter)->bpower_saving == _TRUE) { + /* RTW_INFO("%s: already in LPS or IPS mode\n", __func__); */ + goto exit; + } + + if (rtw_time_after(adapter_to_pwrctl(adapter)->ips_deny_time, rtw_get_current_time())) { + /* RTW_INFO("%s ips_deny_time\n", __func__); */ + goto exit; + } + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + pmlmepriv = &(iface->mlmepriv); +#ifdef CONFIG_P2P + pwdinfo = &(iface->wdinfo); +#endif + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE | WIFI_UNDER_SURVEY) + || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS) + || MLME_IS_AP(iface) + || MLME_IS_MESH(iface) + || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE) + #if defined(CONFIG_IOCTL_CFG80211) + || rtw_cfg80211_get_is_roch(iface) == _TRUE + || (rtw_cfg80211_is_ro_ch_once(adapter) + && rtw_cfg80211_get_last_ro_ch_passing_ms(adapter) < 3000) + #elif defined(CONFIG_P2P) + || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) + || rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) + #endif + ) + goto exit; + + } + } + +#if (MP_DRIVER == 1) + if (adapter->registrypriv.mp_mode == 1) + goto exit; +#endif + + if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF || + pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) { + RTW_PRINT("There are some pkts to transmit\n"); + RTW_PRINT("free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n", + pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt); + goto exit; + } + + ret = _TRUE; + +exit: + return ret; +} + + +/* + * ATTENTION: + * rtw_ps_processor() doesn't handle LPS. + */ +void rtw_ps_processor(_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; +#ifdef SUPPORT_HW_RFOFF_DETECTED + rt_rf_power_state rfpwrstate; +#endif /* SUPPORT_HW_RFOFF_DETECTED */ + u32 ps_deny = 0; + + _enter_pwrlock(&adapter_to_pwrctl(padapter)->lock); + ps_deny = rtw_ps_deny_get(padapter); + _exit_pwrlock(&adapter_to_pwrctl(padapter)->lock); + if (ps_deny != 0) { + if (!MLME_IS_MONITOR(padapter)) { + RTW_INFO(FUNC_ADPT_FMT ": ps_deny=0x%08X, skip power save!\n", + FUNC_ADPT_ARG(padapter), ps_deny); + } + goto exit; + } + + if (pwrpriv->bInSuspend == _TRUE) { /* system suspend or autosuspend */ + pdbgpriv->dbg_ps_insuspend_cnt++; + RTW_INFO("%s, pwrpriv->bInSuspend == _TRUE ignore this process\n", __FUNCTION__); + return; + } + + pwrpriv->ps_processing = _TRUE; + +#ifdef SUPPORT_HW_RFOFF_DETECTED + if (pwrpriv->bips_processing == _TRUE) + goto exit; + + /* RTW_INFO("==> fw report state(0x%x)\n",rtw_read8(padapter,0x1ca)); */ + if (pwrpriv->bHWPwrPindetect) { + + rfpwrstate = RfOnOffDetect(padapter); + RTW_INFO("@@@@- #2 %s==> rfstate:%s\n", __FUNCTION__, (rfpwrstate == rf_on) ? "rf_on" : "rf_off"); + + if (rfpwrstate != pwrpriv->rf_pwrstate) { + if (rfpwrstate == rf_off) { + pwrpriv->change_rfpwrstate = rf_off; + pwrpriv->brfoffbyhw = _TRUE; + rtw_hw_suspend(padapter); + } else { + pwrpriv->change_rfpwrstate = rf_on; + rtw_hw_resume(padapter); + } + RTW_INFO("current rf_pwrstate(%s)\n", (pwrpriv->rf_pwrstate == rf_off) ? "rf_off" : "rf_on"); + } + + pwrpriv->pwr_state_check_cnts++; + } +#endif /* SUPPORT_HW_RFOFF_DETECTED */ + + if (pwrpriv->ips_mode_req == IPS_NONE) + goto exit; + + if (rtw_pwr_unassociated_idle(padapter) == _FALSE) + goto exit; + + if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts % 4) == 0)) { + RTW_INFO("==>%s .fw_state(%x)\n", __FUNCTION__, get_fwstate(pmlmepriv)); + pwrpriv->change_rfpwrstate = rf_off; + +#ifdef CONFIG_IPS + ips_enter(padapter); +#endif + + } +exit: +#ifndef CONFIG_IPS_CHECK_IN_WD + rtw_set_pwr_state_check_timer(pwrpriv); +#endif + pwrpriv->ps_processing = _FALSE; + return; +} + +void pwr_state_check_handler(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + rtw_ps_cmd(padapter); +} + +#ifdef CONFIG_LPS +#ifdef CONFIG_CHECK_LEAVE_LPS +#ifdef CONFIG_LPS_CHK_BY_TP +void traffic_check_for_leave_lps_by_tp(PADAPTER padapter, u8 tx, struct sta_info *sta) +{ + struct stainfo_stats *pstats = &sta->sta_stats; + u64 cur_acc_tx_bytes = 0, cur_acc_rx_bytes = 0; + u32 tx_tp_kbyte = 0, rx_tp_kbyte = 0; + u32 tx_tp_th = 0, rx_tp_th = 0; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 leave_lps = _FALSE; + + if (tx) { /* from tx */ + cur_acc_tx_bytes = pstats->tx_bytes - pstats->acc_tx_bytes; + tx_tp_kbyte = cur_acc_tx_bytes >> 10; + tx_tp_th = pwrpriv->lps_tx_tp_th * 1024 / 8 * 2; /*KBytes @2s*/ + + if (tx_tp_kbyte >= tx_tp_th || + padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod >= pwrpriv->lps_tx_pkts){ + if (pwrpriv->bLeisurePs + && (pwrpriv->pwr_mode != PS_MODE_ACTIVE) + #ifdef CONFIG_BT_COEXIST + && (rtw_btcoex_IsBtControlLps(padapter) == _FALSE) + #endif + ) { + leave_lps = _TRUE; + } + } + + } else { /* from rx path */ + cur_acc_rx_bytes = pstats->rx_bytes - pstats->acc_rx_bytes; + rx_tp_kbyte = cur_acc_rx_bytes >> 10; + rx_tp_th = pwrpriv->lps_rx_tp_th * 1024 / 8 * 2; + + if (rx_tp_kbyte>= rx_tp_th || + padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod >= pwrpriv->lps_rx_pkts) { + if (pwrpriv->bLeisurePs + && (pwrpriv->pwr_mode != PS_MODE_ACTIVE) + #ifdef CONFIG_BT_COEXIST + && (rtw_btcoex_IsBtControlLps(padapter) == _FALSE) + #endif + ) { + leave_lps = _TRUE; + } + } + } + + if (leave_lps) { + #ifdef DBG_LPS_CHK_BY_TP + RTW_INFO("leave lps via %s, ", tx ? "Tx" : "Rx"); + if (tx) + RTW_INFO("Tx = %d [%d] (KB)\n", tx_tp_kbyte, tx_tp_th); + else + RTW_INFO("Rx = %d [%d] (KB)\n", rx_tp_kbyte, rx_tp_th); + #endif + pwrpriv->lps_chk_cnt = pwrpriv->lps_chk_cnt_th; + /* rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 0); */ + rtw_lps_ctrl_wk_cmd(padapter, tx ? LPS_CTRL_TX_TRAFFIC_LEAVE : LPS_CTRL_RX_TRAFFIC_LEAVE, 0); + } +} +#endif /*CONFIG_LPS_CHK_BY_TP*/ + +void traffic_check_for_leave_lps(PADAPTER padapter, u8 tx, u32 tx_packets) +{ + static systime start_time = 0; + static u32 xmit_cnt = 0; + u8 bLeaveLPS = _FALSE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + + + if (tx) { /* from tx */ + xmit_cnt += tx_packets; + + if (start_time == 0) + start_time = rtw_get_current_time(); + + if (rtw_get_passing_time_ms(start_time) > 2000) { /* 2 sec == watch dog timer */ + if (xmit_cnt > 8) { + if ((adapter_to_pwrctl(padapter)->bLeisurePs) + && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE) +#ifdef CONFIG_BT_COEXIST + && (rtw_btcoex_IsBtControlLps(padapter) == _FALSE) +#endif + ) { + /* RTW_INFO("leave lps via Tx = %d\n", xmit_cnt); */ + bLeaveLPS = _TRUE; + } + } + + start_time = rtw_get_current_time(); + xmit_cnt = 0; + } + + } else { /* from rx path */ + if (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4/*2*/) { + if ((adapter_to_pwrctl(padapter)->bLeisurePs) + && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE) +#ifdef CONFIG_BT_COEXIST + && (rtw_btcoex_IsBtControlLps(padapter) == _FALSE) +#endif + ) { + /* RTW_INFO("leave lps via Rx = %d\n", pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */ + bLeaveLPS = _TRUE; + } + } + } + + if (bLeaveLPS) { + /* RTW_INFO("leave lps via %s, Tx = %d, Rx = %d\n", tx?"Tx":"Rx", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod,pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */ + /* rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 0); */ + rtw_lps_ctrl_wk_cmd(padapter, tx ? LPS_CTRL_TX_TRAFFIC_LEAVE : LPS_CTRL_RX_TRAFFIC_LEAVE, tx ? RTW_CMDF_DIRECTLY : 0); + } +} +#endif /* CONFIG_CHECK_LEAVE_LPS */ + +#ifdef CONFIG_LPS_LCLK +#define LPS_CPWM_TIMEOUT_MS 10 /*ms*/ +#define LPS_RPWM_RETRY_CNT 3 + +u8 rtw_cpwm_polling(_adapter *adapter, u8 rpwm, u8 cpwm_orig) +{ + u8 rst = _FAIL; + u8 cpwm_now = 0; + systime start_time; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + #ifdef DBG_CHECK_FW_PS_STATE + struct debug_priv *pdbgpriv = &(adapter_to_dvobj(adapter)->drv_dbg); + #endif + + pwrpriv->rpwm_retry = 0; + + do { + start_time = rtw_get_current_time(); + do { + rtw_msleep_os(1); + rtw_hal_get_hwreg(adapter, HW_VAR_CPWM, &cpwm_now); + + if ((cpwm_orig ^ cpwm_now) & 0x80) { + pwrpriv->cpwm = PS_STATE_S4; + pwrpriv->cpwm_tog = cpwm_now & PS_TOGGLE; + rst = _SUCCESS; + break; + } + } while (rtw_get_passing_time_ms(start_time) < LPS_CPWM_TIMEOUT_MS && !RTW_CANNOT_RUN(adapter)); + + if (rst == _SUCCESS) + break; + else { + /* rpwm retry */ + cpwm_orig = cpwm_now; + rpwm &= ~PS_TOGGLE; + rpwm |= pwrpriv->tog; + rtw_hal_set_hwreg(adapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm)); + pwrpriv->tog += 0x80; + } + } while (pwrpriv->rpwm_retry++ < LPS_RPWM_RETRY_CNT && !RTW_CANNOT_RUN(adapter)); + + if (rst == _SUCCESS) { + #ifdef DBG_CHECK_FW_PS_STATE + RTW_INFO("%s: polling cpwm OK! rpwm_retry=%d, cpwm_orig=%02x, cpwm_now=%02x , 0x100=0x%x\n" + , __func__, pwrpriv->rpwm_retry, cpwm_orig, cpwm_now, rtw_read8(adapter, REG_CR)); + if (rtw_fw_ps_state(adapter) == _FAIL) { + RTW_INFO("leave 32k but fw state in 32k\n"); + pdbgpriv->dbg_rpwm_toogle_cnt++; + } + #endif /* DBG_CHECK_FW_PS_STATE */ + } else { + RTW_ERR("%s: polling cpwm timeout! rpwm_retry=%d, cpwm_orig=%02x, cpwm_now=%02x\n" + , __func__, pwrpriv->rpwm_retry, cpwm_orig, cpwm_now); + #ifdef DBG_CHECK_FW_PS_STATE + if (rtw_fw_ps_state(adapter) == _FAIL) { + RTW_INFO("rpwm timeout and fw ps state in 32k\n"); + pdbgpriv->dbg_rpwm_timeout_fail_cnt++; + } + #endif /* DBG_CHECK_FW_PS_STATE */ + + #ifdef CONFIG_LPS_RPWM_TIMER + _set_timer(&pwrpriv->pwr_rpwm_timer, 1); + #endif /* CONFIG_LPS_RPWM_TIMER */ + } + + return rst; +} +#endif +/* + * Description: + * This function MUST be called under power lock protect + * + * Parameters + * padapter + * pslv power state level, only could be PS_STATE_S0 ~ PS_STATE_S4 + * + */ +u8 rtw_set_rpwm(PADAPTER padapter, u8 pslv) +{ + u8 rpwm = 0xFF; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); +#ifdef CONFIG_LPS_LCLK + u8 cpwm_orig; +#endif + + pslv = PS_STATE(pslv); + +#ifdef CONFIG_LPS_RPWM_TIMER + if (pwrpriv->brpwmtimeout == _TRUE) + RTW_INFO("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __FUNCTION__, pslv); + else +#endif /* CONFIG_LPS_RPWM_TIMER */ + { + if ((pwrpriv->rpwm == pslv) +#ifdef CONFIG_LPS_LCLK + || ((pwrpriv->rpwm >= PS_STATE_S2) && (pslv >= PS_STATE_S2)) +#endif + || (pwrpriv->lps_level == LPS_NORMAL) + ) { + return rpwm; + } + } + + if (rtw_is_surprise_removed(padapter) || + (!rtw_is_hw_init_completed(padapter))) { + + pwrpriv->cpwm = PS_STATE_S4; + + return rpwm; + } + + if (rtw_is_drv_stopped(padapter)) + if (pslv < PS_STATE_S2) + return rpwm; + + rpwm = pslv | pwrpriv->tog; +#ifdef CONFIG_LPS_LCLK + /* only when from PS_STATE S0/S1 to S2 and higher needs ACK */ + if ((pwrpriv->cpwm < PS_STATE_S2) && (pslv >= PS_STATE_S2)) + rpwm |= PS_ACK; +#endif + + pwrpriv->rpwm = pslv; + +#ifdef CONFIG_LPS_LCLK + cpwm_orig = 0; + if (rpwm & PS_ACK) + rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig); +#endif + +#if defined(CONFIG_LPS_RPWM_TIMER) && !defined(CONFIG_DETECT_CPWM_BY_POLLING) + if (rpwm & PS_ACK) { + #if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) || defined(CONFIG_P2P_WOWLAN) + if (pwrpriv->wowlan_mode != _TRUE && + pwrpriv->wowlan_ap_mode != _TRUE && + pwrpriv->wowlan_p2p_mode != _TRUE) + #endif + _set_timer(&pwrpriv->pwr_rpwm_timer, LPS_CPWM_TIMEOUT_MS); + } +#endif /* CONFIG_LPS_RPWM_TIMER & !CONFIG_DETECT_CPWM_BY_POLLING */ + + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm)); + + pwrpriv->tog += 0x80; + +#ifdef CONFIG_LPS_LCLK + /* No LPS 32K, No Ack */ + if (rpwm & PS_ACK) { + #ifdef CONFIG_DETECT_CPWM_BY_POLLING + rtw_cpwm_polling(padapter, rpwm, cpwm_orig); + #else + #if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) || defined(CONFIG_P2P_WOWLAN) + if (pwrpriv->wowlan_mode == _TRUE || + pwrpriv->wowlan_ap_mode == _TRUE || + pwrpriv->wowlan_p2p_mode == _TRUE) + rtw_cpwm_polling(padapter, rpwm, cpwm_orig); + #endif /*#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) || defined(CONFIG_P2P_WOWLAN)*/ + #endif /*#ifdef CONFIG_DETECT_CPWM_BY_POLLING*/ + } else +#endif /* CONFIG_LPS_LCLK */ + { + pwrpriv->cpwm = pslv; + } + + return rpwm; +} + +u8 PS_RDY_CHECK(_adapter *padapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) + if (_TRUE == pwrpriv->bInSuspend && pwrpriv->wowlan_mode) + return _TRUE; + else if (_TRUE == pwrpriv->bInSuspend && pwrpriv->wowlan_ap_mode) + return _TRUE; + else if (_TRUE == pwrpriv->bInSuspend) + return _FALSE; +#else + if (_TRUE == pwrpriv->bInSuspend) + return _FALSE; +#endif + + if (rtw_time_after(pwrpriv->lps_deny_time, rtw_get_current_time())) + return _FALSE; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY) + || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS) + || MLME_IS_AP(padapter) + || MLME_IS_MESH(padapter) + || MLME_IS_MONITOR(padapter) + || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE) + #if defined(CONFIG_IOCTL_CFG80211) + || rtw_cfg80211_get_is_roch(padapter) == _TRUE + #endif + || rtw_is_scan_deny(padapter) + #ifdef CONFIG_TDLS + /* TDLS link is established. */ + || (padapter->tdlsinfo.link_established == _TRUE) + #endif /* CONFIG_TDLS */ + #ifdef CONFIG_DFS_MASTER + || adapter_to_rfctl(padapter)->radar_detect_enabled + #endif + ) + return _FALSE; + + if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == _FALSE)) { + RTW_INFO("Group handshake still in progress !!!\n"); + return _FALSE; + } + +#ifdef CONFIG_IOCTL_CFG80211 + if (!rtw_cfg80211_pwr_mgmt(padapter)) + return _FALSE; +#endif + + return _TRUE; +} + +#if defined(CONFIG_FWLPS_IN_IPS) +void rtw_set_fw_in_ips_mode(PADAPTER padapter, u8 enable) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + int cnt = 0; + systime start_time; + u8 val8 = 0; + u8 cpwm_orig = 0, cpwm_now = 0; + u8 parm[H2C_INACTIVE_PS_LEN] = {0}; + + if (padapter->netif_up == _FALSE) { + RTW_INFO("%s: ERROR, netif is down\n", __func__); + return; + } + + /* u8 cmd_param; */ /* BIT0:enable, BIT1:NoConnect32k */ + if (enable) { +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req); +#endif + /* Enter IPS */ + RTW_INFO("%s: issue H2C to FW when entering IPS\n", __func__); + + parm[0] = 0x1;/* suggest by Isaac.Hsu*/ +#ifdef CONFIG_PNO_SUPPORT + if (pwrpriv->pno_inited) { + parm[1] = pwrpriv->pnlo_info->fast_scan_iterations; + parm[2] = pwrpriv->pnlo_info->slow_scan_period; + } +#endif + + rtw_hal_fill_h2c_cmd(padapter, /* H2C_FWLPS_IN_IPS_, */ + H2C_INACTIVE_PS_, + H2C_INACTIVE_PS_LEN, parm); + /* poll 0x1cc to make sure H2C command already finished by FW; MAC_0x1cc=0 means H2C done by FW. */ + do { + val8 = rtw_read8(padapter, REG_HMETFR); + cnt++; + RTW_INFO("%s polling REG_HMETFR=0x%x, cnt=%d\n", + __func__, val8, cnt); + rtw_mdelay_os(10); + } while (cnt < 100 && (val8 != 0)); + +#ifdef CONFIG_LPS_LCLK + /* H2C done, enter 32k */ + if (val8 == 0) { + /* ser rpwm to enter 32k */ + rtw_hal_get_hwreg(padapter, HW_VAR_RPWM_TOG, &val8); + RTW_INFO("%s: read rpwm=%02x\n", __FUNCTION__, val8); + val8 += 0x80; + val8 |= BIT(0); + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&val8)); + RTW_INFO("%s: write rpwm=%02x\n", __FUNCTION__, val8); + adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80; + cnt = val8 = 0; + if (parm[1] == 0 || parm[2] == 0) { + do { + val8 = rtw_read8(padapter, REG_CR); + cnt++; + RTW_INFO("%s polling 0x100=0x%x, cnt=%d\n", + __func__, val8, cnt); + RTW_INFO("%s 0x08:%02x, 0x03:%02x\n", + __func__, + rtw_read8(padapter, 0x08), + rtw_read8(padapter, 0x03)); + rtw_mdelay_os(10); + } while (cnt < 20 && (val8 != 0xEA)); + } + } +#endif + } else { + /* Leave IPS */ + RTW_INFO("%s: Leaving IPS in FWLPS state\n", __func__); + +#ifdef CONFIG_LPS_LCLK + /* for polling cpwm */ + cpwm_orig = 0; + rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig); + + /* ser rpwm */ + rtw_hal_get_hwreg(padapter, HW_VAR_RPWM_TOG, &val8); + val8 += 0x80; + val8 |= BIT(6); + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&val8)); + RTW_INFO("%s: write rpwm=%02x\n", __FUNCTION__, val8); + adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80; + + /* do polling cpwm */ + start_time = rtw_get_current_time(); + do { + + rtw_mdelay_os(1); + + rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now); + if ((cpwm_orig ^ cpwm_now) & 0x80) + break; + + if (rtw_get_passing_time_ms(start_time) > 100) { + RTW_INFO("%s: polling cpwm timeout when leaving IPS in FWLPS state\n", __FUNCTION__); + break; + } + } while (1); + +#endif + parm[0] = 0x0; + parm[1] = 0x0; + parm[2] = 0x0; + rtw_hal_fill_h2c_cmd(padapter, H2C_INACTIVE_PS_, + H2C_INACTIVE_PS_LEN, parm); +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_IpsNotify(padapter, IPS_NONE); +#endif + } +} +#endif /* CONFIG_PNO_SUPPORT */ + +void rtw_exec_lps(_adapter *padapter, u8 ps_mode) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + + if (ps_mode == PS_MODE_ACTIVE) { +#ifdef CONFIG_LPS_ACK + _enter_critical_mutex(&pwrpriv->lps_ack_mutex, NULL); + rtw_sctx_init(&pwrpriv->lps_ack_sctx, 100); +#endif /* CONFIG_LPS_ACK */ + + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_STATE_CHK, (u8 *)(&ps_mode)); + +#ifdef CONFIG_LPS_ACK + _exit_critical_mutex(&pwrpriv->lps_ack_mutex, NULL); +#endif /* CONFIG_LPS_ACK */ + } else { + if (MLME_IS_ASOC(padapter)) + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode)); + else + RTW_INFO(FUNC_ADPT_FMT": It can't execute LPS without Wi-Fi connection!\n", + FUNC_ADPT_ARG(padapter)); + } +} + +void rtw_lps_rfon_ctrl(_adapter *padapter, u8 rfon_ctrl) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + u8 rpwm = 0; + + if (pwrpriv->bFwCurrentInPSMode && pwrpriv->pwr_mode != PS_MODE_ACTIVE) { + if (rfon_ctrl == rf_on) { +#ifdef CONFIG_LPS_LCLK + if (pwrpriv->lps_level >= LPS_LCLK) { + s32 ready = _FAIL; + systime stime; + s32 utime; + u32 timeout; /* unit: ms */ + +#ifdef LPS_RPWM_WAIT_MS + timeout = LPS_RPWM_WAIT_MS; +#else + timeout = 30; +#endif /* !LPS_RPWM_WAIT_MS */ + + stime = rtw_get_current_time(); + do { + ready = rtw_register_task_alive(padapter, LPS_ALIVE); + if (ready == _SUCCESS) + break; + + utime = rtw_get_passing_time_ms(stime); + if (utime > timeout) + break; + + rtw_msleep_os(1); + } while (1); + + if (ready == _FAIL) + RTW_INFO(FUNC_ADPT_FMT": It is not ready to leave 32K !!!\n", + FUNC_ADPT_ARG(padapter)); + } +#endif /* CONFIG_LPS_LCLK */ + +#ifdef CONFIG_LPS_ACK + _enter_critical_mutex(&pwrpriv->lps_ack_mutex, NULL); + rtw_sctx_init(&pwrpriv->lps_ack_sctx, 100); +#endif /* CONFIG_LPS_ACK */ + + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE_RFON_CTRL, (u8 *)(&rfon_ctrl)); + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_RFON_CHK, (u8 *)(&rfon_ctrl)); + +#ifdef CONFIG_LPS_ACK + _exit_critical_mutex(&pwrpriv->lps_ack_mutex, NULL); +#endif /* CONFIG_LPS_ACK */ + } else { + if (MLME_IS_ASOC(padapter)) { +#ifdef CONFIG_LPS_PG + if (pwrpriv->lps_level == LPS_PG) { + if (rtw_hal_set_lps_pg_info_cmd(padapter) == _FAIL) + RTW_INFO(FUNC_ADPT_FMT": Send PG H2C command Fail! \n", + FUNC_ADPT_ARG(padapter)); + } +#endif /* CONFIG_LPS_PG */ + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE_RFON_CTRL, (u8 *)(&rfon_ctrl)); + } else { + RTW_INFO(FUNC_ADPT_FMT": It can't execute RFON without Wi-Fi connection!\n", + FUNC_ADPT_ARG(padapter)); + } + +#ifdef CONFIG_LPS_LCLK + if (pwrpriv->lps_level >= LPS_LCLK) { + rtw_unregister_task_alive(padapter, LPS_ALIVE); + + if (pwrpriv->alives == 0) { + u8 polling_cnt = 0; + u8 reg_val8 = 0; + u8 result = _FAIL; + + do { + rtw_msleep_os(1); + reg_val8 = rtw_read8(padapter, REG_CR); + if (reg_val8 == 0xEA) { + result= _SUCCESS; + break; + } + polling_cnt++; + } while (polling_cnt < 100); + + if (result == _FAIL ) + RTW_INFO(FUNC_ADPT_FMT": It is not finished to enter 32K !!!\n", + FUNC_ADPT_ARG(padapter)); + } + } +#endif /* CONFIG_LPS_LCLK */ + } + } else { + RTW_INFO(FUNC_ADPT_FMT": RFON can't work due to ps state is not in LPS !\n", + FUNC_ADPT_ARG(padapter)); + } +} + +void rtw_set_ps_mode(PADAPTER padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) || defined(CONFIG_P2P_WOWLAN) + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; +#endif +#ifdef CONFIG_WMMPS_STA + struct registry_priv *pregistrypriv = &padapter->registrypriv; +#endif +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ +#ifdef CONFIG_TDLS + struct sta_priv *pstapriv = &padapter->stapriv; + _irqL irqL; + int i, j; + _list *plist, *phead; + struct sta_info *ptdls_sta; +#endif /* CONFIG_TDLS */ +#ifdef CONFIG_LPS_PG + u8 lps_pg_hdl_id = 0; +#endif + + + + if (ps_mode > PM_Card_Disable) { + return; + } + + if (pwrpriv->pwr_mode == ps_mode) { + if (PS_MODE_ACTIVE == ps_mode) + return; + +#ifndef CONFIG_BT_COEXIST +#ifdef CONFIG_WMMPS_STA + if (!rtw_is_wmmps_mode(padapter)) +#endif /* CONFIG_WMMPS_STA */ + if ((pwrpriv->smart_ps == smart_ps) && + (pwrpriv->bcn_ant_mode == bcn_ant_mode)) + return; +#endif /* !CONFIG_BT_COEXIST */ + } + +#ifdef CONFIG_FW_MULTI_PORT_SUPPORT + if (PS_MODE_ACTIVE != ps_mode) { + rtw_set_ps_rsvd_page(padapter); + rtw_set_default_port_id(padapter); + } +#endif + +#ifdef CONFIG_LPS_PG + if ((PS_MODE_ACTIVE != ps_mode) && (pwrpriv->lps_level == LPS_PG)) { + if (pwrpriv->wowlan_mode != _TRUE) { + /*rtw_hal_set_lps_pg_info(padapter);*/ + lps_pg_hdl_id = LPS_PG_INFO_CFG; + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_PG_HANDLE, (u8 *)(&lps_pg_hdl_id)); + } + } +#endif + +#ifdef CONFIG_LPS_LCLK + _enter_pwrlock(&pwrpriv->lock); +#endif + + /* if(pwrpriv->pwr_mode == PS_MODE_ACTIVE) */ + if (ps_mode == PS_MODE_ACTIVE) { + if (1 +#ifdef CONFIG_BT_COEXIST + && (((rtw_btcoex_IsBtControlLps(padapter) == _FALSE) +#ifdef CONFIG_P2P_PS + && (pwdinfo->opp_ps == 0) +#endif /* CONFIG_P2P_PS */ + ) + || ((rtw_btcoex_IsBtControlLps(padapter) == _TRUE) + && (rtw_btcoex_IsLpsOn(padapter) == _FALSE)) + ) +#else /* !CONFIG_BT_COEXIST */ +#ifdef CONFIG_P2P_PS + && (pwdinfo->opp_ps == 0) +#endif /* CONFIG_P2P_PS */ +#endif /* !CONFIG_BT_COEXIST */ + ) { + RTW_INFO(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n", + FUNC_ADPT_ARG(padapter), msg); + + if (pwrpriv->lps_leave_cnts < UINT_MAX) + pwrpriv->lps_leave_cnts++; + else + pwrpriv->lps_leave_cnts = 0; +#ifdef CONFIG_TDLS + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + ptdls_sta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) + issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->cmn.mac_addr, 0, 0, 0); + plist = get_next(plist); + } + } +#endif /* CONFIG_TDLS */ + + pwrpriv->pwr_mode = ps_mode; + rtw_set_rpwm(padapter, PS_STATE_S4); + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) || defined(CONFIG_P2P_WOWLAN) + if (pwrpriv->wowlan_mode == _TRUE || + pwrpriv->wowlan_ap_mode == _TRUE || + pwrpriv->wowlan_p2p_mode == _TRUE) { + systime start_time; + u32 delay_ms; + u8 val8; + delay_ms = 20; + start_time = rtw_get_current_time(); + do { + rtw_hal_get_hwreg(padapter, HW_VAR_SYS_CLKR, &val8); + if (!(val8 & BIT(4))) { /* 0x08 bit4 =1 --> in 32k, bit4 = 0 --> leave 32k */ + pwrpriv->cpwm = PS_STATE_S4; + break; + } + if (rtw_get_passing_time_ms(start_time) > delay_ms) { + RTW_INFO("%s: Wait for FW 32K leave more than %u ms!!!\n", + __FUNCTION__, delay_ms); + pdbgpriv->dbg_wow_leave_ps_fail_cnt++; + break; + } + rtw_usleep_os(100); + } while (1); + } +#endif +#ifdef CONFIG_LPS_PG + if (pwrpriv->lps_level == LPS_PG) { + lps_pg_hdl_id = LPS_PG_REDLEMEM; + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_PG_HANDLE, (u8 *)(&lps_pg_hdl_id)); + } +#endif +#ifdef CONFIG_WOWLAN + if (pwrpriv->wowlan_mode == _TRUE) + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_INACTIVE_IPS, (u8 *)(&ps_mode)); +#endif /* CONFIG_WOWLAN */ + + rtw_exec_lps(padapter, ps_mode); + +#ifdef CONFIG_LPS_PG + if (pwrpriv->lps_level == LPS_PG) { + lps_pg_hdl_id = LPS_PG_PHYDM_EN; + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_PG_HANDLE, (u8 *)(&lps_pg_hdl_id)); + } +#endif + +#ifdef CONFIG_LPS_POFF + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_POFF_SET_MODE, + (u8 *)(&ps_mode)); +#endif /*CONFIG_LPS_POFF*/ + + pwrpriv->bFwCurrentInPSMode = _FALSE; + +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_LpsNotify(padapter, ps_mode); +#endif /* CONFIG_BT_COEXIST */ + } + } else { + if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE)) +#ifdef CONFIG_BT_COEXIST + || ((rtw_btcoex_IsBtControlLps(padapter) == _TRUE) + && (rtw_btcoex_IsLpsOn(padapter) == _TRUE)) +#endif +#ifdef CONFIG_P2P_WOWLAN + || (_TRUE == pwrpriv->wowlan_p2p_mode) +#endif /* CONFIG_P2P_WOWLAN */ +#ifdef CONFIG_WOWLAN + || WOWLAN_IS_STA_MIX_MODE(padapter) +#endif /* CONFIG_WOWLAN */ + ) { + u8 pslv; + + RTW_INFO(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n", + FUNC_ADPT_ARG(padapter), msg); + + if (pwrpriv->lps_enter_cnts < UINT_MAX) + pwrpriv->lps_enter_cnts++; + else + pwrpriv->lps_enter_cnts = 0; +#ifdef CONFIG_TDLS + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + ptdls_sta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) + issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->cmn.mac_addr, 1, 0, 0); + plist = get_next(plist); + } + } +#endif /* CONFIG_TDLS */ + +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_LpsNotify(padapter, ps_mode); +#endif /* CONFIG_BT_COEXIST */ + +#ifdef CONFIG_LPS_POFF + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_POFF_SET_MODE, + (u8 *)(&ps_mode)); +#endif /*CONFIG_LPS_POFF*/ + + pwrpriv->bFwCurrentInPSMode = _TRUE; + pwrpriv->pwr_mode = ps_mode; + pwrpriv->smart_ps = smart_ps; + pwrpriv->bcn_ant_mode = bcn_ant_mode; +#ifdef CONFIG_LPS_PG + if (pwrpriv->lps_level == LPS_PG) { + lps_pg_hdl_id = LPS_PG_PHYDM_DIS; + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_PG_HANDLE, (u8 *)(&lps_pg_hdl_id)); + } +#endif + +#ifdef CONFIG_WMMPS_STA + pwrpriv->wmm_smart_ps = pregistrypriv->wmm_smart_ps; +#endif /* CONFIG_WMMPS_STA */ + + rtw_exec_lps(padapter, ps_mode); + +#ifdef CONFIG_WOWLAN + if (pwrpriv->wowlan_mode == _TRUE) + rtw_hal_set_hwreg(padapter, HW_VAR_H2C_INACTIVE_IPS, (u8 *)(&ps_mode)); +#endif /* CONFIG_WOWLAN */ + +#ifdef CONFIG_P2P_PS + /* Set CTWindow after LPS */ + if (pwdinfo->opp_ps == 1) + p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 0); +#endif /* CONFIG_P2P_PS */ + + pslv = PS_STATE_S2; +#ifdef CONFIG_LPS_LCLK + if (pwrpriv->alives == 0) + pslv = PS_STATE_S0; +#endif /* CONFIG_LPS_LCLK */ + +#ifdef CONFIG_BT_COEXIST + if ((rtw_btcoex_IsBtDisabled(padapter) == _FALSE) + && (rtw_btcoex_IsBtControlLps(padapter) == _TRUE)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + + } +#endif /* CONFIG_BT_COEXIST */ + + rtw_set_rpwm(padapter, pslv); + } + } + +#ifdef CONFIG_LPS_LCLK + _exit_pwrlock(&pwrpriv->lock); +#endif + +} + +const char * const LPS_CTRL_PHYDM = "LPS_CTRL_PHYDM"; + +/* + * Description: + * Enter the leisure power save mode. + * */ +void LPS_Enter(PADAPTER padapter, const char *msg) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + int i; + char buf[32] = {0}; +#ifdef DBG_LA_MODE + struct registry_priv *registry_par = &(padapter->registrypriv); +#endif + + /* RTW_INFO("+LeisurePSEnter\n"); */ + if (GET_HAL_DATA(padapter)->bFWReady == _FALSE) + return; + +#ifdef CONFIG_BT_COEXIST + if (rtw_btcoex_IsBtControlLps(padapter) == _TRUE) + return; +#endif + +#ifdef DBG_LA_MODE + if(registry_par->la_mode_en == 1) { + RTW_INFO("%s LA debug mode lps_leave \n", __func__); + return; + } +#endif + /* Skip lps enter request if number of assocated adapters is not 1 */ + if (rtw_mi_get_assoc_if_num(padapter) != 1) + return; + +#ifndef CONFIG_FW_MULTI_PORT_SUPPORT + /* Skip lps enter request for adapter not port0 */ + if (get_hw_port(padapter) != HW_PORT0) + return; +#endif + + for (i = 0; i < dvobj->iface_nums; i++) { + if (PS_RDY_CHECK(dvobj->padapters[i]) == _FALSE) + return; + } + +#ifdef CONFIG_CLIENT_PORT_CFG + if ((rtw_hal_get_port(padapter) == CLT_PORT_INVALID) || + get_clt_num(padapter) > MAX_CLIENT_PORT_NUM){ + RTW_ERR(ADPT_FMT" cannot get client port or clt num(%d) over than 4\n", ADPT_ARG(padapter), get_clt_num(padapter)); + return; + } +#endif + +#ifdef CONFIG_P2P_PS + if (padapter->wdinfo.p2p_ps_mode == P2P_PS_NOA) { + return;/* supporting p2p client ps NOA via H2C_8723B_P2P_PS_OFFLOAD */ + } +#endif /* CONFIG_P2P_PS */ + + if (pwrpriv->bLeisurePs) { + /* Idle for a while if we connect to AP a while ago. */ + if (pwrpriv->LpsIdleCount >= 2) { /* 4 Sec */ + if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) { + +#ifdef CONFIG_WMMPS_STA + if (rtw_is_wmmps_mode(padapter)) + msg = "WMMPS_IDLE"; +#endif /* CONFIG_WMMPS_STA */ + + sprintf(buf, "WIFI-%s", msg); + pwrpriv->bpower_saving = _TRUE; + +#ifdef CONFIG_RTW_CFGVENDOR_LLSTATS + pwrpriv->pwr_saving_start_time = rtw_get_current_time(); +#endif /* CONFIG_RTW_CFGVENDOR_LLSTATS */ + + rtw_set_ps_mode(padapter, pwrpriv->power_mgnt, padapter->registrypriv.smart_ps, 0, buf); + +#ifdef CONFIG_PCI_DYNAMIC_ASPM + if (msg != LPS_CTRL_PHYDM) + rtw_pci_dynamic_aspm_set_mode(padapter, ASPM_MODE_PS); +#endif + } + } else + pwrpriv->LpsIdleCount++; + } + + /* RTW_INFO("-LeisurePSEnter\n"); */ + +} + +/* + * Description: + * Leave the leisure power save mode. + * */ +void LPS_Leave(PADAPTER padapter, const char *msg) +{ +#define LPS_LEAVE_TIMEOUT_MS 100 + + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + char buf[32] = {0}; +#ifdef DBG_CHECK_FW_PS_STATE + struct debug_priv *pdbgpriv = &dvobj->drv_dbg; +#endif + + + /* RTW_INFO("+LeisurePSLeave\n"); */ + +#ifdef CONFIG_BT_COEXIST + if (rtw_btcoex_IsBtControlLps(padapter) == _TRUE) + return; +#endif + + if (pwrpriv->bLeisurePs) { + if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) { +#ifdef CONFIG_PCI_DYNAMIC_ASPM + if (msg != LPS_CTRL_PHYDM) + rtw_pci_dynamic_aspm_set_mode(padapter, ASPM_MODE_PERF); +#endif +#ifdef CONFIG_WMMPS_STA + if (rtw_is_wmmps_mode(padapter)) + msg = "WMMPS_BUSY"; +#endif /* CONFIG_WMMPS_STA */ + + sprintf(buf, "WIFI-%s", msg); + rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, buf); + +#ifdef CONFIG_RTW_CFGVENDOR_LLSTATS + pwrpriv->pwr_saving_time += rtw_get_passing_time_ms(pwrpriv->pwr_saving_start_time); +#endif /* CONFIG_RTW_CFGVENDOR_LLSTATS */ + } + } + + pwrpriv->bpower_saving = _FALSE; +#ifdef DBG_CHECK_FW_PS_STATE + if (rtw_fw_ps_state(padapter) == _FAIL) { + RTW_INFO("leave lps, fw in 32k\n"); + pdbgpriv->dbg_leave_lps_fail_cnt++; + } +#endif /* DBG_CHECK_FW_PS_STATE + * RTW_INFO("-LeisurePSLeave\n"); */ + +} + +#ifdef CONFIG_WOWLAN +void rtw_wow_lps_level_decide(_adapter *adapter, u8 wow_en) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + + if (wow_en) { + pwrpriv->lps_level_bk = pwrpriv->lps_level; + pwrpriv->lps_level = pwrpriv->wowlan_lps_level; + #ifdef CONFIG_LPS_1T1R + pwrpriv->lps_1t1r_bk = pwrpriv->lps_1t1r; + pwrpriv->lps_1t1r = pwrpriv->wowlan_lps_1t1r; + #endif + } else { + pwrpriv->lps_level = pwrpriv->lps_level_bk; + #ifdef CONFIG_LPS_1T1R + pwrpriv->lps_1t1r = pwrpriv->lps_1t1r_bk; + #endif + } +} +#endif /* CONFIG_WOWLAN */ +#endif /* CONFIG_LPS */ + +void LeaveAllPowerSaveModeDirect(PADAPTER Adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter); + PADAPTER pri_padapter = GET_PRIMARY_ADAPTER(Adapter); + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter); +#ifdef CONFIG_LPS_LCLK +#ifndef CONFIG_DETECT_CPWM_BY_POLLING + u8 cpwm_orig; +#endif /* CONFIG_DETECT_CPWM_BY_POLLING */ + u8 rpwm; +#endif + int i; + + RTW_INFO("%s.....\n", __FUNCTION__); + + if (rtw_is_surprise_removed(Adapter)) { + RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=_TRUE Skip!\n", FUNC_ADPT_ARG(Adapter)); + return; + } + + if (rtw_mi_check_status(Adapter, MI_LINKED)) { /*connect*/ + + if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) { + RTW_INFO("%s: Driver Already Leave LPS\n", __FUNCTION__); + return; + } + +#ifdef CONFIG_LPS_LCLK + _enter_pwrlock(&pwrpriv->lock); + +#ifndef CONFIG_DETECT_CPWM_BY_POLLING + cpwm_orig = 0; + rtw_hal_get_hwreg(Adapter, HW_VAR_CPWM, &cpwm_orig); +#endif /* CONFIG_DETECT_CPWM_BY_POLLING */ + rpwm = rtw_set_rpwm(Adapter, PS_STATE_S4); + +#ifndef CONFIG_DETECT_CPWM_BY_POLLING + if (rpwm != 0xFF && rpwm & PS_ACK) + rtw_cpwm_polling(Adapter, rpwm, cpwm_orig); +#endif /* CONFIG_DETECT_CPWM_BY_POLLING */ + + _exit_pwrlock(&pwrpriv->lock); +#endif/*CONFIG_LPS_LCLK*/ + +#ifdef CONFIG_P2P_PS + for (i = 0; i < dvobj->iface_nums; i++) { + _adapter *iface = dvobj->padapters[i]; + struct wifidirect_info *pwdinfo = &(iface->wdinfo); + + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) + p2p_ps_wk_cmd(iface, P2P_PS_DISABLE, 0); + } +#endif /* CONFIG_P2P_PS */ + +#ifdef CONFIG_LPS + rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, RTW_CMDF_DIRECTLY); +#endif + } else { + if (pwrpriv->rf_pwrstate == rf_off) { + +#if defined(CONFIG_FWLPS_IN_IPS) || defined(CONFIG_SWLPS_IN_IPS) || defined(CONFIG_RTL8188E) +#ifdef CONFIG_IPS + if (_FALSE == ips_leave(pri_padapter)) + RTW_INFO("======> ips_leave fail.............\n"); +#endif +#endif /* CONFIG_SWLPS_IN_IPS || (CONFIG_PLATFORM_SPRD && CONFIG_RTL8188E) */ + + } + } + +} + +/* + * Description: Leave all power save mode: LPS, FwLPS, IPS if needed. + * Move code to function by tynli. 2010.03.26. + * */ +void LeaveAllPowerSaveMode(PADAPTER Adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter); + u8 enqueue = 0; + int i; + + #ifndef CONFIG_NEW_NETDEV_HDL + if (_FALSE == Adapter->bup) { + RTW_INFO(FUNC_ADPT_FMT ": bup=%d Skip!\n", + FUNC_ADPT_ARG(Adapter), Adapter->bup); + return; + } + #endif + +/* RTW_INFO(FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(Adapter));*/ + + if (rtw_is_surprise_removed(Adapter)) { + RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=_TRUE Skip!\n", FUNC_ADPT_ARG(Adapter)); + return; + } + + if (rtw_mi_get_assoc_if_num(Adapter)) { + /* connect */ +#ifdef CONFIG_LPS_LCLK + enqueue = 1; +#endif + +#ifdef CONFIG_P2P_PS + for (i = 0; i < dvobj->iface_nums; i++) { + _adapter *iface = dvobj->padapters[i]; + struct wifidirect_info *pwdinfo = &(iface->wdinfo); + + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) + p2p_ps_wk_cmd(iface, P2P_PS_DISABLE, enqueue); + } +#endif /* CONFIG_P2P_PS */ + +#ifdef CONFIG_LPS + rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue ? 0 : RTW_CMDF_DIRECTLY); +#endif + +#ifdef CONFIG_LPS_LCLK + LPS_Leave_check(Adapter); +#endif + } else { + if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) { + +#if defined(CONFIG_FWLPS_IN_IPS) || defined(CONFIG_SWLPS_IN_IPS) || (defined(CONFIG_PLATFORM_SPRD) && defined(CONFIG_RTL8188E)) +#ifdef CONFIG_IPS + if (_FALSE == ips_leave(Adapter)) + RTW_INFO("======> ips_leave fail.............\n"); +#endif +#endif /* CONFIG_SWLPS_IN_IPS || (CONFIG_PLATFORM_SPRD && CONFIG_RTL8188E) */ + + } + } + +} + +#ifdef CONFIG_LPS_LCLK +void LPS_Leave_check( + PADAPTER padapter) +{ + struct pwrctrl_priv *pwrpriv; + systime start_time; + u8 bReady; + + + pwrpriv = adapter_to_pwrctl(padapter); + + bReady = _FALSE; + start_time = rtw_get_current_time(); + + rtw_yield_os(); + + while (1) { + _enter_pwrlock(&pwrpriv->lock); + + if (rtw_is_surprise_removed(padapter) + || (!rtw_is_hw_init_completed(padapter)) +#ifdef CONFIG_USB_HCI + || rtw_is_drv_stopped(padapter) +#endif + || (pwrpriv->pwr_mode == PS_MODE_ACTIVE) + ) + bReady = _TRUE; + + _exit_pwrlock(&pwrpriv->lock); + + if (_TRUE == bReady) + break; + + if (rtw_get_passing_time_ms(start_time) > 100) { + RTW_ERR("Wait for cpwm event than 100 ms!!!\n"); + break; + } + rtw_msleep_os(1); + } + +} + +/* + * Caller:ISR handler... + * + * This will be called when CPWM interrupt is up. + * + * using to update cpwn of drv; and drv willl make a decision to up or down pwr level + */ +void cpwm_int_hdl( + PADAPTER padapter, + struct reportpwrstate_parm *preportpwrstate) +{ + struct pwrctrl_priv *pwrpriv; + + if (!padapter) + goto exit; + + if (RTW_CANNOT_RUN(padapter)) + goto exit; + + pwrpriv = adapter_to_pwrctl(padapter); +#if 0 + if (pwrpriv->cpwm_tog == (preportpwrstate->state & PS_TOGGLE)) { + goto exit; + } +#endif + + _enter_pwrlock(&pwrpriv->lock); + +#ifdef CONFIG_LPS_RPWM_TIMER + if (pwrpriv->rpwm < PS_STATE_S2) { + RTW_INFO("%s: Redundant CPWM Int. RPWM=0x%02X CPWM=0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + _exit_pwrlock(&pwrpriv->lock); + goto exit; + } +#endif /* CONFIG_LPS_RPWM_TIMER */ + + pwrpriv->cpwm = PS_STATE(preportpwrstate->state); + pwrpriv->cpwm_tog = preportpwrstate->state & PS_TOGGLE; + + if (pwrpriv->cpwm >= PS_STATE_S2) { + if (pwrpriv->alives & CMD_ALIVE) + _rtw_up_sema(&padapter->cmdpriv.cmd_queue_sema); + + if (pwrpriv->alives & XMIT_ALIVE) + _rtw_up_sema(&padapter->xmitpriv.xmit_sema); + } + + _exit_pwrlock(&pwrpriv->lock); + +exit: + return; +} + +static void cpwm_event_callback(struct work_struct *work) +{ + struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, cpwm_event); + struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + struct reportpwrstate_parm report; + + /* RTW_INFO("%s\n",__FUNCTION__); */ + + report.state = PS_STATE_S2; + cpwm_int_hdl(adapter, &report); +} + +static void dma_event_callback(struct work_struct *work) +{ + struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, dma_event); + struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + + rtw_unregister_tx_alive(adapter); +} + +#ifdef CONFIG_LPS_RPWM_TIMER + +#define DBG_CPWM_CHK_FAIL +#if defined(DBG_CPWM_CHK_FAIL) && (defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) \ + || defined(CONFIG_RTL8723F)) +#define CPU_EXCEPTION_CODE 0xFAFAFAFA +static void rtw_cpwm_chk_fail_debug(_adapter *padapter) +{ + u32 cpu_state; + + cpu_state = rtw_read32(padapter, 0x10FC); + + RTW_INFO("[PS-DBG] Reg_10FC =0x%08x\n", cpu_state); + RTW_INFO("[PS-DBG] Reg_10F8 =0x%08x\n", rtw_read32(padapter, 0x10F8)); + RTW_INFO("[PS-DBG] Reg_11F8 =0x%08x\n", rtw_read32(padapter, 0x11F8)); + RTW_INFO("[PS-DBG] Reg_4A4 =0x%08x\n", rtw_read32(padapter, 0x4A4)); + RTW_INFO("[PS-DBG] Reg_4A8 =0x%08x\n", rtw_read32(padapter, 0x4A8)); + + if (cpu_state == CPU_EXCEPTION_CODE) { + RTW_INFO("[PS-DBG] Reg_48C =0x%08x\n", rtw_read32(padapter, 0x48C)); + RTW_INFO("[PS-DBG] Reg_490 =0x%08x\n", rtw_read32(padapter, 0x490)); + RTW_INFO("[PS-DBG] Reg_494 =0x%08x\n", rtw_read32(padapter, 0x494)); + RTW_INFO("[PS-DBG] Reg_498 =0x%08x\n", rtw_read32(padapter, 0x498)); + RTW_INFO("[PS-DBG] Reg_49C =0x%08x\n", rtw_read32(padapter, 0x49C)); + RTW_INFO("[PS-DBG] Reg_4A0 =0x%08x\n", rtw_read32(padapter, 0x4A0)); + RTW_INFO("[PS-DBG] Reg_1BC =0x%08x\n", rtw_read32(padapter, 0x1BC)); + + RTW_INFO("[PS-DBG] Reg_008 =0x%08x\n", rtw_read32(padapter, 0x08)); + RTW_INFO("[PS-DBG] Reg_2F0 =0x%08x\n", rtw_read32(padapter, 0x2F0)); + RTW_INFO("[PS-DBG] Reg_2F4 =0x%08x\n", rtw_read32(padapter, 0x2F4)); + RTW_INFO("[PS-DBG] Reg_2F8 =0x%08x\n", rtw_read32(padapter, 0x2F8)); + RTW_INFO("[PS-DBG] Reg_2FC =0x%08x\n", rtw_read32(padapter, 0x2FC)); + + rtw_dump_fifo(RTW_DBGDUMP, padapter, 5, 0, 3072); + } +} +#endif +static void rpwmtimeout_workitem_callback(struct work_struct *work) +{ + PADAPTER padapter; + struct dvobj_priv *dvobj; + struct pwrctrl_priv *pwrpriv; + + + pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi); + dvobj = pwrctl_to_dvobj(pwrpriv); + padapter = dvobj_get_primary_adapter(dvobj); + + if (!padapter) + return; + + if (RTW_CANNOT_RUN(padapter)) + return; + + _enter_pwrlock(&pwrpriv->lock); + if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) { + RTW_INFO("%s: rpwm=0x%02X cpwm=0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + goto exit; + } + + if (pwrpriv->rpwm_retry++ < LPS_RPWM_RETRY_CNT) { + u8 rpwm = (pwrpriv->rpwm | pwrpriv->tog | PS_ACK); + + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm)); + + pwrpriv->tog += 0x80; + _set_timer(&pwrpriv->pwr_rpwm_timer, LPS_CPWM_TIMEOUT_MS); + goto exit; + } + + pwrpriv->rpwm_retry = 0; + _exit_pwrlock(&pwrpriv->lock); + +#if defined(DBG_CPWM_CHK_FAIL) && (defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) \ + || defined(CONFIG_RTL8723F)) + RTW_INFO("+%s: rpwm=0x%02X cpwm=0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + rtw_cpwm_chk_fail_debug(padapter); +#endif + + if (rtw_read8(padapter, 0x100) != 0xEA) { +#if 1 + struct reportpwrstate_parm report; + + report.state = PS_STATE_S2; + RTW_INFO("\n%s: FW already leave 32K!\n\n", __func__); + cpwm_int_hdl(padapter, &report); +#else + RTW_INFO("\n%s: FW already leave 32K!\n\n", __func__); + cpwm_event_callback(&pwrpriv->cpwm_event); +#endif + return; + } + + _enter_pwrlock(&pwrpriv->lock); + + if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) { + RTW_INFO("%s: cpwm=%d, nothing to do!\n", __func__, pwrpriv->cpwm); + goto exit; + } + pwrpriv->brpwmtimeout = _TRUE; + rtw_set_rpwm(padapter, pwrpriv->rpwm); + pwrpriv->brpwmtimeout = _FALSE; + +exit: + _exit_pwrlock(&pwrpriv->lock); +} + +/* + * This function is a timer handler, can't do any IO in it. + */ +static void pwr_rpwm_timeout_handler(void *FunctionContext) +{ + PADAPTER padapter; + struct pwrctrl_priv *pwrpriv; + + + padapter = (PADAPTER)FunctionContext; + pwrpriv = adapter_to_pwrctl(padapter); + if (!padapter) + return; + + if (RTW_CANNOT_RUN(padapter)) + return; + + RTW_INFO("+%s: rpwm=0x%02X cpwm=0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); + + if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) { + RTW_INFO("+%s: cpwm=%d, nothing to do!\n", __func__, pwrpriv->cpwm); + return; + } + + _set_workitem(&pwrpriv->rpwmtimeoutwi); +} +#endif /* CONFIG_LPS_RPWM_TIMER */ + +__inline static void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag) +{ + pwrctrl->alives |= tag; +} + +__inline static void unregister_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag) +{ + pwrctrl->alives &= ~tag; +} + + +/* + * Description: + * Check if the fw_pwrstate is okay for I/O. + * If not (cpwm is less than S2), then the sub-routine + * will raise the cpwm to be greater than or equal to S2. + * + * Calling Context: Passive + * + * Constraint: + * 1. this function will request pwrctrl->lock + * + * Return Value: + * _SUCCESS hardware is ready for I/O + * _FAIL can't I/O right now + */ +s32 rtw_register_task_alive(PADAPTER padapter, u32 task) +{ + s32 res; + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + + res = _SUCCESS; + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S2; + + _enter_pwrlock(&pwrctrl->lock); + + register_task_alive(pwrctrl, task); + + if (pwrctrl->bFwCurrentInPSMode == _TRUE) { + + if (pwrctrl->cpwm < pslv) { + if (pwrctrl->cpwm < PS_STATE_S2) + res = _FAIL; + if (pwrctrl->rpwm < pslv) + rtw_set_rpwm(padapter, pslv); + } + } + + _exit_pwrlock(&pwrctrl->lock); + +#ifdef CONFIG_DETECT_CPWM_BY_POLLING + if (_FAIL == res) { + if (pwrctrl->cpwm >= PS_STATE_S2) + res = _SUCCESS; + } +#endif /* CONFIG_DETECT_CPWM_BY_POLLING */ + + + return res; +} + +/* + * Description: + * If task is done, call this func. to power down firmware again. + * + * Constraint: + * 1. this function will request pwrctrl->lock + * + * Return Value: + * none + */ +void rtw_unregister_task_alive(PADAPTER padapter, u32 task) +{ + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S0; + +#ifdef CONFIG_BT_COEXIST + if ((rtw_btcoex_IsBtDisabled(padapter) == _FALSE) + && (rtw_btcoex_IsBtControlLps(padapter) == _TRUE)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + + } +#endif /* CONFIG_BT_COEXIST */ + + _enter_pwrlock(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, task); + + if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) + && (pwrctrl->bFwCurrentInPSMode == _TRUE)) { + + if (pwrctrl->cpwm > pslv) { + if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0)) + rtw_set_rpwm(padapter, pslv); + } + } + + _exit_pwrlock(&pwrctrl->lock); + +} + +/* + * Caller: rtw_xmit_thread + * + * Check if the fw_pwrstate is okay for xmit. + * If not (cpwm is less than S3), then the sub-routine + * will raise the cpwm to be greater than or equal to S3. + * + * Calling Context: Passive + * + * Return Value: + * _SUCCESS rtw_xmit_thread can write fifo/txcmd afterwards. + * _FAIL rtw_xmit_thread can not do anything. + */ +s32 rtw_register_tx_alive(PADAPTER padapter) +{ + s32 res; + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + + res = _SUCCESS; + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S2; + + _enter_pwrlock(&pwrctrl->lock); + + register_task_alive(pwrctrl, XMIT_ALIVE); + + if (pwrctrl->bFwCurrentInPSMode == _TRUE) { + + if (pwrctrl->cpwm < pslv) { + if (pwrctrl->cpwm < PS_STATE_S2) + res = _FAIL; + if (pwrctrl->rpwm < pslv) + rtw_set_rpwm(padapter, pslv); + } + } + + _exit_pwrlock(&pwrctrl->lock); + +#ifdef CONFIG_DETECT_CPWM_BY_POLLING + if (_FAIL == res) { + if (pwrctrl->cpwm >= PS_STATE_S2) + res = _SUCCESS; + } +#endif /* CONFIG_DETECT_CPWM_BY_POLLING */ + + + return res; +} + +/* + * Caller: rtw_cmd_thread + * + * Check if the fw_pwrstate is okay for issuing cmd. + * If not (cpwm should be is less than S2), then the sub-routine + * will raise the cpwm to be greater than or equal to S2. + * + * Calling Context: Passive + * + * Return Value: + * _SUCCESS rtw_cmd_thread can issue cmds to firmware afterwards. + * _FAIL rtw_cmd_thread can not do anything. + */ +s32 rtw_register_cmd_alive(PADAPTER padapter) +{ + s32 res; + struct pwrctrl_priv *pwrctrl; + u8 pslv; + + + res = _SUCCESS; + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S2; + + _enter_pwrlock(&pwrctrl->lock); + + register_task_alive(pwrctrl, CMD_ALIVE); + + if (pwrctrl->bFwCurrentInPSMode == _TRUE) { + + if (pwrctrl->cpwm < pslv) { + if (pwrctrl->cpwm < PS_STATE_S2) + res = _FAIL; + if (pwrctrl->rpwm < pslv) + rtw_set_rpwm(padapter, pslv); + } + } + + _exit_pwrlock(&pwrctrl->lock); + +#ifdef CONFIG_DETECT_CPWM_BY_POLLING + if (_FAIL == res) { + if (pwrctrl->cpwm >= PS_STATE_S2) + res = _SUCCESS; + } +#endif /* CONFIG_DETECT_CPWM_BY_POLLING */ + + + return res; +} + +/* + * Caller: rx_isr + * + * Calling Context: Dispatch/ISR + * + * Return Value: + * _SUCCESS + * _FAIL + */ +s32 rtw_register_rx_alive(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrctrl; + + + pwrctrl = adapter_to_pwrctl(padapter); + + _enter_pwrlock(&pwrctrl->lock); + + register_task_alive(pwrctrl, RECV_ALIVE); + + _exit_pwrlock(&pwrctrl->lock); + + + return _SUCCESS; +} + +/* + * Caller: evt_isr or evt_thread + * + * Calling Context: Dispatch/ISR or Passive + * + * Return Value: + * _SUCCESS + * _FAIL + */ +s32 rtw_register_evt_alive(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrctrl; + + + pwrctrl = adapter_to_pwrctl(padapter); + + _enter_pwrlock(&pwrctrl->lock); + + register_task_alive(pwrctrl, EVT_ALIVE); + + _exit_pwrlock(&pwrctrl->lock); + + + return _SUCCESS; +} + +/* + * Caller: ISR + * + * If ISR's txdone, + * No more pkts for TX, + * Then driver shall call this fun. to power down firmware again. + */ +void rtw_unregister_tx_alive(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrctrl; + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + u8 pslv, i; + + + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S0; + +#ifdef CONFIG_BT_COEXIST + if ((rtw_btcoex_IsBtDisabled(padapter) == _FALSE) + && (rtw_btcoex_IsBtControlLps(padapter) == _TRUE)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + + } +#endif /* CONFIG_BT_COEXIST */ + +#ifdef CONFIG_P2P_PS + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + if (iface->wdinfo.p2p_ps_mode > P2P_PS_NONE) { + pslv = PS_STATE_S2; + break; + } + } + } +#endif + _enter_pwrlock(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, XMIT_ALIVE); + + if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) + && (pwrctrl->bFwCurrentInPSMode == _TRUE)) { + + if (pwrctrl->cpwm > pslv) { + if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0)) + rtw_set_rpwm(padapter, pslv); + } + } + + _exit_pwrlock(&pwrctrl->lock); + +} + +/* + * Caller: ISR + * + * If all commands have been done, + * and no more command to do, + * then driver shall call this fun. to power down firmware again. + */ +void rtw_unregister_cmd_alive(PADAPTER padapter) +{ + _adapter *iface; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrctrl; + u8 pslv, i; + + + pwrctrl = adapter_to_pwrctl(padapter); + pslv = PS_STATE_S0; + +#ifdef CONFIG_BT_COEXIST + if ((rtw_btcoex_IsBtDisabled(padapter) == _FALSE) + && (rtw_btcoex_IsBtControlLps(padapter) == _TRUE)) { + u8 val8; + + val8 = rtw_btcoex_LpsVal(padapter); + if (val8 & BIT(4)) + pslv = PS_STATE_S2; + + } +#endif /* CONFIG_BT_COEXIST */ + +#ifdef CONFIG_P2P_PS + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + if (iface->wdinfo.p2p_ps_mode > P2P_PS_NONE) { + pslv = PS_STATE_S2; + break; + } + } + } +#endif + + _enter_pwrlock(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, CMD_ALIVE); + + if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) + && (pwrctrl->bFwCurrentInPSMode == _TRUE)) { + + if (pwrctrl->cpwm > pslv) { + if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0)) + rtw_set_rpwm(padapter, pslv); + } + } + + _exit_pwrlock(&pwrctrl->lock); + +} + +/* + * Caller: ISR + */ +void rtw_unregister_rx_alive(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrctrl; + + + pwrctrl = adapter_to_pwrctl(padapter); + + _enter_pwrlock(&pwrctrl->lock); + + unregister_task_alive(pwrctrl, RECV_ALIVE); + + + _exit_pwrlock(&pwrctrl->lock); + +} + +void rtw_unregister_evt_alive(PADAPTER padapter) +{ + struct pwrctrl_priv *pwrctrl; + + + pwrctrl = adapter_to_pwrctl(padapter); + + unregister_task_alive(pwrctrl, EVT_ALIVE); + + + _exit_pwrlock(&pwrctrl->lock); + +} +#endif /* CONFIG_LPS_LCLK */ + +#ifdef CONFIG_RESUME_IN_WORKQUEUE + static void resume_workitem_callback(struct work_struct *work); +#endif /* CONFIG_RESUME_IN_WORKQUEUE */ + +void rtw_init_pwrctrl_priv(PADAPTER padapter) +{ +#ifdef CONFIG_LPS_1T1R +#define LPS_1T1R_FMT ", LPS_1T1R=%d" +#define LPS_1T1R_ARG , pwrctrlpriv->lps_1t1r +#else +#define LPS_1T1R_FMT "" +#define LPS_1T1R_ARG +#endif + + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); +#ifdef CONFIG_WOWLAN + struct registry_priv *registry_par = &padapter->registrypriv; +#endif +#ifdef CONFIG_GPIO_WAKEUP + PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter); +#endif + +#if defined(CONFIG_CONCURRENT_MODE) + if (!is_primary_adapter(padapter)) + return; +#endif + + _init_pwrlock(&pwrctrlpriv->lock); + _init_pwrlock(&pwrctrlpriv->check_32k_lock); + pwrctrlpriv->rf_pwrstate = rf_on; + pwrctrlpriv->ips_enter_cnts = 0; + pwrctrlpriv->ips_leave_cnts = 0; + pwrctrlpriv->lps_enter_cnts = 0; + pwrctrlpriv->lps_leave_cnts = 0; + pwrctrlpriv->bips_processing = _FALSE; +#ifdef CONFIG_LPS_CHK_BY_TP + pwrctrlpriv->lps_chk_by_tp = padapter->registrypriv.lps_chk_by_tp; + pwrctrlpriv->lps_tx_tp_th = LPS_TX_TP_TH; + pwrctrlpriv->lps_rx_tp_th = LPS_RX_TP_TH; + pwrctrlpriv->lps_bi_tp_th = LPS_BI_TP_TH; + pwrctrlpriv->lps_chk_cnt = pwrctrlpriv->lps_chk_cnt_th = LPS_TP_CHK_CNT; + pwrctrlpriv->lps_tx_pkts = LPS_CHK_PKTS_TX; + pwrctrlpriv->lps_rx_pkts = LPS_CHK_PKTS_RX; +#endif + + pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode; + pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode; + pwrctrlpriv->ips_deny_time = rtw_get_current_time(); + pwrctrlpriv->lps_level = padapter->registrypriv.lps_level; +#ifdef CONFIG_LPS_1T1R + pwrctrlpriv->lps_1t1r = padapter->registrypriv.lps_1t1r; +#endif + + pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL; + pwrctrlpriv->pwr_state_check_cnts = 0; + pwrctrlpriv->bInSuspend = _FALSE; + pwrctrlpriv->bkeepfwalive = _FALSE; + pwrctrlpriv->LpsIdleCount = 0; + + /* pwrctrlpriv->FWCtrlPSMode =padapter->registrypriv.power_mgnt; */ /* PS_MODE_MIN; */ + if (padapter->registrypriv.mp_mode == 1) + pwrctrlpriv->power_mgnt = PS_MODE_ACTIVE ; + else + pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt; /* PS_MODE_MIN; */ + pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? _TRUE : _FALSE; + + pwrctrlpriv->bFwCurrentInPSMode = _FALSE; + pwrctrlpriv->lps_deny_time = rtw_get_current_time(); + + pwrctrlpriv->rpwm = 0; + pwrctrlpriv->cpwm = PS_STATE_S4; + + pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE; + pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps; + pwrctrlpriv->bcn_ant_mode = 0; + pwrctrlpriv->dtim = 0; + + pwrctrlpriv->tog = 0x80; + pwrctrlpriv->rpwm_retry = 0; + + RTW_INFO("%s: IPS_mode=%d, LPS_mode=%d, LPS_level=%d"LPS_1T1R_FMT"\n", + __func__, pwrctrlpriv->ips_mode, pwrctrlpriv->power_mgnt, pwrctrlpriv->lps_level + LPS_1T1R_ARG + ); + +#ifdef CONFIG_LPS_LCLK + rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm)); + + _init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL); + + _init_workitem(&pwrctrlpriv->dma_event, dma_event_callback, NULL); + +#ifdef CONFIG_LPS_RPWM_TIMER + pwrctrlpriv->brpwmtimeout = _FALSE; + _init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL); + rtw_init_timer(&pwrctrlpriv->pwr_rpwm_timer, padapter, pwr_rpwm_timeout_handler, padapter); +#endif /* CONFIG_LPS_RPWM_TIMER */ +#endif /* CONFIG_LPS_LCLK */ + +#ifdef CONFIG_LPS_PG + pwrctrlpriv->lpspg_info.name = "LPSPG_INFO"; + #ifdef CONFIG_RTL8822C + pwrctrlpriv->lpspg_dpk_info.name = "LPSPG_DPK_INFO"; + pwrctrlpriv->lpspg_iqk_info.name = "LPSPG_IQK_INFO"; + #endif +#endif + + rtw_init_timer(&pwrctrlpriv->pwr_state_check_timer, padapter, pwr_state_check_handler, padapter); + + pwrctrlpriv->wowlan_mode = _FALSE; + pwrctrlpriv->wowlan_ap_mode = _FALSE; + pwrctrlpriv->wowlan_p2p_mode = _FALSE; + pwrctrlpriv->wowlan_in_resume = _FALSE; + pwrctrlpriv->wowlan_last_wake_reason = 0; + +#ifdef CONFIG_RESUME_IN_WORKQUEUE + _init_workitem(&pwrctrlpriv->resume_work, resume_workitem_callback, NULL); + pwrctrlpriv->rtw_workqueue = create_singlethread_workqueue("rtw_workqueue"); +#endif /* CONFIG_RESUME_IN_WORKQUEUE */ + +#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER) + pwrctrlpriv->early_suspend.suspend = NULL; + rtw_register_early_suspend(pwrctrlpriv); +#endif /* CONFIG_HAS_EARLYSUSPEND || CONFIG_ANDROID_POWER */ + +#ifdef CONFIG_GPIO_WAKEUP + pwrctrlpriv->wowlan_gpio_index = WAKEUP_GPIO_IDX; + /* set output low state in initial */ + pwrctrlpriv->wowlan_gpio_output_state = GPIO_OUTPUT_LOW; + /*default low active*/ + pwrctrlpriv->is_high_active = HIGH_ACTIVE_DEV2HST; + pwrctrlpriv->hst2dev_high_active = HIGH_ACTIVE_HST2DEV; + +#if (defined(CONFIG_RTL8192F) && defined(CONFIG_USB_HCI) && defined(CONFIG_BT_COEXIST)) + if (pHalData->EEPROMBluetoothCoexist == _TRUE) { + /* for 8725AU case */ + pwrctrlpriv->wowlan_gpio_index = WAKEUP_GPIO_IDX_8725AU; + pwrctrlpriv->is_high_active = HIGH_ACTIVE_DEV2HST_8725AU; + } +#endif +#ifdef CONFIG_RTW_ONE_PIN_GPIO + rtw_hal_switch_gpio_wl_ctrl(padapter, pwrctrlpriv->wowlan_gpio_index, _TRUE); + rtw_hal_set_input_gpio(padapter, pwrctrlpriv->wowlan_gpio_index); +#else + #ifdef CONFIG_WAKEUP_GPIO_INPUT_MODE + if (pwrctrlpriv->is_high_active == 0) + rtw_hal_set_input_gpio(padapter, pwrctrlpriv->wowlan_gpio_index); + else + rtw_hal_set_output_gpio(padapter, pwrctrlpriv->wowlan_gpio_index, + GPIO_OUTPUT_LOW); + #else + rtw_hal_set_output_gpio(padapter, pwrctrlpriv->wowlan_gpio_index + , pwrctrlpriv->wowlan_gpio_output_state); + rtw_hal_switch_gpio_wl_ctrl(padapter, pwrctrlpriv->wowlan_gpio_index, _TRUE); + RTW_INFO("%s: set GPIO_%d to OUTPUT %s state in initial and %s_ACTIVE.\n", + __func__, pwrctrlpriv->wowlan_gpio_index, + pwrctrlpriv->wowlan_gpio_output_state ? "HIGH" : "LOW", + pwrctrlpriv->is_high_active ? "HIGI" : "LOW"); + #endif /*CONFIG_WAKEUP_GPIO_INPUT_MODE*/ +#endif /* CONFIG_RTW_ONE_PIN_GPIO */ +#endif /* CONFIG_GPIO_WAKEUP */ + +#ifdef CONFIG_WOWLAN +#ifdef CONFIG_LPS_1T1R +#define WOW_LPS_1T1R_FMT ", WOW_LPS_1T1R=%d" +#define WOW_LPS_1T1R_ARG , pwrctrlpriv->wowlan_lps_1t1r +#else +#define WOW_LPS_1T1R_FMT "" +#define WOW_LPS_1T1R_ARG +#endif + + pwrctrlpriv->wowlan_power_mgmt = padapter->registrypriv.wow_power_mgnt; + pwrctrlpriv->wowlan_lps_level = padapter->registrypriv.wow_lps_level; +#ifdef CONFIG_LPS_1T1R + pwrctrlpriv->wowlan_lps_1t1r = padapter->registrypriv.wow_lps_1t1r; +#endif + + RTW_INFO("%s: WOW_LPS_mode=%d, WOW_LPS_level=%d"WOW_LPS_1T1R_FMT"\n", + __func__, pwrctrlpriv->wowlan_power_mgmt, pwrctrlpriv->wowlan_lps_level + WOW_LPS_1T1R_ARG + ); + + if (registry_par->wakeup_event & BIT(1)) + pwrctrlpriv->default_patterns_en = _TRUE; + else + pwrctrlpriv->default_patterns_en = _FALSE; + + rtw_wow_pattern_sw_reset(padapter); +#ifdef CONFIG_PNO_SUPPORT + pwrctrlpriv->pno_inited = _FALSE; + pwrctrlpriv->pnlo_info = NULL; + pwrctrlpriv->pscan_info = NULL; + pwrctrlpriv->pno_ssid_list = NULL; +#endif /* CONFIG_PNO_SUPPORT */ +#ifdef CONFIG_WOW_PATTERN_HW_CAM + _rtw_mutex_init(&pwrctrlpriv->wowlan_pattern_cam_mutex); +#endif + +#ifdef CONFIG_WOW_KEEP_ALIVE_PATTERN + pwrctrlpriv->wowlan_keep_alive_ack_index = 0xFF; + pwrctrlpriv->wowlan_wake_pattern_index = 0xFF; +#endif/*CONFIG_WOW_KEEP_ALIVE_PATTERN*/ + pwrctrlpriv->wowlan_aoac_rpt_loc = 0; +#ifdef CONFIG_WAR_OFFLOAD +#if defined(CONFIG_OFFLOAD_MDNS_V4) || defined(CONFIG_OFFLOAD_MDNS_V6) + rtw_wow_war_mdns_parms_reset(padapter, _TRUE); +#endif /* defined(CONFIG_OFFLOAD_MDNS_V4) || defined(CONFIG_OFFLOAD_MDNS_V6) */ +#endif /* CONFIG_WAR_OFFLOAD */ +#endif /* CONFIG_WOWLAN */ + +#ifdef CONFIG_LPS_POFF + rtw_hal_set_hwreg(padapter, HW_VAR_LPS_POFF_INIT, 0); +#endif + +#ifdef CONFIG_LPS_ACK + _rtw_mutex_init(&pwrctrlpriv->lps_ack_mutex); + pwrctrlpriv->lps_ack_status = -1; +#endif /* CONFIG_LPS_ACK */ +} + + +void rtw_free_pwrctrl_priv(PADAPTER adapter) +{ + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(adapter); + +#if defined(CONFIG_CONCURRENT_MODE) + if (!is_primary_adapter(adapter)) + return; +#endif + + + /* _rtw_memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv)); */ + + +#ifdef CONFIG_RESUME_IN_WORKQUEUE + if (pwrctrlpriv->rtw_workqueue) { + flush_workqueue(pwrctrlpriv->rtw_workqueue); + destroy_workqueue(pwrctrlpriv->rtw_workqueue); + } +#endif + +#ifdef CONFIG_LPS_POFF + rtw_hal_set_hwreg(adapter, HW_VAR_LPS_POFF_DEINIT, 0); +#endif + +#ifdef CONFIG_LPS_LCLK + _cancel_workitem_sync(&pwrctrlpriv->cpwm_event); + _cancel_workitem_sync(&pwrctrlpriv->dma_event); + #ifdef CONFIG_LPS_RPWM_TIMER + _cancel_workitem_sync(&pwrctrlpriv->rpwmtimeoutwi); + #endif +#endif /* CONFIG_LPS_LCLK */ + +#ifdef CONFIG_LPS_PG + rsvd_page_cache_free(&pwrctrlpriv->lpspg_info); + #ifdef CONFIG_RTL8822C + rsvd_page_cache_free(&pwrctrlpriv->lpspg_dpk_info); + rsvd_page_cache_free(&pwrctrlpriv->lpspg_iqk_info); + #endif +#endif + +#ifdef CONFIG_WOWLAN +#ifdef CONFIG_PNO_SUPPORT + if (pwrctrlpriv->pnlo_info != NULL) + printk("****** pnlo_info memory leak********\n"); + + if (pwrctrlpriv->pscan_info != NULL) + printk("****** pscan_info memory leak********\n"); + + if (pwrctrlpriv->pno_ssid_list != NULL) + printk("****** pno_ssid_list memory leak********\n"); +#endif +#ifdef CONFIG_WOW_PATTERN_HW_CAM + _rtw_mutex_free(&pwrctrlpriv->wowlan_pattern_cam_mutex); +#endif + +#endif /* CONFIG_WOWLAN */ + +#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER) + rtw_unregister_early_suspend(pwrctrlpriv); +#endif /* CONFIG_HAS_EARLYSUSPEND || CONFIG_ANDROID_POWER */ + + _free_pwrlock(&pwrctrlpriv->lock); + _free_pwrlock(&pwrctrlpriv->check_32k_lock); + +#ifdef CONFIG_LPS_ACK + _rtw_mutex_free(&pwrctrlpriv->lps_ack_mutex); +#endif /* CONFIG_LPS_ACK */ +} + +#ifdef CONFIG_RESUME_IN_WORKQUEUE +extern int rtw_resume_process(_adapter *padapter); + +static void resume_workitem_callback(struct work_struct *work) +{ + struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, resume_work); + struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + + RTW_INFO("%s\n", __FUNCTION__); + + rtw_resume_process(adapter); + + rtw_resume_unlock_suspend(); +} + +void rtw_resume_in_workqueue(struct pwrctrl_priv *pwrpriv) +{ + /* accquire system's suspend lock preventing from falliing asleep while resume in workqueue */ + /* rtw_lock_suspend(); */ + + rtw_resume_lock_suspend(); + +#if 1 + queue_work(pwrpriv->rtw_workqueue, &pwrpriv->resume_work); +#else + _set_workitem(&pwrpriv->resume_work); +#endif +} +#endif /* CONFIG_RESUME_IN_WORKQUEUE */ + +#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER) +inline bool rtw_is_earlysuspend_registered(struct pwrctrl_priv *pwrpriv) +{ + return (pwrpriv->early_suspend.suspend) ? _TRUE : _FALSE; +} + +inline bool rtw_is_do_late_resume(struct pwrctrl_priv *pwrpriv) +{ + return (pwrpriv->do_late_resume) ? _TRUE : _FALSE; +} + +inline void rtw_set_do_late_resume(struct pwrctrl_priv *pwrpriv, bool enable) +{ + pwrpriv->do_late_resume = enable; +} +#endif + +#ifdef CONFIG_HAS_EARLYSUSPEND +extern int rtw_resume_process(_adapter *padapter); +static void rtw_early_suspend(struct early_suspend *h) +{ + struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend); + RTW_INFO("%s\n", __FUNCTION__); + + rtw_set_do_late_resume(pwrpriv, _FALSE); +} + +static void rtw_late_resume(struct early_suspend *h) +{ + struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend); + struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + + RTW_INFO("%s\n", __FUNCTION__); + + if (pwrpriv->do_late_resume) { + rtw_set_do_late_resume(pwrpriv, _FALSE); + rtw_resume_process(adapter); + } +} + +void rtw_register_early_suspend(struct pwrctrl_priv *pwrpriv) +{ + RTW_INFO("%s\n", __FUNCTION__); + + /* jeff: set the early suspend level before blank screen, so we wll do late resume after scree is lit */ + pwrpriv->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN - 20; + pwrpriv->early_suspend.suspend = rtw_early_suspend; + pwrpriv->early_suspend.resume = rtw_late_resume; + register_early_suspend(&pwrpriv->early_suspend); + + +} + +void rtw_unregister_early_suspend(struct pwrctrl_priv *pwrpriv) +{ + RTW_INFO("%s\n", __FUNCTION__); + + rtw_set_do_late_resume(pwrpriv, _FALSE); + + if (pwrpriv->early_suspend.suspend) + unregister_early_suspend(&pwrpriv->early_suspend); + + pwrpriv->early_suspend.suspend = NULL; + pwrpriv->early_suspend.resume = NULL; +} +#endif /* CONFIG_HAS_EARLYSUSPEND */ + +#ifdef CONFIG_ANDROID_POWER +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + extern int rtw_resume_process(PADAPTER padapter); +#endif +static void rtw_early_suspend(android_early_suspend_t *h) +{ + struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend); + RTW_INFO("%s\n", __FUNCTION__); + + rtw_set_do_late_resume(pwrpriv, _FALSE); +} + +static void rtw_late_resume(android_early_suspend_t *h) +{ + struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend); + struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + + RTW_INFO("%s\n", __FUNCTION__); + if (pwrpriv->do_late_resume) { +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + rtw_set_do_late_resume(pwrpriv, _FALSE); + rtw_resume_process(adapter); +#endif + } +} + +void rtw_register_early_suspend(struct pwrctrl_priv *pwrpriv) +{ + RTW_INFO("%s\n", __FUNCTION__); + + /* jeff: set the early suspend level before blank screen, so we wll do late resume after scree is lit */ + pwrpriv->early_suspend.level = ANDROID_EARLY_SUSPEND_LEVEL_BLANK_SCREEN - 20; + pwrpriv->early_suspend.suspend = rtw_early_suspend; + pwrpriv->early_suspend.resume = rtw_late_resume; + android_register_early_suspend(&pwrpriv->early_suspend); +} + +void rtw_unregister_early_suspend(struct pwrctrl_priv *pwrpriv) +{ + RTW_INFO("%s\n", __FUNCTION__); + + rtw_set_do_late_resume(pwrpriv, _FALSE); + + if (pwrpriv->early_suspend.suspend) + android_unregister_early_suspend(&pwrpriv->early_suspend); + + pwrpriv->early_suspend.suspend = NULL; + pwrpriv->early_suspend.resume = NULL; +} +#endif /* CONFIG_ANDROID_POWER */ + +u8 rtw_interface_ps_func(_adapter *padapter, HAL_INTF_PS_FUNC efunc_id, u8 *val) +{ + u8 bResult = _TRUE; + rtw_hal_intf_ps_func(padapter, efunc_id, val); + + return bResult; +} + + +inline void rtw_set_ips_deny(_adapter *padapter, u32 ms) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ms); +} + +/* +* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend +* @adapter: pointer to _adapter structure +* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup +* Return _SUCCESS or _FAIL +*/ + +int _rtw_pwr_wakeup(_adapter *padapter, u32 ips_deffer_ms, const char *caller) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj); + struct mlme_priv *pmlmepriv; + int ret = _SUCCESS; + systime start = rtw_get_current_time(); + + /*RTW_INFO(FUNC_ADPT_FMT "===>\n", FUNC_ADPT_ARG(padapter));*/ + /* for LPS */ + LeaveAllPowerSaveMode(padapter); + + /* IPS still bound with primary adapter */ + padapter = GET_PRIMARY_ADAPTER(padapter); + pmlmepriv = &padapter->mlmepriv; + + if (rtw_time_after(rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms), pwrpriv->ips_deny_time)) + pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms); + + + if (pwrpriv->ps_processing) { + RTW_INFO("%s wait ps_processing...\n", __func__); + while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000) + rtw_msleep_os(10); + if (pwrpriv->ps_processing) + RTW_INFO("%s wait ps_processing timeout\n", __func__); + else + RTW_INFO("%s wait ps_processing done\n", __func__); + } + +#ifdef DBG_CONFIG_ERROR_DETECT + if (rtw_hal_sreset_inprogress(padapter)) { + RTW_INFO("%s wait sreset_inprogress...\n", __func__); + while (rtw_hal_sreset_inprogress(padapter) && rtw_get_passing_time_ms(start) <= 4000) + rtw_msleep_os(10); + if (rtw_hal_sreset_inprogress(padapter)) + RTW_INFO("%s wait sreset_inprogress timeout\n", __func__); + else + RTW_INFO("%s wait sreset_inprogress done\n", __func__); + } +#endif + + if (pwrpriv->bInSuspend) { + RTW_INFO("%s wait bInSuspend...\n", __func__); + while (pwrpriv->bInSuspend + && ((rtw_get_passing_time_ms(start) <= 3000 && !rtw_is_do_late_resume(pwrpriv)) + || (rtw_get_passing_time_ms(start) <= 500 && rtw_is_do_late_resume(pwrpriv))) + ) + rtw_msleep_os(10); + if (pwrpriv->bInSuspend) + RTW_INFO("%s wait bInSuspend timeout\n", __func__); + else + RTW_INFO("%s wait bInSuspend done\n", __func__); + } + + /* System suspend is not allowed to wakeup */ + if (_TRUE == pwrpriv->bInSuspend) { + ret = _FAIL; + goto exit; + } + /* I think this should be check in IPS, LPS, autosuspend functions... */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) { + ret = _SUCCESS; + goto exit; + } + + if (rf_off == pwrpriv->rf_pwrstate) { + +#ifdef CONFIG_IPS + RTW_INFO("%s call ips_leave....\n", __FUNCTION__); + if (_FAIL == ips_leave(padapter)) { + RTW_INFO("======> ips_leave fail.............\n"); + ret = _FAIL; + goto exit; + } +#endif + + } + + /* TODO: the following checking need to be merged... */ + if (rtw_is_drv_stopped(padapter) + || !padapter->bup + || !rtw_is_hw_init_completed(padapter) + ) { + RTW_INFO("%s: bDriverStopped=%s, bup=%d, hw_init_completed=%u\n" + , caller + , rtw_is_drv_stopped(padapter) ? "True" : "False" + , padapter->bup + , rtw_get_hw_init_completed(padapter)); + ret = _FALSE; + goto exit; + } + +exit: + if (rtw_time_after(rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms), pwrpriv->ips_deny_time)) + pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms); + /*RTW_INFO(FUNC_ADPT_FMT "<===\n", FUNC_ADPT_ARG(padapter));*/ + return ret; + +} + +int rtw_pm_set_lps(_adapter *padapter, u8 mode) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (mode < PS_MODE_NUM) { + if (pwrctrlpriv->power_mgnt != mode) { + if (PS_MODE_ACTIVE == mode) + LeaveAllPowerSaveMode(padapter); + else + pwrctrlpriv->LpsIdleCount = 2; + pwrctrlpriv->power_mgnt = mode; + pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? _TRUE : _FALSE; + } + } else + ret = -EINVAL; + + return ret; +} + +int rtw_pm_set_lps_level(_adapter *padapter, u8 level) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (level < LPS_LEVEL_MAX) { + if (pwrctrlpriv->lps_level != level) { + #ifdef CONFIG_LPS + if (rtw_lps_ctrl_leave_set_level_cmd(padapter, level, RTW_CMDF_WAIT_ACK) != _SUCCESS) + #endif + pwrctrlpriv->lps_level = level; + } + } else + ret = -EINVAL; + + return ret; +} + +#ifdef CONFIG_LPS_1T1R +int rtw_pm_set_lps_1t1r(_adapter *padapter, u8 en) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + en = en ? 1 : 0; + if (pwrctrlpriv->lps_1t1r != en) { + if (rtw_lps_ctrl_leave_set_1t1r_cmd(padapter, en, RTW_CMDF_WAIT_ACK) != _SUCCESS) + pwrctrlpriv->lps_1t1r = en; + } + + return ret; +} +#endif + +inline void rtw_set_lps_deny(_adapter *adapter, u32 ms) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + pwrpriv->lps_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ms); +} + +#ifdef CONFIG_WOWLAN +int rtw_pm_set_wow_lps(_adapter *padapter, u8 mode) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (mode < PS_MODE_NUM) { + if (pwrctrlpriv->wowlan_power_mgmt != mode) + pwrctrlpriv->wowlan_power_mgmt = mode; + } else + ret = -EINVAL; + + return ret; +} +int rtw_pm_set_wow_lps_level(_adapter *padapter, u8 level) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (level < LPS_LEVEL_MAX) + pwrctrlpriv->wowlan_lps_level = level; + else + ret = -EINVAL; + + return ret; +} + +#ifdef CONFIG_LPS_1T1R +int rtw_pm_set_wow_lps_1t1r(_adapter *padapter, u8 en) +{ + int ret = 0; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + en = en ? 1 : 0; + pwrctrlpriv->wowlan_lps_1t1r = en; + + return ret; +} +#endif /* CONFIG_LPS_1T1R */ +#endif /* CONFIG_WOWLAN */ + +int rtw_pm_set_ips(_adapter *padapter, u8 mode) +{ + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); + + if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) { + rtw_ips_mode_req(pwrctrlpriv, mode); + RTW_INFO("%s %s\n", __FUNCTION__, mode == IPS_NORMAL ? "IPS_NORMAL" : "IPS_LEVEL_2"); + return 0; + } else if (mode == IPS_NONE) { + rtw_ips_mode_req(pwrctrlpriv, mode); + RTW_INFO("%s %s\n", __FUNCTION__, "IPS_NONE"); + if (!rtw_is_surprise_removed(padapter) && (_FAIL == rtw_pwr_wakeup(padapter))) + return -EFAULT; + } else + return -EINVAL; + return 0; +} + +/* + * ATTENTION: + * This function will request pwrctrl LOCK! + */ +void rtw_ps_deny(PADAPTER padapter, PS_DENY_REASON reason) +{ + struct pwrctrl_priv *pwrpriv; + + /* RTW_INFO("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n", + * FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */ + + pwrpriv = adapter_to_pwrctl(padapter); + + _enter_pwrlock(&pwrpriv->lock); + if (pwrpriv->ps_deny & BIT(reason)) { + RTW_INFO(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n", + FUNC_ADPT_ARG(padapter), reason); + } + pwrpriv->ps_deny |= BIT(reason); + _exit_pwrlock(&pwrpriv->lock); + + /* RTW_INFO("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", + * FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */ +} + +/* + * ATTENTION: + * This function will request pwrctrl LOCK! + */ +void rtw_ps_deny_cancel(PADAPTER padapter, PS_DENY_REASON reason) +{ + struct pwrctrl_priv *pwrpriv; + + + /* RTW_INFO("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n", + * FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */ + + pwrpriv = adapter_to_pwrctl(padapter); + + _enter_pwrlock(&pwrpriv->lock); + if ((pwrpriv->ps_deny & BIT(reason)) == 0) { + RTW_INFO(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n", + FUNC_ADPT_ARG(padapter), reason); + } + pwrpriv->ps_deny &= ~BIT(reason); + _exit_pwrlock(&pwrpriv->lock); + + /* RTW_INFO("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", + * FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */ +} + +/* + * ATTENTION: + * Before calling this function pwrctrl lock should be occupied already, + * otherwise it may return incorrect value. + */ +u32 rtw_ps_deny_get(PADAPTER padapter) +{ + u32 deny; + + + deny = adapter_to_pwrctl(padapter)->ps_deny; + + return deny; +} + +static void _rtw_ssmps(_adapter *adapter, struct sta_info *sta) +{ + struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (MLME_IS_STA(adapter)) { + issue_action_SM_PS_wait_ack(adapter , get_my_bssid(&(pmlmeinfo->network)), + sta->cmn.sm_ps, 3 , 1); + } + else if (MLME_IS_AP(adapter)) { + + } + rtw_phydm_ra_registed(adapter, sta); +} +void rtw_ssmps_enter(_adapter *adapter, struct sta_info *sta) +{ + if (sta->cmn.sm_ps == SM_PS_STATIC) + return; + + RTW_INFO(ADPT_FMT" STA [" MAC_FMT "]\n", ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + + sta->cmn.sm_ps = SM_PS_STATIC; + _rtw_ssmps(adapter, sta); +} +void rtw_ssmps_leave(_adapter *adapter, struct sta_info *sta) +{ + if (sta->cmn.sm_ps == SM_PS_DISABLE) + return; + + RTW_INFO(ADPT_FMT" STA [" MAC_FMT "] \n", ADPT_ARG(adapter), MAC_ARG(sta->cmn.mac_addr)); + sta->cmn.sm_ps = SM_PS_DISABLE; + _rtw_ssmps(adapter, sta); +} + diff --git a/core/rtw_recv.c b/core/rtw_recv.c new file mode 100644 index 0000000..90f27ac --- /dev/null +++ b/core/rtw_recv.c @@ -0,0 +1,4959 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_RECV_C_ + +#include +#include + +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS +static void rtw_signal_stat_timer_hdl(void *ctx); + +enum { + SIGNAL_STAT_CALC_PROFILE_0 = 0, + SIGNAL_STAT_CALC_PROFILE_1, + SIGNAL_STAT_CALC_PROFILE_2, + SIGNAL_STAT_CALC_PROFILE_MAX +}; + +u8 signal_stat_calc_profile[SIGNAL_STAT_CALC_PROFILE_MAX][3] = { + {4, 1}, /* Profile 0 => pre_stat : curr_stat = 4 : 1 */ + {3, 7}, /* Profile 1 => pre_stat : curr_stat = 3 : 7 */ + {0, 10} /* Profile 2 => pre_stat : curr_stat = 0 : 10 */ +}; + +#ifndef RTW_SIGNAL_STATE_CALC_PROFILE + #define RTW_SIGNAL_STATE_CALC_PROFILE SIGNAL_STAT_CALC_PROFILE_1 +#endif + +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + +u8 rtw_bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 }; +u8 rtw_rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 }; +static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37}; +static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3}; +#ifdef CONFIG_TDLS +static u8 SNAP_ETH_TYPE_TDLS[2] = {0x89, 0x0d}; +#endif + +void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv) +{ + + + + _rtw_memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv)); + + _rtw_spinlock_init(&psta_recvpriv->lock); + + /* for(i=0; iblk_strms[i]); */ + + _rtw_init_queue(&psta_recvpriv->defrag_q); + + +} + +sint _rtw_init_recv_priv(struct recv_priv *precvpriv, _adapter *padapter) +{ + sint i; + + union recv_frame *precvframe; + sint res = _SUCCESS; + + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */ + /* _rtw_memset((unsigned char *)precvpriv, 0, sizeof (struct recv_priv)); */ + + _rtw_spinlock_init(&precvpriv->lock); + +#ifdef CONFIG_RECV_THREAD_MODE + _rtw_init_sema(&precvpriv->recv_sema, 0); + +#endif + + _rtw_init_queue(&precvpriv->free_recv_queue); + _rtw_init_queue(&precvpriv->recv_pending_queue); + _rtw_init_queue(&precvpriv->uc_swdec_pending_queue); + + precvpriv->adapter = padapter; + + precvpriv->free_recvframe_cnt = NR_RECVFRAME; + + precvpriv->sink_udpport = 0; + precvpriv->pre_rtp_rxseq = 0; + precvpriv->cur_rtp_rxseq = 0; + +#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA + precvpriv->store_law_data_flag = 1; +#else + precvpriv->store_law_data_flag = 0; +#endif + + rtw_os_recv_resource_init(precvpriv, padapter); + + precvpriv->pallocated_frame_buf = rtw_zvmalloc(NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); + + if (precvpriv->pallocated_frame_buf == NULL) { + res = _FAIL; + goto exit; + } + /* _rtw_memset(precvpriv->pallocated_frame_buf, 0, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); */ + + precvpriv->precv_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ); + /* precvpriv->precv_frame_buf = precvpriv->pallocated_frame_buf + RXFRAME_ALIGN_SZ - */ + /* ((SIZE_PTR) (precvpriv->pallocated_frame_buf) &(RXFRAME_ALIGN_SZ-1)); */ + + precvframe = (union recv_frame *) precvpriv->precv_frame_buf; + + + for (i = 0; i < NR_RECVFRAME ; i++) { + _rtw_init_listhead(&(precvframe->u.list)); + + rtw_list_insert_tail(&(precvframe->u.list), &(precvpriv->free_recv_queue.queue)); + + rtw_os_recv_resource_alloc(padapter, precvframe); + + precvframe->u.hdr.len = 0; + + precvframe->u.hdr.adapter = padapter; + precvframe++; + + } + +#ifdef CONFIG_USB_HCI + + ATOMIC_SET(&(precvpriv->rx_pending_cnt), 1); + + _rtw_init_sema(&precvpriv->allrxreturnevt, 0); + +#endif + + res = rtw_hal_init_recv_priv(padapter); + +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + rtw_init_timer(&precvpriv->signal_stat_timer, padapter, rtw_signal_stat_timer_hdl, padapter); + + precvpriv->signal_stat_sampling_interval = 2000; /* ms */ + /* precvpriv->signal_stat_converging_constant = 5000; */ /* ms */ + + rtw_set_signal_stat_timer(precvpriv); +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + +exit: + + + return res; + +} + +void rtw_mfree_recv_priv_lock(struct recv_priv *precvpriv); +void rtw_mfree_recv_priv_lock(struct recv_priv *precvpriv) +{ + _rtw_spinlock_free(&precvpriv->lock); +#ifdef CONFIG_RECV_THREAD_MODE + _rtw_free_sema(&precvpriv->recv_sema); +#endif + + _rtw_spinlock_free(&precvpriv->free_recv_queue.lock); + _rtw_spinlock_free(&precvpriv->recv_pending_queue.lock); + + _rtw_spinlock_free(&precvpriv->free_recv_buf_queue.lock); + +#ifdef CONFIG_USE_USB_BUFFER_ALLOC_RX + _rtw_spinlock_free(&precvpriv->recv_buf_pending_queue.lock); +#endif /* CONFIG_USE_USB_BUFFER_ALLOC_RX */ +} + +void _rtw_free_recv_priv(struct recv_priv *precvpriv) +{ + _adapter *padapter = precvpriv->adapter; + + + rtw_free_uc_swdec_pending_queue(padapter); + + rtw_mfree_recv_priv_lock(precvpriv); + + rtw_os_recv_resource_free(precvpriv); + + if (precvpriv->pallocated_frame_buf) + rtw_vmfree(precvpriv->pallocated_frame_buf, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); + + rtw_hal_free_recv_priv(padapter); + + +} + +bool rtw_rframe_del_wfd_ie(union recv_frame *rframe, u8 ies_offset) +{ +#define DBG_RFRAME_DEL_WFD_IE 0 + u8 *ies = rframe->u.hdr.rx_data + sizeof(struct rtw_ieee80211_hdr_3addr) + ies_offset; + uint ies_len_ori = rframe->u.hdr.len - (ies - rframe->u.hdr.rx_data); + uint ies_len; + + ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_RFRAME_DEL_WFD_IE ? __func__ : NULL); + rframe->u.hdr.len -= ies_len_ori - ies_len; + + return ies_len_ori != ies_len; +} + +union recv_frame *_rtw_alloc_recvframe(_queue *pfree_recv_queue) +{ + + union recv_frame *precvframe; + _list *plist, *phead; + _adapter *padapter; + struct recv_priv *precvpriv; + + if (_rtw_queue_empty(pfree_recv_queue) == _TRUE) + precvframe = NULL; + else { + phead = get_list_head(pfree_recv_queue); + + plist = get_next(phead); + + precvframe = LIST_CONTAINOR(plist, union recv_frame, u); + + rtw_list_delete(&precvframe->u.hdr.list); + padapter = precvframe->u.hdr.adapter; + if (padapter != NULL) { + precvpriv = &padapter->recvpriv; + if (pfree_recv_queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt--; + } + } + + + return precvframe; + +} + +union recv_frame *rtw_alloc_recvframe(_queue *pfree_recv_queue) +{ + _irqL irqL; + union recv_frame *precvframe; + + _enter_critical_bh(&pfree_recv_queue->lock, &irqL); + + precvframe = _rtw_alloc_recvframe(pfree_recv_queue); + + _exit_critical_bh(&pfree_recv_queue->lock, &irqL); + + return precvframe; +} + +void rtw_init_recvframe(union recv_frame *precvframe, struct recv_priv *precvpriv) +{ + /* Perry: This can be removed */ + _rtw_init_listhead(&precvframe->u.hdr.list); + + precvframe->u.hdr.len = 0; +} + +int rtw_free_recvframe(union recv_frame *precvframe, _queue *pfree_recv_queue) +{ + _irqL irqL; + _adapter *padapter = precvframe->u.hdr.adapter; + struct recv_priv *precvpriv = &padapter->recvpriv; + + +#ifdef CONFIG_CONCURRENT_MODE + padapter = GET_PRIMARY_ADAPTER(padapter); + precvpriv = &padapter->recvpriv; + pfree_recv_queue = &precvpriv->free_recv_queue; + precvframe->u.hdr.adapter = padapter; +#endif + + + rtw_os_free_recvframe(precvframe); + + + _enter_critical_bh(&pfree_recv_queue->lock, &irqL); + + rtw_list_delete(&(precvframe->u.hdr.list)); + + precvframe->u.hdr.len = 0; + precvframe->u.hdr.attrib.phy_info.physts_rpt_valid = _FALSE; + + rtw_list_insert_tail(&(precvframe->u.hdr.list), get_list_head(pfree_recv_queue)); + + if (padapter != NULL) { + if (pfree_recv_queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt++; + } + + _exit_critical_bh(&pfree_recv_queue->lock, &irqL); + + + return _SUCCESS; + +} + + + + +sint _rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue) +{ + + _adapter *padapter = precvframe->u.hdr.adapter; + struct recv_priv *precvpriv = &padapter->recvpriv; + + + /* _rtw_init_listhead(&(precvframe->u.hdr.list)); */ + rtw_list_delete(&(precvframe->u.hdr.list)); + + + rtw_list_insert_tail(&(precvframe->u.hdr.list), get_list_head(queue)); + + if (padapter != NULL) { + if (queue == &precvpriv->free_recv_queue) + precvpriv->free_recvframe_cnt++; + } + + + return _SUCCESS; +} + +sint rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue) +{ + sint ret; + _irqL irqL; + + /* _spinlock(&pfree_recv_queue->lock); */ + _enter_critical_bh(&queue->lock, &irqL); + ret = _rtw_enqueue_recvframe(precvframe, queue); + /* _rtw_spinunlock(&pfree_recv_queue->lock); */ + _exit_critical_bh(&queue->lock, &irqL); + + return ret; +} + +/* +sint rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue) +{ + return rtw_free_recvframe(precvframe, queue); +} +*/ + + + + +/* +caller : defrag ; recvframe_chk_defrag in recv_thread (passive) +pframequeue: defrag_queue : will be accessed in recv_thread (passive) + +using spinlock to protect + +*/ + +void rtw_free_recvframe_queue(_queue *pframequeue, _queue *pfree_recv_queue) +{ + union recv_frame *precvframe; + _list *plist, *phead; + + _rtw_spinlock(&pframequeue->lock); + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + precvframe = LIST_CONTAINOR(plist, union recv_frame, u); + + plist = get_next(plist); + + /* rtw_list_delete(&precvframe->u.hdr.list); */ /* will do this in rtw_free_recvframe() */ + + rtw_free_recvframe(precvframe, pfree_recv_queue); + } + + _rtw_spinunlock(&pframequeue->lock); + + +} + +u32 rtw_free_uc_swdec_pending_queue(_adapter *adapter) +{ + u32 cnt = 0; + union recv_frame *pending_frame; + while ((pending_frame = rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) { + rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue); + cnt++; + } + + if (cnt) + RTW_INFO(FUNC_ADPT_FMT" dequeue %d\n", FUNC_ADPT_ARG(adapter), cnt); + + return cnt; +} + + +#ifndef CONFIG_RECVBUF_QUEUE_LOCK_BH +#ifdef CONFIG_SDIO_HCI +#define CONFIG_RECVBUF_QUEUE_LOCK_BH 1 +#else +#define CONFIG_RECVBUF_QUEUE_LOCK_BH 0 +#endif +#endif /* CONFIG_RECVBUF_QUEUE_LOCK_BH */ + +sint rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, _queue *queue) +{ + _irqL irqL; + +#if CONFIG_RECVBUF_QUEUE_LOCK_BH + _enter_critical_bh(&queue->lock, &irqL); +#else + _enter_critical_ex(&queue->lock, &irqL); +#endif + + rtw_list_delete(&precvbuf->list); + rtw_list_insert_head(&precvbuf->list, get_list_head(queue)); + +#if CONFIG_RECVBUF_QUEUE_LOCK_BH + _exit_critical_bh(&queue->lock, &irqL); +#else + _exit_critical_ex(&queue->lock, &irqL); +#endif + + return _SUCCESS; +} + +sint rtw_enqueue_recvbuf(struct recv_buf *precvbuf, _queue *queue) +{ + _irqL irqL; + +#if CONFIG_RECVBUF_QUEUE_LOCK_BH + _enter_critical_bh(&queue->lock, &irqL); +#else + _enter_critical_ex(&queue->lock, &irqL); +#endif + + rtw_list_delete(&precvbuf->list); + + rtw_list_insert_tail(&precvbuf->list, get_list_head(queue)); + +#if CONFIG_RECVBUF_QUEUE_LOCK_BH + _exit_critical_bh(&queue->lock, &irqL); +#else + _exit_critical_ex(&queue->lock, &irqL); +#endif + + return _SUCCESS; + +} + +struct recv_buf *rtw_dequeue_recvbuf(_queue *queue) +{ + _irqL irqL; + struct recv_buf *precvbuf; + _list *plist, *phead; + +#if CONFIG_RECVBUF_QUEUE_LOCK_BH + _enter_critical_bh(&queue->lock, &irqL); +#else + _enter_critical_ex(&queue->lock, &irqL); +#endif + + if (_rtw_queue_empty(queue) == _TRUE) + precvbuf = NULL; + else { + phead = get_list_head(queue); + + plist = get_next(phead); + + precvbuf = LIST_CONTAINOR(plist, struct recv_buf, list); + + rtw_list_delete(&precvbuf->list); + + } + +#if CONFIG_RECVBUF_QUEUE_LOCK_BH + _exit_critical_bh(&queue->lock, &irqL); +#else + _exit_critical_ex(&queue->lock, &irqL); +#endif + + return precvbuf; + +} + +sint recvframe_chkmic(_adapter *adapter, union recv_frame *precvframe); +sint recvframe_chkmic(_adapter *adapter, union recv_frame *precvframe) +{ + + sint i, res = _SUCCESS; + u32 datalen; + u8 miccode[8]; + u8 bmic_err = _FALSE, brpt_micerror = _TRUE; + u8 *pframe, *payload, *pframemic; + u8 *mickey; + /* u8 *iv,rxdata_key_idx=0; */ + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &precvframe->u.hdr.attrib; + struct security_priv *psecuritypriv = &adapter->securitypriv; + + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + stainfo = rtw_get_stainfo(&adapter->stapriv , &prxattrib->ta[0]); + + if (prxattrib->encrypt == _TKIP_) { + + /* calculate mic code */ + if (stainfo != NULL) { + if (IS_MCAST(prxattrib->ra)) { + /* mickey=&psecuritypriv->dot118021XGrprxmickey.skey[0]; */ + /* iv = precvframe->u.hdr.rx_data+prxattrib->hdrlen; */ + /* rxdata_key_idx =( ((iv[3])>>6)&0x3) ; */ + mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0]; + + /* RTW_INFO("\n recvframe_chkmic: bcmc key psecuritypriv->dot118021XGrpKeyid(%d),pmlmeinfo->key_index(%d) ,recv key_id(%d)\n", */ + /* psecuritypriv->dot118021XGrpKeyid,pmlmeinfo->key_index,rxdata_key_idx); */ + + if (psecuritypriv->binstallGrpkey == _FALSE) { + res = _FAIL; + RTW_INFO("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n"); + goto exit; + } + } else { + mickey = &stainfo->dot11tkiprxmickey.skey[0]; + } + + datalen = precvframe->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len - prxattrib->icv_len - 8; /* icv_len included the mic code */ + pframe = precvframe->u.hdr.rx_data; + payload = pframe + prxattrib->hdrlen + prxattrib->iv_len; + + + /* rtw_seccalctkipmic(&stainfo->dot11tkiprxmickey.skey[0],pframe,payload, datalen ,&miccode[0],(unsigned char)prxattrib->priority); */ /* care the length of the data */ + + rtw_seccalctkipmic(mickey, pframe, payload, datalen , &miccode[0], (unsigned char)prxattrib->priority); /* care the length of the data */ + + pframemic = payload + datalen; + + bmic_err = _FALSE; + + for (i = 0; i < 8; i++) { + if (miccode[i] != *(pframemic + i)) { + bmic_err = _TRUE; + } + } + + + if (bmic_err == _TRUE) { + + + + /* double check key_index for some timing issue , */ + /* cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */ + if ((IS_MCAST(prxattrib->ra) == _TRUE) && (prxattrib->key_index != pmlmeinfo->key_index)) + brpt_micerror = _FALSE; + + if ((prxattrib->bdecrypted == _TRUE) && (brpt_micerror == _TRUE)) { + rtw_handle_tkip_mic_err(adapter, stainfo, (u8)IS_MCAST(prxattrib->ra)); + RTW_INFO(" mic error :prxattrib->bdecrypted=%d\n", prxattrib->bdecrypted); + } else { + RTW_INFO(" mic error :prxattrib->bdecrypted=%d\n", prxattrib->bdecrypted); + } + + res = _FAIL; + + } else { + /* mic checked ok */ + if ((psecuritypriv->bcheck_grpkey == _FALSE) && (IS_MCAST(prxattrib->ra) == _TRUE)) { + psecuritypriv->bcheck_grpkey = _TRUE; + } + } + + } + + recvframe_pull_tail(precvframe, 8); + + } + +exit: + + + return res; + +} + +/*#define DBG_RX_SW_DECRYPTOR*/ + +/* decrypt and set the ivlen,icvlen of the recv_frame */ +union recv_frame *decryptor(_adapter *padapter, union recv_frame *precv_frame); +union recv_frame *decryptor(_adapter *padapter, union recv_frame *precv_frame) +{ + + struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + union recv_frame *return_packet = precv_frame; + u32 res = _SUCCESS; + + + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt); + + + if (prxattrib->encrypt > 0) { + u8 *iv = precv_frame->u.hdr.rx_data + prxattrib->hdrlen; + prxattrib->key_index = (((iv[3]) >> 6) & 0x3) ; + + if (prxattrib->key_index > WEP_KEYS) { + RTW_INFO("prxattrib->key_index(%d) > WEP_KEYS\n", prxattrib->key_index); + + switch (prxattrib->encrypt) { + case _WEP40_: + case _WEP104_: + prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex; + break; + case _TKIP_: + case _AES_: + case _GCMP_: + case _GCMP_256_: + case _CCMP_256_: + default: + prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid; + break; + } + } + } + + if (prxattrib->encrypt && !prxattrib->bdecrypted) { + if (GetFrameType(get_recvframe_data(precv_frame)) == WIFI_DATA + #ifdef CONFIG_CONCURRENT_MODE + && !IS_MCAST(prxattrib->ra) /* bc/mc packets may use sw decryption for concurrent mode */ + #endif + ) + psecuritypriv->hw_decrypted = _FALSE; + +#ifdef DBG_RX_SW_DECRYPTOR + RTW_INFO(ADPT_FMT" - sec_type:%s DO SW decryption\n", + ADPT_ARG(padapter), security_type_str(prxattrib->encrypt)); +#endif + +#ifdef DBG_RX_DECRYPTOR + RTW_INFO("[%s] %d:prxstat->bdecrypted:%d, prxattrib->encrypt:%d, Setting psecuritypriv->hw_decrypted = %d\n", + __FUNCTION__, + __LINE__, + prxattrib->bdecrypted, + prxattrib->encrypt, + psecuritypriv->hw_decrypted); +#endif + + switch (prxattrib->encrypt) { + case _WEP40_: + case _WEP104_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_wep); + rtw_wep_decrypt(padapter, (u8 *)precv_frame); + break; + case _TKIP_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_tkip); + res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame); + break; + case _AES_: + case _CCMP_256_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_aes); + res = rtw_aes_decrypt(padapter, (u8 *)precv_frame); + break; + case _GCMP_: + case _GCMP_256_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_gcmp); + res = rtw_gcmp_decrypt(padapter, (u8 *)precv_frame); + break; +#ifdef CONFIG_WAPI_SUPPORT + case _SMS4_: + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_wapi); + rtw_sms4_decrypt(padapter, (u8 *)precv_frame); + break; +#endif + default: + break; + } + } else if (prxattrib->bdecrypted == 1 + && prxattrib->encrypt > 0 + && (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_) + ) { +#if 0 + if ((prxstat->icv == 1) && (prxattrib->encrypt != _AES_)) { + psecuritypriv->hw_decrypted = _FALSE; + + + rtw_free_recvframe(precv_frame, &padapter->recvpriv.free_recv_queue); + + return_packet = NULL; + + } else +#endif + { + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_hw); + + psecuritypriv->hw_decrypted = _TRUE; +#ifdef DBG_RX_DECRYPTOR + RTW_INFO("[%s] %d:prxstat->bdecrypted:%d, prxattrib->encrypt:%d, Setting psecuritypriv->hw_decrypted = %d\n", + __FUNCTION__, + __LINE__, + prxattrib->bdecrypted, + prxattrib->encrypt, + psecuritypriv->hw_decrypted); + +#endif + } + } else { + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_unknown); +#ifdef DBG_RX_DECRYPTOR + RTW_INFO("[%s] %d:prxstat->bdecrypted:%d, prxattrib->encrypt:%d, Setting psecuritypriv->hw_decrypted = %d\n", + __FUNCTION__, + __LINE__, + prxattrib->bdecrypted, + prxattrib->encrypt, + psecuritypriv->hw_decrypted); +#endif + } + + #ifdef CONFIG_RTW_MESH + if (res != _FAIL + && !prxattrib->amsdu + && prxattrib->mesh_ctrl_present) + res = rtw_mesh_rx_validate_mctrl_non_amsdu(padapter, precv_frame); + #endif + + if (res == _FAIL) { + rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue); + return_packet = NULL; + } else + prxattrib->bdecrypted = _TRUE; + /* recvframe_chkmic(adapter, precv_frame); */ /* move to recvframme_defrag function */ + + + return return_packet; + +} +/* ###set the security information in the recv_frame */ +union recv_frame *portctrl(_adapter *adapter, union recv_frame *precv_frame); +union recv_frame *portctrl(_adapter *adapter, union recv_frame *precv_frame) +{ + u8 *psta_addr = NULL; + u8 *ptr; + uint auth_alg; + struct recv_frame_hdr *pfhdr; + struct sta_info *psta; + struct sta_priv *pstapriv ; + union recv_frame *prtnframe; + u16 ether_type = 0; + u16 eapol_type = 0x888e;/* for Funia BD's WPA issue */ + struct rx_pkt_attrib *pattrib; + + + pstapriv = &adapter->stapriv; + + auth_alg = adapter->securitypriv.dot11AuthAlgrthm; + + ptr = get_recvframe_data(precv_frame); + pfhdr = &precv_frame->u.hdr; + pattrib = &pfhdr->attrib; + psta_addr = pattrib->ta; + + prtnframe = NULL; + + psta = rtw_get_stainfo(pstapriv, psta_addr); + + + if (auth_alg == dot11AuthAlgrthm_8021X) { + if ((psta != NULL) && (psta->ieee8021x_blocked)) { + /* blocked */ + /* only accept EAPOL frame */ + + prtnframe = precv_frame; + + /* get ether_type */ + ptr = ptr + pfhdr->attrib.hdrlen + pfhdr->attrib.iv_len + LLC_HEADER_SIZE; + _rtw_memcpy(ðer_type, ptr, 2); + ether_type = ntohs((unsigned short)ether_type); + + if (ether_type == eapol_type) + prtnframe = precv_frame; + else { + /* free this frame */ + rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue); + prtnframe = NULL; + } + } else { + /* allowed */ + /* check decryption status, and decrypt the frame if needed */ + + + prtnframe = precv_frame; + /* check is the EAPOL frame or not (Rekey) */ + /* if(ether_type == eapol_type){ */ + /* check Rekey */ + + /* prtnframe=precv_frame; */ + /* } */ + } + } else + prtnframe = precv_frame; + + + return prtnframe; + +} + +/* VALID_PN_CHK + * Return true when PN is legal, otherwise false. + * Legal PN: + * 1. If old PN is 0, any PN is legal + * 2. PN > old PN + */ +#define PN_LESS_CHK(a, b) (((a-b) & 0x800000000000) != 0) +#define VALID_PN_CHK(new, old) (((old) == 0) || PN_LESS_CHK(old, new)) +#define CCMPH_2_KEYID(ch) (((ch) & 0x00000000c0000000) >> 30) +sint recv_ucast_pn_decache(union recv_frame *precv_frame); +sint recv_ucast_pn_decache(union recv_frame *precv_frame) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_info *sta = precv_frame->u.hdr.psta; + struct stainfo_rxcache *prxcache = &sta->sta_recvpriv.rxcache; + u8 *pdata = precv_frame->u.hdr.rx_data; + sint tid = precv_frame->u.hdr.attrib.priority; + u64 tmp_iv_hdr = 0; + u64 curr_pn = 0, pkt_pn = 0; + + if (tid > 15) + return _FAIL; + + if (pattrib->encrypt == _AES_) { + tmp_iv_hdr = le64_to_cpu(*(u64*)(pdata + pattrib->hdrlen)); + pkt_pn = CCMPH_2_PN(tmp_iv_hdr); + tmp_iv_hdr = le64_to_cpu(*(u64*)prxcache->iv[tid]); + curr_pn = CCMPH_2_PN(tmp_iv_hdr); + + if (!VALID_PN_CHK(pkt_pn, curr_pn)) { + /* return _FAIL; */ + } else { + prxcache->last_tid = tid; + _rtw_memcpy(prxcache->iv[tid], + (pdata + pattrib->hdrlen), + sizeof(prxcache->iv[tid])); + } + } + + return _SUCCESS; +} + +sint recv_bcast_pn_decache(union recv_frame *precv_frame); +sint recv_bcast_pn_decache(union recv_frame *precv_frame) +{ + _adapter *padapter = precv_frame->u.hdr.adapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + u8 *pdata = precv_frame->u.hdr.rx_data; + u64 tmp_iv_hdr = 0; + u64 curr_pn = 0, pkt_pn = 0; + u8 key_id; + + if ((pattrib->encrypt == _AES_) && + (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE)) { + + tmp_iv_hdr = le64_to_cpu(*(u64*)(pdata + pattrib->hdrlen)); + key_id = CCMPH_2_KEYID(tmp_iv_hdr); + pkt_pn = CCMPH_2_PN(tmp_iv_hdr); + + curr_pn = le64_to_cpu(*(u64*)psecuritypriv->iv_seq[key_id]); + curr_pn &= 0x0000ffffffffffff; + + if (!VALID_PN_CHK(pkt_pn, curr_pn)) + return _FAIL; + + *(u64*)psecuritypriv->iv_seq[key_id] = cpu_to_le64(pkt_pn); + } + + return _SUCCESS; +} + +sint recv_decache(union recv_frame *precv_frame) +{ + struct sta_info *psta = precv_frame->u.hdr.psta; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + _adapter *adapter = psta->padapter; + sint tid = pattrib->priority; + u16 seq_ctrl = ((precv_frame->u.hdr.attrib.seq_num & 0xffff) << 4) | + (precv_frame->u.hdr.attrib.frag_num & 0xf); + u16 *prxseq; + + if (tid > 15) + return _FAIL; + + if (pattrib->qos) { + if (IS_MCAST(pattrib->ra)) + prxseq = &psta->sta_recvpriv.bmc_tid_rxseq[tid]; + else + prxseq = &psta->sta_recvpriv.rxcache.tid_rxseq[tid]; + } else { + if (IS_MCAST(pattrib->ra)) { + prxseq = &psta->sta_recvpriv.nonqos_bmc_rxseq; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" nonqos bmc seq_num:%d\n" + , FUNC_ADPT_ARG(adapter), pattrib->seq_num); + #endif + + } else { + prxseq = &psta->sta_recvpriv.nonqos_rxseq; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" nonqos seq_num:%d\n" + , FUNC_ADPT_ARG(adapter), pattrib->seq_num); + #endif + } + } + + if (seq_ctrl == *prxseq) { + /* for non-AMPDU case */ + psta->sta_stats.duplicate_cnt++; + + if (psta->sta_stats.duplicate_cnt % 100 == 0) + RTW_INFO("%s: tid=%u seq=%d frag=%d\n", __func__ + , tid, precv_frame->u.hdr.attrib.seq_num + , precv_frame->u.hdr.attrib.frag_num); + + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" recv_decache _FAIL for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + return _FAIL; + } + *prxseq = seq_ctrl; + + return _SUCCESS; +} + +void process_pwrbit_data(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *psta) +{ +#ifdef CONFIG_AP_MODE + unsigned char pwrbit; + u8 *ptr = precv_frame->u.hdr.rx_data; + + pwrbit = GetPwrMgt(ptr); + + if (pwrbit) { + if (!(psta->state & WIFI_SLEEP_STATE)) { + /* psta->state |= WIFI_SLEEP_STATE; */ + /* rtw_tim_map_set(padapter, pstapriv->sta_dz_bitmap, BIT(psta->cmn.aid)); */ + + stop_sta_xmit(padapter, psta); + /* RTW_INFO_DUMP("to sleep, sta_dz_bitmap=", pstapriv->sta_dz_bitmap, pstapriv->aid_bmp_len); */ + } + } else { + if (psta->state & WIFI_SLEEP_STATE) { + /* psta->state ^= WIFI_SLEEP_STATE; */ + /* rtw_tim_map_clear(padapter, pstapriv->sta_dz_bitmap, BIT(psta->cmn.aid)); */ + + wakeup_sta_to_xmit(padapter, psta); + /* RTW_INFO_DUMP("to wakeup, sta_dz_bitmap=", pstapriv->sta_dz_bitmap, pstapriv->aid_bmp_len); */ + } + } +#endif +} + +void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *psta) +{ +#ifdef CONFIG_AP_MODE + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + +#ifdef CONFIG_TDLS + if (!(psta->tdls_sta_state & TDLS_LINKED_STATE)) { +#endif /* CONFIG_TDLS */ + + if (!psta->qos_option) + return; + + if (!(psta->qos_info & 0xf)) + return; + +#ifdef CONFIG_TDLS + } +#endif /* CONFIG_TDLS */ + + if (psta->state & WIFI_SLEEP_STATE) { + u8 wmmps_ac = 0; + + switch (pattrib->priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk & BIT(1); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi & BIT(1); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo & BIT(1); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be & BIT(1); + break; + } + + if (wmmps_ac) { + if (psta->sleepq_ac_len > 0) { + /* process received triggered frame */ + xmit_delivery_enabled_frames(padapter, psta); + } else { + /* issue one qos null frame with More data bit = 0 and the EOSP bit set (=1) */ + issue_qos_nulldata(padapter, psta->cmn.mac_addr, (u16)pattrib->priority, 0, 0, 0); + } + } + + } + + +#endif + +} + +#ifdef CONFIG_TDLS +sint OnTDLS(_adapter *adapter, union recv_frame *precv_frame) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + sint ret = _SUCCESS; + u8 *paction = get_recvframe_data(precv_frame); + u8 category_field = 1; +#ifdef CONFIG_WFD + u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a }; +#endif /* CONFIG_WFD */ + struct tdls_info *ptdlsinfo = &(adapter->tdlsinfo); + u8 *ptr = precv_frame->u.hdr.rx_data; + struct sta_priv *pstapriv = &(adapter->stapriv); + struct sta_info *ptdls_sta = NULL; + + /* point to action field */ + paction += pattrib->hdrlen + + pattrib->iv_len + + SNAP_SIZE + + ETH_TYPE_LEN + + PAYLOAD_TYPE_LEN + + category_field; + + RTW_INFO("[TDLS] Recv %s from "MAC_FMT" with SeqNum = %d\n", rtw_tdls_action_txt(*paction), MAC_ARG(pattrib->src), GetSequence(get_recvframe_data(precv_frame))); + + if (hal_chk_wl_func(adapter, WL_FUNC_TDLS) == _FALSE) { + RTW_INFO("Ignore tdls frame since hal doesn't support tdls\n"); + ret = _FAIL; + return ret; + } + + if (rtw_is_tdls_enabled(adapter) == _FALSE) { + RTW_INFO("recv tdls frame, " + "but tdls haven't enabled\n"); + ret = _FAIL; + return ret; + } + + ptdls_sta = rtw_get_stainfo(pstapriv, get_sa(ptr)); + if (ptdls_sta == NULL) { + switch (*paction) { + case TDLS_SETUP_REQUEST: + case TDLS_DISCOVERY_REQUEST: + break; + default: + RTW_INFO("[TDLS] %s - Direct Link Peer = "MAC_FMT" not found for action = %d\n", __func__, MAC_ARG(get_sa(ptr)), *paction); + ret = _FAIL; + goto exit; + } + } + + switch (*paction) { + case TDLS_SETUP_REQUEST: + ret = On_TDLS_Setup_Req(adapter, precv_frame, ptdls_sta); + break; + case TDLS_SETUP_RESPONSE: + ret = On_TDLS_Setup_Rsp(adapter, precv_frame, ptdls_sta); + break; + case TDLS_SETUP_CONFIRM: + ret = On_TDLS_Setup_Cfm(adapter, precv_frame, ptdls_sta); + break; + case TDLS_TEARDOWN: + ret = On_TDLS_Teardown(adapter, precv_frame, ptdls_sta); + break; + case TDLS_DISCOVERY_REQUEST: + ret = On_TDLS_Dis_Req(adapter, precv_frame); + break; + case TDLS_PEER_TRAFFIC_INDICATION: + ret = On_TDLS_Peer_Traffic_Indication(adapter, precv_frame, ptdls_sta); + break; + case TDLS_PEER_TRAFFIC_RESPONSE: + ret = On_TDLS_Peer_Traffic_Rsp(adapter, precv_frame, ptdls_sta); + break; +#ifdef CONFIG_TDLS_CH_SW + case TDLS_CHANNEL_SWITCH_REQUEST: + ret = On_TDLS_Ch_Switch_Req(adapter, precv_frame, ptdls_sta); + break; + case TDLS_CHANNEL_SWITCH_RESPONSE: + ret = On_TDLS_Ch_Switch_Rsp(adapter, precv_frame, ptdls_sta); + break; +#endif +#ifdef CONFIG_WFD + /* First byte of WFA OUI */ + case 0x50: + if (_rtw_memcmp(WFA_OUI, paction, 3)) { + /* Probe request frame */ + if (*(paction + 3) == 0x04) { + /* WFDTDLS: for sigma test, do not setup direct link automatically */ + ptdlsinfo->dev_discovered = _TRUE; + RTW_INFO("recv tunneled probe request frame\n"); + issue_tunneled_probe_rsp(adapter, precv_frame); + } + /* Probe response frame */ + if (*(paction + 3) == 0x05) { + /* WFDTDLS: for sigma test, do not setup direct link automatically */ + ptdlsinfo->dev_discovered = _TRUE; + RTW_INFO("recv tunneled probe response frame\n"); + } + } + break; +#endif /* CONFIG_WFD */ + default: + RTW_INFO("receive TDLS frame %d but not support\n", *paction); + ret = _FAIL; + break; + } + +exit: + return ret; + +} + +sint rtw_tdls_rx_data_validate_hdr( + _adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta +) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + sint ret = _SUCCESS; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u8 *mybssid = get_bssid(pmlmepriv); + u8 *myhwaddr = adapter_mac_addr(adapter); + u8 *sta_addr = pattrib->ta; + sint bmcast = IS_MCAST(pattrib->dst); + + struct tdls_info *ptdlsinfo = &adapter->tdlsinfo; +#ifdef CONFIG_TDLS_CH_SW + struct tdls_ch_switch *pchsw_info = &ptdlsinfo->chsw_info; +#endif + struct sta_info *ptdls_sta = NULL; + u8 *psnap_type = ptr + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE; + /* frame body located after [+2]: ether-type, [+1]: payload type */ + u8 *pframe_body = psnap_type + 2 + 1; + + *psta = ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->ta); + if (ptdls_sta == NULL) { + ret = _FAIL; + goto exit; + } else if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) { + /* filter packets that SA is myself or multicast or broadcast */ + if (_rtw_memcmp(myhwaddr, pattrib->src, ETH_ALEN)) { + ret = _FAIL; + goto exit; + } + /* da should be for me */ + if ((!_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) { + ret = _FAIL; + goto exit; + } + /* check BSSID */ + if (_rtw_memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + _rtw_memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + (!_rtw_memcmp(pattrib->bssid, mybssid, ETH_ALEN))) { + ret = _FAIL; + goto exit; + } + +#ifdef CONFIG_TDLS_CH_SW + if (ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) { + if (adapter->mlmeextpriv.cur_channel != rtw_get_oper_ch(adapter)) { + pchsw_info->ch_sw_state |= TDLS_PEER_AT_OFF_STATE; + if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE)) + _cancel_timer_ex(&ptdls_sta->ch_sw_timer); + /* On_TDLS_Peer_Traffic_Rsp(adapter, precv_frame); */ + } + } +#endif + + /* process UAPSD tdls sta */ + process_pwrbit_data(adapter, precv_frame, ptdls_sta); + + /* if NULL-frame, check pwrbit */ + if ((get_frame_sub_type(ptr) & WIFI_DATA_NULL) == WIFI_DATA_NULL) { + /* NULL-frame with pwrbit=1, buffer_STA should buffer frames for sleep_STA */ + if (GetPwrMgt(ptr)) { + /* it would be triggered when we are off channel and receiving NULL DATA */ + /* we can confirm that peer STA is at off channel */ + RTW_INFO("TDLS: recv peer null frame with pwr bit 1\n"); + /* ptdls_sta->tdls_sta_state|=TDLS_PEER_SLEEP_STATE; */ + } + + /* TODO: Updated BSSID's seq. */ + /* RTW_INFO("drop Null Data\n"); */ + ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE); + ret = _FAIL; + goto exit; + } + + /* receive some of all TDLS management frames, process it at ON_TDLS */ + if (_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_TDLS, 2)) { + ret = OnTDLS(adapter, precv_frame); + goto exit; + } + + if ((get_frame_sub_type(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) + process_wmmps_data(adapter, precv_frame, ptdls_sta); + + ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE); + + } + +exit: + return ret; +} +#endif /* CONFIG_TDLS */ + +void count_rx_stats(_adapter *padapter, union recv_frame *prframe, struct sta_info *sta) +{ + int sz; + struct sta_info *psta = NULL; + struct stainfo_stats *pstats = NULL; + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + struct recv_priv *precvpriv = &padapter->recvpriv; + + sz = get_recvframe_len(prframe); + precvpriv->rx_bytes += sz; + + padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++; + + if ((!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst))) + padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++; + + if (sta) + psta = sta; + else + psta = prframe->u.hdr.psta; + + if (psta) { + u8 is_ra_bmc = IS_MCAST(pattrib->ra); + + pstats = &psta->sta_stats; + + pstats->last_rx_time = rtw_get_current_time(); + pstats->rx_data_pkts++; + pstats->rx_bytes += sz; + if (is_broadcast_mac_addr(pattrib->ra)) { + pstats->rx_data_bc_pkts++; + pstats->rx_bc_bytes += sz; + } else if (is_ra_bmc) { + pstats->rx_data_mc_pkts++; + pstats->rx_mc_bytes += sz; + } + + if (!is_ra_bmc) { + pstats->rxratecnt[pattrib->data_rate]++; + /*record rx packets for every tid*/ + pstats->rx_data_qos_pkts[pattrib->priority]++; + } +#ifdef CONFIG_DYNAMIC_SOML + rtw_dyn_soml_byte_update(padapter, pattrib->data_rate, sz); +#endif +#if defined(CONFIG_CHECK_LEAVE_LPS) && defined(CONFIG_LPS_CHK_BY_TP) + if (adapter_to_pwrctl(padapter)->lps_chk_by_tp) + traffic_check_for_leave_lps_by_tp(padapter, _FALSE, psta); +#endif /* CONFIG_LPS */ + + } + +#ifdef CONFIG_CHECK_LEAVE_LPS +#ifdef CONFIG_LPS_CHK_BY_TP + if (!adapter_to_pwrctl(padapter)->lps_chk_by_tp) +#endif + traffic_check_for_leave_lps(padapter, _FALSE, 0); +#endif /* CONFIG_CHECK_LEAVE_LPS */ + +} + +int rtw_sta_rx_data_validate_hdr(_adapter *adapter, union recv_frame *rframe, struct sta_info **sta) +{ + struct sta_priv *stapriv = &adapter->stapriv; + u8 *mybssid = get_bssid(&adapter->mlmepriv); + u8 *myhwaddr = adapter_mac_addr(adapter); + struct rx_pkt_attrib *rattrib = &rframe->u.hdr.attrib; + u8 *whdr = get_recvframe_data(rframe); + u8 is_ra_bmc = IS_MCAST(GetAddr1Ptr(whdr)) ? 1 : 0; + sint ret = _FAIL; + + if (rattrib->to_fr_ds == 0) { + _rtw_memcpy(rattrib->ra, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->ta, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->dst, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->src, get_addr2_ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->bssid, GetAddr3Ptr(whdr), ETH_ALEN); + + #ifdef CONFIG_TDLS + if (adapter->tdlsinfo.link_established == _TRUE) + ret = rtw_tdls_rx_data_validate_hdr(adapter, rframe, sta); + else + #endif + { + /* For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */ + if (!_rtw_memcmp(rattrib->bssid, rattrib->src, ETH_ALEN)) + goto exit; + + *sta = rtw_get_stainfo(stapriv, get_addr2_ptr(whdr)); + if (*sta) + ret = _SUCCESS; + } + goto exit; + } + + if (!(MLME_STATE(adapter) & (WIFI_ASOC_STATE | WIFI_UNDER_LINKING))) { + if (!is_ra_bmc) { + /* for AP multicast issue , modify by yiwei */ + static systime send_issue_deauth_time = 0; + + /* RTW_INFO("After send deauth , %u ms has elapsed.\n", rtw_get_passing_time_ms(send_issue_deauth_time)); */ + if (rtw_get_passing_time_ms(send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) { + send_issue_deauth_time = rtw_get_current_time(); + RTW_INFO(FUNC_ADPT_FMT" issue_deauth to "MAC_FMT" with reason(7), mlme_state:0x%x\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(get_addr2_ptr(whdr)), MLME_STATE(adapter)); + issue_deauth(adapter, get_addr2_ptr(whdr), WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + } + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" fw_state:0x%x\n" + , FUNC_ADPT_ARG(adapter), MLME_STATE(adapter)); + #endif + goto exit; + } + + _rtw_memcpy(rattrib->ra, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->ta, get_addr2_ptr(whdr), ETH_ALEN); + + switch (rattrib->to_fr_ds) { + case 2: + _rtw_memcpy(rattrib->dst, GetAddr1Ptr(whdr), ETH_ALEN); + _rtw_memcpy(rattrib->src, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->bssid, get_addr2_ptr(whdr), ETH_ALEN); + break; + case 3: + _rtw_memcpy(rattrib->dst, GetAddr3Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->src, GetAddr4Ptr(whdr), ETH_ALEN); /* may change after checking AMSDU subframe header */ + _rtw_memcpy(rattrib->bssid, get_addr2_ptr(whdr), ETH_ALEN); + break; + default: + ret = RTW_RX_HANDLED; /* don't count for drop */ + goto exit; + } + + /* filter packets that SA is myself */ + if (!rattrib->amsdu && _rtw_memcmp(myhwaddr, rattrib->src, ETH_ALEN)) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" SA="MAC_FMT", myhwaddr="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(rattrib->src), MAC_ARG(myhwaddr)); + #endif + goto exit; + } + + *sta = rtw_get_stainfo(stapriv, rattrib->ta); + if (*sta == NULL) { + #ifndef CONFIG_CUSTOMER_ALIBABA_GENERAL + if (!is_ra_bmc && !IS_RADAR_DETECTED(adapter_to_rfctl(adapter))) { + RTW_INFO(FUNC_ADPT_FMT" issue_deauth to "MAC_FMT" with reason(7), unknown TA\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(rattrib->ta)); + issue_deauth(adapter, rattrib->ta, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + #endif + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" can't get psta under STATION_MODE ; drop pkt\n" + , FUNC_ADPT_ARG(adapter)); + #endif + goto exit; + } + +#ifdef CONFIG_RTW_WDS_AUTO_EN + if (rattrib->to_fr_ds == 3 && !(sta->flags & WLAN_STA_WDS)) + sta->flags |= WLAN_STA_WDS; +#endif + + /*if ((get_frame_sub_type(whdr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) { + } + */ + + if (get_frame_sub_type(whdr) & BIT(6)) { + /* No data, will not indicate to upper layer, temporily count it here */ + count_rx_stats(adapter, rframe, *sta); + ret = RTW_RX_HANDLED; + goto exit; + } + +#ifdef CONFIG_RTW_WDS + if (adapter_use_wds(adapter) + && !rattrib->amsdu && IS_MCAST(rattrib->dst) + && rtw_rx_wds_gptr_check(adapter, rattrib->src) + ) { + /* will not indicate to upper layer, temporily count it here */ + count_rx_stats(adapter, rframe, *sta); + ret = RTW_RX_HANDLED; + goto exit; + } +#endif + + ret = _SUCCESS; + +exit: + return ret; +} + +int rtw_sta_rx_amsdu_act_check(union recv_frame *rframe + , const u8 *da, const u8 *sa) +{ + int act = RTW_RX_MSDU_ACT_INDICATE; + +#ifdef CONFIG_RTW_WDS + _adapter *adapter = rframe->u.hdr.adapter; + + if (adapter_use_wds(adapter) + && IS_MCAST(da) + && rtw_rx_wds_gptr_check(adapter, sa) + ) { + act = 0; + } +#endif + + return act; +} + +sint sta2sta_data_frame( + _adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta +) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + sint ret = _SUCCESS; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u8 *mybssid = get_bssid(pmlmepriv); + u8 *myhwaddr = adapter_mac_addr(adapter); + u8 *sta_addr = pattrib->ta; + sint bmcast = IS_MCAST(pattrib->dst); + + /* RTW_INFO("[%s] %d, seqnum:%d\n", __FUNCTION__, __LINE__, pattrib->seq_num); */ + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) { + + /* filter packets that SA is myself or multicast or broadcast */ + if (_rtw_memcmp(myhwaddr, pattrib->src, ETH_ALEN)) { + ret = _FAIL; + goto exit; + } + + if ((!_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) { + ret = _FAIL; + goto exit; + } + + if (_rtw_memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + _rtw_memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) || + (!_rtw_memcmp(pattrib->bssid, mybssid, ETH_ALEN))) { + ret = _FAIL; + goto exit; + } + + } else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) { + _rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + sta_addr = mybssid; + } else + ret = _FAIL; + + *psta = rtw_get_stainfo(pstapriv, sta_addr); + if (*psta == NULL) { +#ifdef CONFIG_MP_INCLUDED + if (adapter->registrypriv.mp_mode == 1) { + if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) + adapter->mppriv.rx_pktloss++; + } +#endif + ret = _FAIL; + goto exit; + } + +exit: + return ret; +} + +sint ap2sta_data_frame( + _adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + sint ret = _SUCCESS; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + u8 *myhwaddr = adapter_mac_addr(adapter); + sint bmcast = IS_MCAST(pattrib->dst); + + if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) && + (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE)) { + _rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + + *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */ + if (*psta == NULL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" can't get psta under WIFI_MP_STATE ; drop pkt\n" + , FUNC_ADPT_ARG(adapter)); + #endif + ret = _FAIL; + goto exit; + } + + } else { + if (_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN) && (!bmcast)) { + *psta = rtw_get_stainfo(pstapriv, pattrib->ta); + if (*psta == NULL) { + + /* for AP multicast issue , modify by yiwei */ + static systime send_issue_deauth_time = 0; + + /* RTW_INFO("After send deauth , %u ms has elapsed.\n", rtw_get_passing_time_ms(send_issue_deauth_time)); */ + + if (rtw_get_passing_time_ms(send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) { + send_issue_deauth_time = rtw_get_current_time(); + + RTW_INFO("issue_deauth to the ap=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid)); + + issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); + } + } + } + + ret = _FAIL; + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" fw_state:0x%x\n" + , FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv)); + #endif + } + +exit: + return ret; + +} + +sint sta2ap_data_frame( + _adapter *adapter, + union recv_frame *precv_frame, + struct sta_info **psta) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &adapter->stapriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + unsigned char *mybssid = get_bssid(pmlmepriv); + sint ret = _SUCCESS; + + if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) && + (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE)) { + /* RTW_INFO("%s ,in WIFI_MP_STATE\n",__func__); */ + _rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + + *psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */ + if (*psta == NULL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" can't get psta under WIFI_MP_STATE ; drop pkt\n" + , FUNC_ADPT_ARG(adapter)); + #endif + ret = _FAIL; + goto exit; + } + + } else { + u8 *myhwaddr = adapter_mac_addr(adapter); + if (!_rtw_memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) { + ret = RTW_RX_HANDLED; + goto exit; + } +#ifndef CONFIG_CUSTOMER_ALIBABA_GENERAL + RTW_INFO("issue_deauth to sta=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src)); + issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA); +#endif + ret = RTW_RX_HANDLED; + goto exit; + } + +exit: + + + return ret; + +} + +sint validate_recv_ctrl_frame(_adapter *padapter, union recv_frame *precv_frame); +sint validate_recv_ctrl_frame(_adapter *padapter, union recv_frame *precv_frame) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_info *psta = NULL; + /* uint len = precv_frame->u.hdr.len; */ + + /* RTW_INFO("+validate_recv_ctrl_frame\n"); */ + + if (GetFrameType(pframe) != WIFI_CTRL_TYPE) + return _FAIL; + + /* receive the frames that ra(a1) is my address */ + if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN)) + return _FAIL; + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta == NULL) + return _FAIL; + + /* for rx pkt statistics */ + psta->sta_stats.last_rx_time = rtw_get_current_time(); + psta->sta_stats.rx_ctrl_pkts++; + + /* only handle ps-poll */ + if (get_frame_sub_type(pframe) == WIFI_PSPOLL) { +#ifdef CONFIG_AP_MODE + u16 aid; + u8 wmmps_ac = 0; + + aid = GetAid(pframe); + if (psta->cmn.aid != aid) + return _FAIL; + + switch (pattrib->priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk & BIT(0); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi & BIT(0); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo & BIT(0); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be & BIT(0); + break; + } + + if (wmmps_ac) + return _FAIL; + + #ifdef CONFIG_AP_MODE + if (psta->state & WIFI_STA_ALIVE_CHK_STATE) { + RTW_INFO("%s alive check-rx ps-poll\n", __func__); + psta->expire_to = pstapriv->expire_to; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + } + #endif + + if ((psta->state & WIFI_SLEEP_STATE) && (rtw_tim_map_is_set(padapter, pstapriv->sta_dz_bitmap, psta->cmn.aid))) { + _irqL irqL; + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + /* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */ + _enter_critical_bh(&pxmitpriv->lock, &irqL); + + xmitframe_phead = get_list_head(&psta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + if ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + rtw_list_delete(&pxmitframe->list); + + psta->sleepq_len--; + + if (psta->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + pxmitframe->attrib.triggered = 1; + + /* RTW_INFO("handling ps-poll, q_len=%d\n", psta->sleepq_len); */ + /* RTW_INFO_DUMP("handling, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + +#if 0 + _exit_critical_bh(&psta->sleep_q.lock, &irqL); + if (rtw_hal_xmit(padapter, pxmitframe) == _TRUE) + rtw_os_xmit_complete(padapter, pxmitframe); + _enter_critical_bh(&psta->sleep_q.lock, &irqL); +#endif + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + if (psta->sleepq_len == 0) { + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + /* RTW_INFO("after handling ps-poll\n"); */ + /* RTW_INFO_DUMP("after handling, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0); + } + + /* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */ + _exit_critical_bh(&pxmitpriv->lock, &irqL); + + } else { + /* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */ + _exit_critical_bh(&pxmitpriv->lock, &irqL); + + /* RTW_INFO("no buffered packets to xmit\n"); */ + if (rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid)) { + if (psta->sleepq_len == 0) { + RTW_INFO("no buffered packets to xmit\n"); + + /* issue nulldata with More data bit = 0 to indicate we have no buffered packets */ + issue_nulldata(padapter, psta->cmn.mac_addr, 0, 0, 0); + } else { + RTW_INFO("error!psta->sleepq_len=%d\n", psta->sleepq_len); + psta->sleepq_len = 0; + } + + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0); + } + } + } +#endif /* CONFIG_AP_MODE */ + } else if (get_frame_sub_type(pframe) == WIFI_NDPA) { +#ifdef CONFIG_BEAMFORMING + rtw_beamforming_get_ndpa_frame(padapter, precv_frame); +#endif/*CONFIG_BEAMFORMING*/ + } else if (get_frame_sub_type(pframe) == WIFI_BAR) { + rtw_process_bar_frame(padapter, precv_frame); + } + + return _FAIL; + +} + +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) +static sint validate_mgmt_protect(_adapter *adapter, union recv_frame *precv_frame) +{ +#define DBG_VALIDATE_MGMT_PROTECT 0 +#define DBG_VALIDATE_MGMT_DEC 0 + + struct security_priv *sec = &adapter->securitypriv; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_info *psta = precv_frame->u.hdr.psta; + u8 *ptr; + u8 type; + u8 subtype; + u8 is_bmc; + u8 category = 0xFF; + +#ifdef CONFIG_IEEE80211W + const u8 *igtk; + u16 igtk_id; + u64* ipn; + enum security_type bip_cipher; +#endif + + u8 *mgmt_DATA; + u32 data_len = 0; + + sint ret; + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + if (!adapter->mesh_info.mesh_auth_id) + return pattrib->privacy ? _FAIL : _SUCCESS; + } else +#endif + if (SEC_IS_BIP_KEY_INSTALLED(sec) == _FALSE) + return _SUCCESS; + + ptr = precv_frame->u.hdr.rx_data; + type = GetFrameType(ptr); + subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2) */ + is_bmc = IS_MCAST(GetAddr1Ptr(ptr)); + +#if DBG_VALIDATE_MGMT_PROTECT + if (subtype == WIFI_DEAUTH) { + RTW_INFO(FUNC_ADPT_FMT" bmc:%u, deauth, privacy:%u, encrypt:%u, bdecrypted:%u\n" + , FUNC_ADPT_ARG(adapter) + , is_bmc, pattrib->privacy, pattrib->encrypt, pattrib->bdecrypted); + } else if (subtype == WIFI_DISASSOC) { + RTW_INFO(FUNC_ADPT_FMT" bmc:%u, disassoc, privacy:%u, encrypt:%u, bdecrypted:%u\n" + , FUNC_ADPT_ARG(adapter) + , is_bmc, pattrib->privacy, pattrib->encrypt, pattrib->bdecrypted); + } if (subtype == WIFI_ACTION) { + if (pattrib->privacy) { + RTW_INFO(FUNC_ADPT_FMT" bmc:%u, action(?), privacy:%u, encrypt:%u, bdecrypted:%u\n" + , FUNC_ADPT_ARG(adapter) + , is_bmc, pattrib->privacy, pattrib->encrypt, pattrib->bdecrypted); + } else { + RTW_INFO(FUNC_ADPT_FMT" bmc:%u, action(%u), privacy:%u, encrypt:%u, bdecrypted:%u\n" + , FUNC_ADPT_ARG(adapter), is_bmc + , *(ptr + sizeof(struct rtw_ieee80211_hdr_3addr)) + , pattrib->privacy, pattrib->encrypt, pattrib->bdecrypted); + } + } +#endif + + if (!pattrib->privacy) { + if (!psta || !(psta->flags & WLAN_STA_MFP)) { + /* peer is not MFP capable, no need to check */ + goto exit; + } + + if (subtype == WIFI_ACTION) + category = *(ptr + sizeof(struct rtw_ieee80211_hdr_3addr)); + + if (is_bmc) { + /* broadcast cases */ + if (subtype == WIFI_ACTION) { + if (CATEGORY_IS_GROUP_PRIVACY(category)) { + /* drop broadcast group privacy action frame without encryption */ + #if DBG_VALIDATE_MGMT_PROTECT + RTW_INFO(FUNC_ADPT_FMT" broadcast gp action(%u) w/o encrypt\n" + , FUNC_ADPT_ARG(adapter), category); + #endif + goto fail; + } + if (CATEGORY_IS_ROBUST(category)) { + /* broadcast robust action frame need BIP check */ + goto bip_verify; + } + } + if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) { + /* broadcast deauth or disassoc frame need BIP check */ + goto bip_verify; + } + goto exit; + + } else { + /* unicast cases */ + #ifdef CONFIG_IEEE80211W + if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) { + if (!MLME_IS_MESH(adapter)) { + unsigned short reason = le16_to_cpu(*(unsigned short *)(ptr + WLAN_HDR_A3_LEN)); + + #if DBG_VALIDATE_MGMT_PROTECT + RTW_INFO(FUNC_ADPT_FMT" unicast %s, reason=%d w/o encrypt\n" + , FUNC_ADPT_ARG(adapter), subtype == WIFI_DEAUTH ? "deauth" : "disassoc", reason); + #endif + if (reason == 6 || reason == 7) { + /* issue sa query request */ + issue_action_SA_Query(adapter, psta->cmn.mac_addr, 0, 0, IEEE80211W_RIGHT_KEY); + } + } + goto fail; + } + #endif + + if (subtype == WIFI_ACTION && CATEGORY_IS_ROBUST(category)) { + if (psta->bpairwise_key_installed == _TRUE) { + #if DBG_VALIDATE_MGMT_PROTECT + RTW_INFO(FUNC_ADPT_FMT" unicast robust action(%d) w/o encrypt\n" + , FUNC_ADPT_ARG(adapter), category); + #endif + goto fail; + } + } + goto exit; + } + +bip_verify: +#ifdef CONFIG_IEEE80211W + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + if (psta->igtk_bmp) { + bip_cipher = psta->dot11wCipher; + igtk = psta->igtk.skey; + igtk_id = psta->igtk_id; + ipn = &psta->igtk_pn.val; + } else { + /* mesh MFP without IGTK */ + goto exit; + } + } else + #endif + { + bip_cipher = sec->dot11wCipher; + igtk = sec->dot11wBIPKey[sec->dot11wBIPKeyid].skey; + igtk_id = sec->dot11wBIPKeyid; + ipn = &sec->dot11wBIPrxpn.val; + } + + /* verify BIP MME IE */ + ret = rtw_bip_verify(bip_cipher, pattrib->pkt_len, + get_recvframe_data(precv_frame), + get_recvframe_len(precv_frame), + igtk, igtk_id, ipn); + + if (ret == _FAIL) { + /* RTW_INFO("802.11w BIP verify fail\n"); */ + goto fail; + + } else if (ret == RTW_RX_HANDLED) { + #if DBG_VALIDATE_MGMT_PROTECT + RTW_INFO(FUNC_ADPT_FMT" none protected packet\n", FUNC_ADPT_ARG(adapter)); + #endif + goto fail; + } +#endif /* CONFIG_IEEE80211W */ + goto exit; + } + + if (!psta || !(psta->flags & WLAN_STA_MFP)) { + /* not peer or peer is not MFP capable, drop it */ + goto fail; + } + + /* cases to decrypt mgmt frame */ + pattrib->bdecrypted = 0; +#ifdef CONFIG_RTW_MESH + if (is_bmc) + pattrib->encrypt = psta->group_privacy; + else +#endif + pattrib->encrypt = psta->dot118021XPrivacy; + pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* set iv and icv length */ + SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt); + _rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN); + + /* actual management data frame body */ + data_len = pattrib->pkt_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + mgmt_DATA = rtw_zmalloc(data_len); + if (mgmt_DATA == NULL) { + RTW_INFO(FUNC_ADPT_FMT" mgmt allocate fail !!!!!!!!!\n", FUNC_ADPT_ARG(adapter)); + goto fail; + } + +#if DBG_VALIDATE_MGMT_DEC + /* dump the packet content before decrypt */ + { + int pp; + + printk("pattrib->pktlen = %d =>", pattrib->pkt_len); + for (pp = 0; pp < pattrib->pkt_len; pp++) + printk(" %02x ", ptr[pp]); + printk("\n"); + } +#endif + + precv_frame = decryptor(adapter, precv_frame); + /* save actual management data frame body */ + _rtw_memcpy(mgmt_DATA, ptr + pattrib->hdrlen + pattrib->iv_len, data_len); + /* overwrite the iv field */ + _rtw_memcpy(ptr + pattrib->hdrlen, mgmt_DATA, data_len); + /* remove the iv and icv length */ + pattrib->pkt_len = pattrib->pkt_len - pattrib->iv_len - pattrib->icv_len; + rtw_mfree(mgmt_DATA, data_len); + +#if DBG_VALIDATE_MGMT_DEC + /* print packet content after decryption */ + { + int pp; + + printk("after decryption pattrib->pktlen = %d @@=>", pattrib->pkt_len); + for (pp = 0; pp < pattrib->pkt_len; pp++) + printk(" %02x ", ptr[pp]); + printk("\n"); + } +#endif + + if (!precv_frame) { + #if DBG_VALIDATE_MGMT_PROTECT + RTW_INFO(FUNC_ADPT_FMT" mgmt descrypt fail !!!!!!!!!\n", FUNC_ADPT_ARG(adapter)); + #endif + goto fail; + } + +exit: + return _SUCCESS; + +fail: + return _FAIL; + +} +#endif /* defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) */ + +union recv_frame *recvframe_chk_defrag(PADAPTER padapter, union recv_frame *precv_frame); + +sint validate_recv_mgnt_frame(PADAPTER padapter, union recv_frame *precv_frame) +{ + struct sta_info *psta = precv_frame->u.hdr.psta + = rtw_get_stainfo(&padapter->stapriv, get_addr2_ptr(precv_frame->u.hdr.rx_data)); + +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) + if (validate_mgmt_protect(padapter, precv_frame) == _FAIL) { + DBG_COUNTER(padapter->rx_logs.core_rx_pre_mgmt_err_80211w); + goto exit; + } +#endif + + precv_frame = recvframe_chk_defrag(padapter, precv_frame); + if (precv_frame == NULL) + return _SUCCESS; + + /* for rx pkt statistics */ + if (psta) { + psta->sta_stats.last_rx_time = rtw_get_current_time(); + psta->sta_stats.rx_mgnt_pkts++; + if (get_frame_sub_type(precv_frame->u.hdr.rx_data) == WIFI_BEACON) + psta->sta_stats.rx_beacon_pkts++; + else if (get_frame_sub_type(precv_frame->u.hdr.rx_data) == WIFI_PROBEREQ) + psta->sta_stats.rx_probereq_pkts++; + else if (get_frame_sub_type(precv_frame->u.hdr.rx_data) == WIFI_PROBERSP) { + if (_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(precv_frame->u.hdr.rx_data), ETH_ALEN) == _TRUE) + psta->sta_stats.rx_probersp_pkts++; + else if (is_broadcast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data)) + || is_multicast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data))) + psta->sta_stats.rx_probersp_bm_pkts++; + else + psta->sta_stats.rx_probersp_uo_pkts++; + } + } + + mgt_dispatcher(padapter, precv_frame); + +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) +exit: +#endif + return _SUCCESS; + +} + +sint validate_recv_data_frame(_adapter *adapter, union recv_frame *precv_frame) +{ + u8 bretry, a4_shift; + struct sta_info *psta = NULL; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct security_priv *psecuritypriv = &adapter->securitypriv; + sint ret = _SUCCESS; + + bretry = GetRetry(ptr); + a4_shift = (pattrib->to_fr_ds == 3) ? ETH_ALEN : 0; + + /* some address fields are different when using AMSDU */ + if (pattrib->qos) + pattrib->amsdu = GetAMsdu(ptr + WLAN_HDR_A3_LEN + a4_shift); + else + pattrib->amsdu = 0; + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(adapter)) { + ret = rtw_mesh_rx_data_validate_hdr(adapter, precv_frame, &psta); + goto pre_validate_status_chk; + } else +#endif +#ifdef CONFIG_AP_MODE + if (MLME_IS_AP(adapter)) { + ret = rtw_ap_rx_data_validate_hdr(adapter, precv_frame, &psta); + goto pre_validate_status_chk; + } else +#endif + if (MLME_IS_STA(adapter)) { + ret = rtw_sta_rx_data_validate_hdr(adapter, precv_frame, &psta); + goto pre_validate_status_chk; + } + + switch (pattrib->to_fr_ds) { + case 0: + _rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN); + ret = sta2sta_data_frame(adapter, precv_frame, &psta); + break; + + case 1: + _rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->dst, GetAddr3Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->bssid, GetAddr1Ptr(ptr), ETH_ALEN); + ret = sta2ap_data_frame(adapter, precv_frame, &psta); + break; + + case 2: + _rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->src, GetAddr3Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->bssid, get_addr2_ptr(ptr), ETH_ALEN); + ret = ap2sta_data_frame(adapter, precv_frame, &psta); + break; + + case 3: + default: + /* WDS is not supported */ + ret = _FAIL; + break; + } + +pre_validate_status_chk: + + if (ret == _FAIL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" case:%d, res:%d, ra="MAC_FMT", ta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), pattrib->to_fr_ds, ret, MAC_ARG(GetAddr1Ptr(ptr)), MAC_ARG(get_addr2_ptr(ptr))); + #endif + goto exit; + } else if (ret == RTW_RX_HANDLED) + goto exit; + + + if (psta == NULL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" psta == NULL, ra="MAC_FMT", ta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(GetAddr1Ptr(ptr)), MAC_ARG(get_addr2_ptr(ptr))); + #endif + ret = _FAIL; + goto exit; + } + + if ((psta->flags & WLAN_STA_AMSDU_DISABLE) && pattrib->amsdu) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" amsdu not allowed"MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + + } + + precv_frame->u.hdr.psta = psta; + precv_frame->u.hdr.preorder_ctrl = NULL; + pattrib->ack_policy = 0; + + /* parsing QC field */ + if (pattrib->qos == 1) { + pattrib->priority = GetPriority((ptr + WLAN_HDR_A3_LEN + a4_shift)); /* point to Qos field*/ + pattrib->ack_policy = GetAckpolicy((ptr + WLAN_HDR_A3_LEN + a4_shift)); + pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN + a4_shift; + if (pattrib->priority != 0 && pattrib->priority != 3) + adapter->recvpriv.is_any_non_be_pkts = _TRUE; + else + adapter->recvpriv.is_any_non_be_pkts = _FALSE; + } else { + pattrib->priority = 0; + pattrib->hdrlen = WLAN_HDR_A3_LEN + a4_shift; + } + + if (pattrib->order) /* HT-CTRL 11n */ + pattrib->hdrlen += 4; + + /* decache, drop duplicate recv packets */ + ret = recv_decache(precv_frame); + if (ret == _FAIL) + goto exit; + + if (!IS_MCAST(pattrib->ra)) { + + if (pattrib->qos) + precv_frame->u.hdr.preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority]; + + if (recv_ucast_pn_decache(precv_frame) == _FAIL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" recv_ucast_pn_decache return _FAIL for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + } + } else { + if (recv_bcast_pn_decache(precv_frame) == _FAIL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" recv_bcast_pn_decache return _FAIL for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + } + } + + if (pattrib->privacy) { +#ifdef CONFIG_TDLS + if ((psta->tdls_sta_state & TDLS_LINKED_STATE) && (psta->dot118021XPrivacy == _AES_)) + pattrib->encrypt = psta->dot118021XPrivacy; + else +#endif /* CONFIG_TDLS */ + GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra)); + + + SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt); + } else { + pattrib->encrypt = 0; + pattrib->iv_len = pattrib->icv_len = 0; + } + + /* drop unprotected frame in protected network. */ + if (psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_ ) { + if (IS_MCAST(pattrib->ra)) { + if (!pattrib->privacy) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT"recv plaintext bmc packet for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + } + } else { + /* unicast */ + u16 ether_type; + u8* ether_ptr = NULL; + u16 eapol_type = 0x888e; + ether_ptr = ptr + pattrib->hdrlen + pattrib->iv_len + RATTRIB_GET_MCTRL_LEN(pattrib) + LLC_HEADER_SIZE; + _rtw_memcpy(ðer_type, ether_ptr, 2); + ether_type = ntohs((unsigned short)ether_type); + + if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) { + /* CVE-2020-26140, CVE-2020-26143, CVE-2020-26147, let eapol packet go through*/ + if (!pattrib->privacy && ether_type != eapol_type ) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT"recv plaintext unicast packet for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + } + /* CVE-2020-26144, pevernt plaintext A-MSDU */ + /* This can prevent plantext A-MSDU cloacked as an EAPOL frame */ + if (!pattrib->privacy && pattrib->amsdu) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT"recv plaintext A-MSDU for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + } + /* CVE-2020-26139, Drop any forwarding eapol packet until 4-way has done. */ + if ((ether_type == eapol_type) + && (MLME_IS_AP(adapter) || MLME_IS_MESH(adapter)) + && (psta->dot118021XPrivacy == _NO_PRIVACY_) + && (!_rtw_memcmp( adapter_mac_addr(adapter), pattrib->dst, ETH_ALEN))) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" recv eapol packet forwarding(dst:"MAC_FMT") before 4-way finish.\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(pattrib->dst)); + #endif + ret = _FAIL; + goto exit; + } + } else { + /* CVE-2020-26140, CVE-2020-26143, CVE-2020-26147 */ + if (!pattrib->privacy) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT"recv plaintext packet for sta="MAC_FMT"\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(psta->cmn.mac_addr)); + #endif + ret = _FAIL; + goto exit; + } + } + } + } + +#ifdef CONFIG_RTW_MESH + if (!pattrib->amsdu + && pattrib->mesh_ctrl_present + && (!pattrib->encrypt || pattrib->bdecrypted)) + ret = rtw_mesh_rx_validate_mctrl_non_amsdu(adapter, precv_frame); +#endif + +exit: + return ret; +} + +static inline void dump_rx_packet(u8 *ptr) +{ + int i; + + RTW_INFO("#############################\n"); + for (i = 0; i < 64; i = i + 8) + RTW_INFO("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr + i), + *(ptr + i + 1), *(ptr + i + 2) , *(ptr + i + 3) , *(ptr + i + 4), *(ptr + i + 5), *(ptr + i + 6), *(ptr + i + 7)); + RTW_INFO("#############################\n"); +} + +sint validate_recv_frame(_adapter *adapter, union recv_frame *precv_frame); +sint validate_recv_frame(_adapter *adapter, union recv_frame *precv_frame) +{ + /* shall check frame subtype, to / from ds, da, bssid */ + + /* then call check if rx seq/frag. duplicated. */ + + u8 type; + u8 subtype; + sint retval = _SUCCESS; + + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct recv_priv *precvpriv = &adapter->recvpriv; + + u8 *ptr = precv_frame->u.hdr.rx_data; + u8 ver = (unsigned char)(*ptr) & 0x3 ; +#ifdef CONFIG_FIND_BEST_CHANNEL + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; +#endif + +#ifdef CONFIG_TDLS + struct tdls_info *ptdlsinfo = &adapter->tdlsinfo; +#endif /* CONFIG_TDLS */ +#ifdef CONFIG_WAPI_SUPPORT + PRT_WAPI_T pWapiInfo = &adapter->wapiInfo; + struct recv_frame_hdr *phdr = &precv_frame->u.hdr; + u8 wai_pkt = 0; + u16 sc; + u8 external_len = 0; +#endif + + +#ifdef CONFIG_FIND_BEST_CHANNEL + if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) { + int ch_set_idx = rtw_chset_search_ch(rfctl->channel_set, rtw_get_oper_ch(adapter)); + if (ch_set_idx >= 0) + rfctl->channel_set[ch_set_idx].rx_count++; + } +#endif + +#ifdef CONFIG_TDLS + if (ptdlsinfo->ch_sensing == 1 && ptdlsinfo->cur_channel != 0) + ptdlsinfo->collect_pkt_num[ptdlsinfo->cur_channel - 1]++; +#endif /* CONFIG_TDLS */ + +#ifdef RTK_DMP_PLATFORM + if (0) { + RTW_INFO("++\n"); + { + int i; + for (i = 0; i < 64; i = i + 8) + RTW_INFO("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:", *(ptr + i), + *(ptr + i + 1), *(ptr + i + 2) , *(ptr + i + 3) , *(ptr + i + 4), *(ptr + i + 5), *(ptr + i + 6), *(ptr + i + 7)); + + } + RTW_INFO("--\n"); + } +#endif /* RTK_DMP_PLATFORM */ + + /* add version chk */ + if (ver != 0) { + retval = _FAIL; + DBG_COUNTER(adapter->rx_logs.core_rx_pre_ver_err); + goto exit; + } + + type = GetFrameType(ptr); + subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2) */ + + pattrib->to_fr_ds = get_tofr_ds(ptr); + + pattrib->frag_num = GetFragNum(ptr); + pattrib->seq_num = GetSequence(ptr); + + pattrib->pw_save = GetPwrMgt(ptr); + pattrib->mfrag = GetMFrag(ptr); + pattrib->mdata = GetMData(ptr); + pattrib->privacy = GetPrivacy(ptr); + pattrib->order = GetOrder(ptr); +#ifdef CONFIG_WAPI_SUPPORT + sc = (pattrib->seq_num << 4) | pattrib->frag_num; +#endif + +#if 1 /* Dump rx packets */ + { + u8 bDumpRxPkt = 0; + + rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt)); + if (bDumpRxPkt == 1) /* dump all rx packets */ + dump_rx_packet(ptr); + else if ((bDumpRxPkt == 2) && (type == WIFI_MGT_TYPE)) + dump_rx_packet(ptr); + else if ((bDumpRxPkt == 3) && (type == WIFI_DATA_TYPE)) + dump_rx_packet(ptr); + } +#endif + switch (type) { + case WIFI_MGT_TYPE: /* mgnt */ + DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt); + retval = validate_recv_mgnt_frame(adapter, precv_frame); + if (retval == _FAIL) { + DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt_err); + } + retval = _FAIL; /* only data frame return _SUCCESS */ + break; + case WIFI_CTRL_TYPE: /* ctrl */ + DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl); + retval = validate_recv_ctrl_frame(adapter, precv_frame); + if (retval == _FAIL) { + DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl_err); + } + retval = _FAIL; /* only data frame return _SUCCESS */ + break; + case WIFI_DATA_TYPE: /* data */ + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data); +#ifdef CONFIG_WAPI_SUPPORT + if (pattrib->qos) + external_len = 2; + else + external_len = 0; + + wai_pkt = rtw_wapi_is_wai_packet(adapter, ptr); + + phdr->bIsWaiPacket = wai_pkt; + + if (wai_pkt != 0) { + if (sc != adapter->wapiInfo.wapiSeqnumAndFragNum) + adapter->wapiInfo.wapiSeqnumAndFragNum = sc; + else { + retval = _FAIL; + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_wapi_seq_err); + break; + } + } else { + + if (rtw_wapi_drop_for_key_absent(adapter, get_addr2_ptr(ptr))) { + retval = _FAIL; + WAPI_TRACE(WAPI_RX, "drop for key absent for rx\n"); + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_wapi_key_err); + break; + } + } + +#endif + + pattrib->qos = (subtype & BIT(7)) ? 1 : 0; + retval = validate_recv_data_frame(adapter, precv_frame); + if (retval == _FAIL) { + precvpriv->dbg_rx_drop_count++; + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_err); + } else if (retval == _SUCCESS) { + #ifdef DBG_RX_DUMP_EAP + if (!pattrib->encrypt || pattrib->bdecrypted) { + u8 bDumpRxPkt; + u16 eth_type; + + /* dump eapol */ + rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt)); + /* get ether_type */ + _rtw_memcpy(ð_type, ptr + pattrib->hdrlen + pattrib->iv_len + RATTRIB_GET_MCTRL_LEN(pattrib) + LLC_HEADER_SIZE, 2); + eth_type = ntohs((unsigned short) eth_type); + if ((bDumpRxPkt == 4) && (eth_type == 0x888e)) + dump_rx_packet(ptr); + } + #endif + } else + DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_handled); + break; + default: + DBG_COUNTER(adapter->rx_logs.core_rx_pre_unknown); + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" fail! type=0x%x\n" + , FUNC_ADPT_ARG(adapter), type); + #endif + retval = _FAIL; + break; + } + +exit: + + + return retval; +} + +/* Reture expected handling for LLC */ +enum rtw_rx_llc_hdl rtw_recv_llc_parse(u8 *msdu, u16 msdu_len) +{ + u16 eth_type; + + if (msdu_len < 8) + return RTW_RX_LLC_KEEP; + + eth_type = RTW_GET_BE16(msdu + SNAP_SIZE); + + if ((_rtw_memcmp(msdu, rtw_rfc1042_header, SNAP_SIZE) + && eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) + || _rtw_memcmp(msdu, rtw_bridge_tunnel_header, SNAP_SIZE)) { + /* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */ + return RTW_RX_LLC_REMOVE; + } else { + /* Leave Ethernet header part of hdr and full payload */ + return RTW_RX_LLC_KEEP; + } + + /* TODO: VLAN tagged */ +} + +/* remove the wlanhdr and add the eth_hdr */ +sint wlanhdr_to_ethhdr(union recv_frame *precvframe, enum rtw_rx_llc_hdl llc_hdl) +{ + u8 *ptr = get_recvframe_data(precvframe) ; /* point to frame_ctrl field */ + struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib; + sint rmv_len; + u16 eth_type, len; + sint ret = _SUCCESS; + + if (pattrib->encrypt) + recvframe_pull_tail(precvframe, pattrib->icv_len); + + rmv_len = pattrib->hdrlen + pattrib->iv_len + RATTRIB_GET_MCTRL_LEN(pattrib) + (llc_hdl ? SNAP_SIZE : 0); + len = precvframe->u.hdr.len - rmv_len; + + ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + (llc_hdl ? 2 : 0))); + if (!ptr) { + ret = _FAIL; + goto exiting; + } + + _rtw_memcpy(ptr, pattrib->dst, ETH_ALEN); + _rtw_memcpy(ptr + ETH_ALEN, pattrib->src, ETH_ALEN); + + if (!llc_hdl) { + len = htons(len); + _rtw_memcpy(ptr + 12, &len, 2); + } + + rtw_rframe_set_os_pkt(precvframe); + +exiting: + return ret; +} + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) +#ifndef CONFIG_SDIO_RX_COPY +#ifdef PLATFORM_LINUX +static void recvframe_expand_pkt( + PADAPTER padapter, + union recv_frame *prframe) +{ + struct recv_frame_hdr *pfhdr; + _pkt *ppkt; + u8 shift_sz; + u32 alloc_sz; + u8 *ptr; + + + pfhdr = &prframe->u.hdr; + + /* 6 is for IP header 8 bytes alignment in QoS packet case. */ + if (pfhdr->attrib.qos) + shift_sz = 6; + else + shift_sz = 0; + + /* for first fragment packet, need to allocate */ + /* (1536 + RXDESC_SIZE + drvinfo_sz) to reassemble packet */ + /* 8 is for skb->data 8 bytes alignment. + * alloc_sz = _RND(1536 + RXDESC_SIZE + pfhdr->attrib.drvinfosize + shift_sz + 8, 128); */ + alloc_sz = 1664; /* round (1536 + 24 + 32 + shift_sz + 8) to 128 bytes alignment */ + + /* 3 1. alloc new skb */ + /* prepare extra space for 4 bytes alignment */ + ppkt = rtw_skb_alloc(alloc_sz); + + if (!ppkt) + return; /* no way to expand */ + + /* 3 2. Prepare new skb to replace & release old skb */ + /* force ppkt->data at 8-byte alignment address */ + skb_reserve(ppkt, 8 - ((SIZE_PTR)ppkt->data & 7)); + /* force ip_hdr at 8-byte alignment address according to shift_sz */ + skb_reserve(ppkt, shift_sz); + + /* copy data to new pkt */ + ptr = skb_put(ppkt, pfhdr->len); + if (ptr) + _rtw_memcpy(ptr, pfhdr->rx_data, pfhdr->len); + + rtw_skb_free(pfhdr->pkt); + + /* attach new pkt to recvframe */ + pfhdr->pkt = ppkt; + pfhdr->rx_head = ppkt->head; + pfhdr->rx_data = ppkt->data; + pfhdr->rx_tail = skb_tail_pointer(ppkt); + pfhdr->rx_end = skb_end_pointer(ppkt); +} +#else /*!= PLATFORM_LINUX*/ +#warning "recvframe_expand_pkt not implement, defrag may crash system" +#endif +#endif /*#ifndef CONFIG_SDIO_RX_COPY*/ +#endif + +/* perform defrag */ +union recv_frame *recvframe_defrag(_adapter *adapter, _queue *defrag_q); +union recv_frame *recvframe_defrag(_adapter *adapter, _queue *defrag_q) +{ + _list *plist, *phead; + u8 *data, wlanhdr_offset; + u8 curfragnum; + struct recv_frame_hdr *pfhdr, *pnfhdr; + union recv_frame *prframe, *pnextrframe; + _queue *pfree_recv_queue; + u8 *pdata = NULL; + u64 tmp_iv_hdr = 0; + u64 pkt_pn = 0, cur_pn = 0; + struct rx_pkt_attrib *pattrib = NULL; + + curfragnum = 0; + pfree_recv_queue = &adapter->recvpriv.free_recv_queue; + + phead = get_list_head(defrag_q); + plist = get_next(phead); + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + /* CVE-2020-26146 */ + pattrib = &prframe->u.hdr.attrib; + if (pattrib->encrypt == _AES_ || pattrib->encrypt == _CCMP_256_ + || pattrib->encrypt == _GCMP_ || pattrib->encrypt == _GCMP_256_ ) { + pdata = prframe->u.hdr.rx_data; + tmp_iv_hdr = le64_to_cpu(*(u64*)(pdata + pattrib->hdrlen)); + /* get the first frame's PN. */ + cur_pn = CCMPH_2_PN(tmp_iv_hdr); + } + pfhdr = &prframe->u.hdr; + rtw_list_delete(&(prframe->u.list)); + + if (curfragnum != pfhdr->attrib.frag_num) { + /* the first fragment number must be 0 */ + /* free the whole queue */ + rtw_free_recvframe(prframe, pfree_recv_queue); + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + + return NULL; + } + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) +#ifndef CONFIG_SDIO_RX_COPY + recvframe_expand_pkt(adapter, prframe); +#endif +#endif + + curfragnum++; + + plist = get_list_head(defrag_q); + + plist = get_next(plist); + + data = get_recvframe_data(prframe); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + pnextrframe = LIST_CONTAINOR(plist, union recv_frame , u); + pnfhdr = &pnextrframe->u.hdr; + /* CVE-2020-26146, check whether the PN is consecutive. */ + pattrib = &pnextrframe->u.hdr.attrib; + if (pattrib->encrypt == _AES_ || pattrib->encrypt == _CCMP_256_ + || pattrib->encrypt == _GCMP_ || pattrib->encrypt == _GCMP_256_ ) { + pdata = pnextrframe->u.hdr.rx_data; + tmp_iv_hdr = le64_to_cpu(*(u64*)(pdata + pattrib->hdrlen)); + pkt_pn = CCMPH_2_PN(tmp_iv_hdr); + if (pkt_pn != cur_pn + 1) { + RTW_INFO("%s non-consective PN! old:%llu, new:%llu\n", + __func__, cur_pn, pkt_pn); + /* PN must be consecutive */ + /* release the defrag_q & prframe */ + rtw_free_recvframe(prframe, pfree_recv_queue); + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + return NULL; + } else { + cur_pn = pkt_pn; + } + } + + /* CVE-2020-24587, The keytrack of the fragment is supposed to be the same with other's */ + if (pfhdr->keytrack != pnfhdr->keytrack) { + RTW_INFO("Inconsistent key track, drop fragmented frame!\n"); + rtw_free_recvframe(prframe, pfree_recv_queue); + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + return NULL; + } + + /* check the fragment sequence (2nd ~n fragment frame) */ + + if (curfragnum != pnfhdr->attrib.frag_num) { + /* the fragment number must be increasing (after decache) */ + /* release the defrag_q & prframe */ + rtw_free_recvframe(prframe, pfree_recv_queue); + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + return NULL; + } + + curfragnum++; + + /* copy the 2nd~n fragment frame's payload to the first fragment */ + /* get the 2nd~last fragment frame's payload */ + + wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len; + + recvframe_pull(pnextrframe, wlanhdr_offset); + + /* append to first fragment frame's tail (if privacy frame, pull the ICV) */ + recvframe_pull_tail(prframe, pfhdr->attrib.icv_len); + + /* memcpy */ + _rtw_memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len); + + recvframe_put(prframe, pnfhdr->len); + + pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len; + plist = get_next(plist); + + }; + + /* free the defrag_q queue and return the prframe */ + rtw_free_recvframe_queue(defrag_q, pfree_recv_queue); + + + + return prframe; +} + +/* check if need to defrag, if needed queue the frame to defrag_q */ +union recv_frame *recvframe_chk_defrag(PADAPTER padapter, union recv_frame *precv_frame) +{ + u8 ismfrag; + u8 fragnum; + u8 *psta_addr; + struct recv_frame_hdr *pfhdr; + struct sta_info *psta; + struct sta_priv *pstapriv; + _list *phead; + union recv_frame *prtnframe = NULL; + _queue *pfree_recv_queue, *pdefrag_q = NULL; + + + pstapriv = &padapter->stapriv; + + pfhdr = &precv_frame->u.hdr; + + pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + + /* need to define struct of wlan header frame ctrl */ + ismfrag = pfhdr->attrib.mfrag; + fragnum = pfhdr->attrib.frag_num; + + psta_addr = pfhdr->attrib.ta; + psta = rtw_get_stainfo(pstapriv, psta_addr); + if (psta == NULL) { + u8 type = GetFrameType(pfhdr->rx_data); + if (type != WIFI_DATA_TYPE) { + psta = rtw_get_bcmc_stainfo(padapter); + if (psta) + pdefrag_q = &psta->sta_recvpriv.defrag_q; + } else + pdefrag_q = NULL; + } else + pdefrag_q = &psta->sta_recvpriv.defrag_q; + + if ((ismfrag == 0) && (fragnum == 0)) { + prtnframe = precv_frame;/* isn't a fragment frame */ + } else { + /* CVE-2020-26145, group addressed frame cannot use fragmentation!! */ + if (IS_MCAST(pfhdr->attrib.ra)) { + RTW_INFO("DROP group addressed fragment!\n"); + rtw_free_recvframe(precv_frame, pfree_recv_queue); + return NULL; + } + /* CVE-2020-24587 */ + if ((psta) && (pdefrag_q)) + precv_frame->u.hdr.keytrack = ATOMIC_READ(&psta->keytrack); + } + + if (ismfrag == 1) { + /* 0~(n-1) fragment frame */ + /* enqueue to defraf_g */ + if (pdefrag_q != NULL) { + if (fragnum == 0) { + /* the first fragment */ + if (_rtw_queue_empty(pdefrag_q) == _FALSE) { + /* free current defrag_q */ + rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue); + } + } + + + /* Then enqueue the 0~(n-1) fragment into the defrag_q */ + + /* _rtw_spinlock(&pdefrag_q->lock); */ + phead = get_list_head(pdefrag_q); + rtw_list_insert_tail(&pfhdr->list, phead); + /* _rtw_spinunlock(&pdefrag_q->lock); */ + + + prtnframe = NULL; + + } else { + /* can't find this ta's defrag_queue, so free this recv_frame */ + rtw_free_recvframe(precv_frame, pfree_recv_queue); + prtnframe = NULL; + } + + } + + if ((ismfrag == 0) && (fragnum != 0)) { + /* the last fragment frame */ + /* enqueue the last fragment */ + if (pdefrag_q != NULL) { + /* _rtw_spinlock(&pdefrag_q->lock); */ + phead = get_list_head(pdefrag_q); + rtw_list_insert_tail(&pfhdr->list, phead); + /* _rtw_spinunlock(&pdefrag_q->lock); */ + + /* call recvframe_defrag to defrag */ + precv_frame = recvframe_defrag(padapter, pdefrag_q); + prtnframe = precv_frame; + + } else { + /* can't find this ta's defrag_queue, so free this recv_frame */ + rtw_free_recvframe(precv_frame, pfree_recv_queue); + prtnframe = NULL; + } + + } + + + if ((prtnframe != NULL) && (prtnframe->u.hdr.attrib.privacy)) { + /* after defrag we must check tkip mic code */ + if (recvframe_chkmic(padapter, prtnframe) == _FAIL) { + rtw_free_recvframe(prtnframe, pfree_recv_queue); + prtnframe = NULL; + } + } + + + return prtnframe; + +} + +static int rtw_recv_indicatepkt_check(union recv_frame *rframe, u8 *ehdr_pos, u32 pkt_len) +{ + _adapter *adapter = rframe->u.hdr.adapter; + struct recv_priv *recvpriv = &adapter->recvpriv; + struct ethhdr *ehdr = (struct ethhdr *)ehdr_pos; + struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib; +#ifdef DBG_IP_R_MONITOR + int i; + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + struct wlan_network *cur_network = &(pmlmepriv->cur_network); +#endif/*DBG_IP_R_MONITOR*/ + enum eap_type eapol_type; + int ret = _FAIL; + +#ifdef CONFIG_WAPI_SUPPORT + if (rtw_wapi_check_for_drop(adapter, rframe, ehdr_pos)) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" rtw_wapi_check_for_drop\n" + , FUNC_ADPT_ARG(adapter)); + #endif + goto exit; + } +#endif + + if (rframe->u.hdr.psta) + rtw_st_ctl_rx(rframe->u.hdr.psta, ehdr_pos); + + if (ntohs(ehdr->h_proto) == 0x888e) { + eapol_type = parsing_eapol_packet(adapter, ehdr_pos + ETH_HLEN, rframe->u.hdr.psta, 0); + if ((eapol_type == EAPOL_1_4 || eapol_type == EAPOL_3_4) && pattrib->encrypt == 0) { + rframe->u.hdr.psta->resp_nonenc_eapol_key_starttime = rtw_get_current_time(); + RTW_INFO("Receive unencrypted eapol key\n"); + } + } +#ifdef DBG_ARP_DUMP + else if (ntohs(ehdr->h_proto) == ETH_P_ARP) + dump_arp_pkt(RTW_DBGDUMP, ehdr->h_dest, ehdr->h_source, ehdr_pos + ETH_HLEN, 0); +#endif + + if (recvpriv->sink_udpport > 0) + rtw_sink_rtp_seq_dbg(adapter, ehdr_pos); + +#ifdef DBG_UDP_PKT_LOSE_11AC + #define PAYLOAD_LEN_LOC_OF_IP_HDR 0x10 /*ethernet payload length location of ip header (DA + SA+eth_type+(version&hdr_len)) */ + + if (ntohs(ehdr->h_proto) == ETH_P_ARP) { + /* ARP Payload length will be 42bytes or 42+18(tailer)=60bytes*/ + if (pkt_len != 42 && pkt_len != 60) + RTW_INFO("Error !!%s,ARP Payload length %u not correct\n" , __func__ , pkt_len); + } else if (ntohs(ehdr->h_proto) == ETH_P_IP) { + if (be16_to_cpu(*((u16 *)(ehdr_pos + PAYLOAD_LEN_LOC_OF_IP_HDR))) != (pkt_len) - ETH_HLEN) { + RTW_INFO("Error !!%s,Payload length not correct\n" , __func__); + RTW_INFO("%s, IP header describe Total length=%u\n" , __func__ , be16_to_cpu(*((u16 *)(ehdr_pos + PAYLOAD_LEN_LOC_OF_IP_HDR)))); + RTW_INFO("%s, Pkt real length=%u\n" , __func__ , (pkt_len) - ETH_HLEN); + } + } +#endif + +#ifdef DBG_IP_R_MONITOR + #define LEN_ARP_OP_HDR 7 /*ARP OERATION */ + if (ntohs(ehdr->h_proto) == ETH_P_ARP) { + + if(check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE){ + if(ehdr_pos[ETHERNET_HEADER_SIZE+LEN_ARP_OP_HDR] == 2) { + + RTW_INFO("%s,[DBG_ARP] Rx ARP RSP Packet;SeqNum = %d !\n", + __FUNCTION__, pattrib->seq_num); + + dump_arp_pkt(RTW_DBGDUMP, ehdr->h_dest, ehdr->h_source, ehdr_pos + ETH_HLEN, 0); + + } + } + } +#endif/*DBG_IP_R_MONITOR*/ + +#ifdef CONFIG_AUTO_AP_MODE + if (ntohs(ehdr->h_proto) == 0x8899) + rtw_auto_ap_rx_msg_dump(adapter, rframe, ehdr_pos); +#endif + + ret = _SUCCESS; + +#ifdef CONFIG_WAPI_SUPPORT +exit: +#endif + return ret; +} + +#if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) +static void recv_free_fwd_resource(_adapter *adapter, struct xmit_frame *fwd_frame, _list *b2u_list) +{ + struct xmit_priv *xmitpriv = &adapter->xmitpriv; + + if (fwd_frame) + rtw_free_xmitframe(xmitpriv, fwd_frame); + +#if CONFIG_RTW_DATA_BMC_TO_UC + if (!rtw_is_list_empty(b2u_list)) { + struct xmit_frame *b2uframe; + _list *list; + + list = get_next(b2u_list); + while (rtw_end_of_queue_search(b2u_list, list) == _FALSE) { + b2uframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + rtw_list_delete(&b2uframe->list); + rtw_free_xmitframe(xmitpriv, b2uframe); + } + } +#endif +} + +static void recv_fwd_pkt_hdl(_adapter *adapter, _pkt *pkt + , u8 act, struct xmit_frame *fwd_frame, _list *b2u_list) +{ + struct xmit_priv *xmitpriv = &adapter->xmitpriv; + _pkt *fwd_pkt = pkt; + + if (act & RTW_RX_MSDU_ACT_INDICATE) { + fwd_pkt = rtw_os_pkt_copy(pkt); + if (!fwd_pkt) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s rtw_os_pkt_copy fail\n", __func__); + #endif + recv_free_fwd_resource(adapter, fwd_frame, b2u_list); + goto exit; + } + } + +#if CONFIG_RTW_DATA_BMC_TO_UC + if (!rtw_is_list_empty(b2u_list)) { + _list *list = get_next(b2u_list); + struct xmit_frame *b2uframe; + + while (rtw_end_of_queue_search(b2u_list, list) == _FALSE) { + b2uframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + rtw_list_delete(&b2uframe->list); + + if (!fwd_frame && rtw_is_list_empty(b2u_list)) /* the last fwd_pkt */ + b2uframe->pkt = fwd_pkt; + else + b2uframe->pkt = rtw_os_pkt_copy(fwd_pkt); + if (!b2uframe->pkt) { + rtw_free_xmitframe(xmitpriv, b2uframe); + continue; + } + + rtw_xmit_posthandle(adapter, b2uframe, b2uframe->pkt); + } + } +#endif + + if (fwd_frame) { + fwd_frame->pkt = fwd_pkt; + if (rtw_xmit_posthandle(adapter, fwd_frame, fwd_pkt) < 0) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s rtw_xmit_posthandle fail\n", __func__); + #endif + xmitpriv->tx_drop++; + } + } + +exit: + return; +} +#endif /* defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) */ + +/* + * From WFA suggestion: * + * If first subframe meets one of the following condition, * + * the whole received AMSDU should drop. * + * 1. subframe's DA is not the same as RA in From DS case. * + * 2. subframe's SA is not the same as TA in To DS case. * + * 3. subframe's DA is AA:AA:03:00:00:00 * + */ +static u8 validate_amsdu_content(_adapter *padapter, union recv_frame *prframe, + const u8 *da, const u8 *sa) +{ + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + u8 ret = _SUCCESS; + + /* Use the recommendation method form Wi-Fi alliance to check subframe */ + /* in protected network */ + if (padapter->registrypriv.amsdu_mode == RTW_AMSDU_MODE_NON_SPP && + padapter->securitypriv.dot11PrivacyAlgrthm != _NO_PRIVACY_) { + + /* 1.check From DS */ + if (pattrib->to_fr_ds == 2) { + if (_rtw_memcmp(da, pattrib->ra, ETH_ALEN) == _FALSE) + ret = _FAIL; + } + + /* 2.check To DS */ + if (pattrib->to_fr_ds == 1) { + if (_rtw_memcmp(sa, pattrib->ta, ETH_ALEN) == _FALSE) + ret = _FAIL; + } + + /* 3.Check whether DA is AA:AA:03:00:00:00 */ + if (_rtw_memcmp(da, rtw_rfc1042_header, ETH_ALEN) == _TRUE) + ret = _FAIL; + + } + + return ret; + +} + +int amsdu_to_msdu(_adapter *padapter, union recv_frame *prframe) +{ + struct rx_pkt_attrib *rattrib = &prframe->u.hdr.attrib; + int a_len, padding_len; + u16 nSubframe_Length; + u8 nr_subframes, i; + u8 *pdata; + _pkt *sub_pkt, *subframes[MAX_SUBFRAME_COUNT]; + struct recv_priv *precvpriv = &padapter->recvpriv; + _queue *pfree_recv_queue = &(precvpriv->free_recv_queue); + const u8 *da, *sa; + int act; +#if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + struct xmit_frame *fwd_frame; + _list b2u_list; +#endif + enum rtw_rx_llc_hdl llc_hdl; + u8 mctrl_len = 0; + int ret = _SUCCESS; + + nr_subframes = 0; + + recvframe_pull(prframe, rattrib->hdrlen); + + if (rattrib->iv_len > 0) + recvframe_pull(prframe, rattrib->iv_len); + if (rattrib->encrypt) + recvframe_pull_tail(prframe, rattrib->icv_len); + + a_len = prframe->u.hdr.len; + pdata = prframe->u.hdr.rx_data; + + while (a_len > ETH_HLEN) { + /* Offset 12 denote 2 mac address */ + nSubframe_Length = RTW_GET_BE16(pdata + 12); + if (a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) { + RTW_INFO("nRemain_Length is %d and nSubframe_Length is : %d\n", a_len, nSubframe_Length); + break; + } + + act = RTW_RX_MSDU_ACT_INDICATE; + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + fwd_frame = NULL; + #endif + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + u8 *mda = pdata, *msa = pdata + ETH_ALEN; + struct rtw_ieee80211s_hdr *mctrl = (struct rtw_ieee80211s_hdr *)(pdata + ETH_HLEN); + int v_ret; + + v_ret = rtw_mesh_rx_data_validate_mctrl(padapter, prframe + , mctrl, mda, msa, &mctrl_len, &da, &sa); + + if (validate_amsdu_content(padapter, prframe, da, sa) == _FAIL) { + RTW_INFO("%s check subframe content fail!\n", __func__); + break; + } + + if (v_ret != _SUCCESS) + goto move_to_next; + + llc_hdl = rtw_recv_llc_parse(pdata + ETH_HLEN + mctrl_len, nSubframe_Length - mctrl_len); + act = rtw_mesh_rx_msdu_act_check(prframe + , mda, msa, da, sa, mctrl + , pdata + ETH_HLEN + mctrl_len, llc_hdl + , &fwd_frame, &b2u_list); + } else + #endif + { + da = pdata; + sa = pdata + ETH_ALEN; + + if (validate_amsdu_content(padapter, prframe, da, sa) == _FAIL) { + RTW_INFO("%s check subframe content fail!\n", __func__); + break; + } + + llc_hdl = rtw_recv_llc_parse(pdata + ETH_HLEN, nSubframe_Length); + #ifdef CONFIG_AP_MODE + if (MLME_IS_AP(padapter)) { + act = rtw_ap_rx_msdu_act_check(prframe, da, sa + , pdata + ETH_HLEN, llc_hdl, &fwd_frame, &b2u_list); + } else + #endif + if (MLME_IS_STA(padapter)) + act = rtw_sta_rx_amsdu_act_check(prframe, da, sa); + } + + if (!act) + goto move_to_next; + + rtw_led_rx_control(padapter, da); + + sub_pkt = rtw_os_alloc_msdu_pkt(prframe, da, sa + , pdata + ETH_HLEN + mctrl_len, nSubframe_Length - mctrl_len, llc_hdl); + if (sub_pkt == NULL) { + if (act & RTW_RX_MSDU_ACT_INDICATE) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME %s rtw_os_alloc_msdu_pkt fail\n", __func__); + #endif + } + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + if (act & RTW_RX_MSDU_ACT_FORWARD) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s rtw_os_alloc_msdu_pkt fail\n", __func__); + #endif + recv_free_fwd_resource(padapter, fwd_frame, &b2u_list); + } + #endif + break; + } + + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + if (act & RTW_RX_MSDU_ACT_FORWARD) { + recv_fwd_pkt_hdl(padapter, sub_pkt, act, fwd_frame, &b2u_list); + if (!(act & RTW_RX_MSDU_ACT_INDICATE)) + goto move_to_next; + } + #endif + + if (rtw_recv_indicatepkt_check(prframe, rtw_os_pkt_data(sub_pkt), rtw_os_pkt_len(sub_pkt)) == _SUCCESS) + subframes[nr_subframes++] = sub_pkt; + else + rtw_os_pkt_free(sub_pkt); + +move_to_next: + /* move the data point to data content */ + pdata += ETH_HLEN; + a_len -= ETH_HLEN; + + if (nr_subframes >= MAX_SUBFRAME_COUNT) { + RTW_WARN("ParseSubframe(): Too many Subframes! Packets dropped!\n"); + break; + } + + pdata += nSubframe_Length; + a_len -= nSubframe_Length; + if (a_len != 0) { + padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4 - 1)); + if (padding_len == 4) + padding_len = 0; + + if (a_len < padding_len) { + RTW_INFO("ParseSubframe(): a_len < padding_len !\n"); + break; + } + pdata += padding_len; + a_len -= padding_len; + } + } + + for (i = 0; i < nr_subframes; i++) { + sub_pkt = subframes[i]; + + /* Indicat the packets to upper layer */ + if (sub_pkt) + rtw_os_recv_indicate_pkt(padapter, sub_pkt, prframe); + } + + prframe->u.hdr.len = 0; + rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */ + + return ret; +} + +static int recv_process_mpdu(_adapter *padapter, union recv_frame *prframe) +{ + _queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + int ret; + + if (pattrib->amsdu) { + ret = amsdu_to_msdu(padapter, prframe); + if (ret != _SUCCESS) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" amsdu_to_msdu fail\n" + , FUNC_ADPT_ARG(padapter)); + #endif + rtw_free_recvframe(prframe, pfree_recv_queue); + goto exit; + } + } else { + u8 *msdu = get_recvframe_data(prframe) + + pattrib->hdrlen + pattrib->iv_len + RATTRIB_GET_MCTRL_LEN(pattrib); + u16 msdu_len = prframe->u.hdr.len + - pattrib->hdrlen - pattrib->iv_len - RATTRIB_GET_MCTRL_LEN(pattrib) + - (pattrib->encrypt ? pattrib->icv_len : 0); + enum rtw_rx_llc_hdl llc_hdl = rtw_recv_llc_parse(msdu, msdu_len); + int act = RTW_RX_MSDU_ACT_INDICATE; + + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + struct xmit_frame *fwd_frame = NULL; + _list b2u_list; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + if (pattrib->mesh_ctrl_present) + act = rtw_mesh_rx_msdu_act_check(prframe + , pattrib->mda, pattrib->msa + , pattrib->dst, pattrib->src + , (struct rtw_ieee80211s_hdr *)(msdu - RATTRIB_GET_MCTRL_LEN(pattrib)) + , msdu, llc_hdl + , &fwd_frame, &b2u_list); + } else + #endif + if (MLME_IS_AP(padapter)) + act = rtw_ap_rx_msdu_act_check(prframe, pattrib->dst, pattrib->src + , msdu, llc_hdl, &fwd_frame, &b2u_list); + #endif + + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + if (!act) { + rtw_free_recvframe(prframe, pfree_recv_queue); + ret = _FAIL; + goto exit; + } + #endif + + rtw_led_rx_control(padapter, pattrib->dst); + + ret = wlanhdr_to_ethhdr(prframe, llc_hdl); + if (ret != _SUCCESS) { + if (act & RTW_RX_MSDU_ACT_INDICATE) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" wlanhdr_to_ethhdr: drop pkt\n" + , FUNC_ADPT_ARG(padapter)); + #endif + } + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + if (act & RTW_RX_MSDU_ACT_FORWARD) { + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s wlanhdr_to_ethhdr fail\n", __func__); + #endif + recv_free_fwd_resource(padapter, fwd_frame, &b2u_list); + } + #endif + rtw_free_recvframe(prframe, pfree_recv_queue); + goto exit; + } + + #if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + if (act & RTW_RX_MSDU_ACT_FORWARD) { + recv_fwd_pkt_hdl(padapter, prframe->u.hdr.pkt, act, fwd_frame, &b2u_list); + if (!(act & RTW_RX_MSDU_ACT_INDICATE)) { + prframe->u.hdr.pkt = NULL; + rtw_free_recvframe(prframe, pfree_recv_queue); + goto exit; + } + } + #endif + + if (!RTW_CANNOT_RUN(padapter)) { + ret = rtw_recv_indicatepkt_check(prframe + , get_recvframe_data(prframe), get_recvframe_len(prframe)); + if (ret != _SUCCESS) { + rtw_free_recvframe(prframe, pfree_recv_queue); + goto exit; + } + + /* indicate this recv_frame */ + ret = rtw_recv_indicatepkt(padapter, prframe); + if (ret != _SUCCESS) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" rtw_recv_indicatepkt fail!\n" + , FUNC_ADPT_ARG(padapter)); + #endif + goto exit; + } + } else { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" DS:%u SR:%u\n" + , FUNC_ADPT_ARG(padapter) + , rtw_is_drv_stopped(padapter) + , rtw_is_surprise_removed(padapter)); + #endif + ret = _SUCCESS; /* don't count as packet drop */ + rtw_free_recvframe(prframe, pfree_recv_queue); + } + } + +exit: + return ret; +} + +#if defined(CONFIG_80211N_HT) && defined(CONFIG_RECV_REORDERING_CTRL) +static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num) +{ + PADAPTER padapter = preorder_ctrl->padapter; + struct recv_priv *precvpriv = &padapter->recvpriv; + u8 wsize = preorder_ctrl->wsize_b; + u16 wend; + + /* Rx Reorder initialize condition. */ + if (preorder_ctrl->indicate_seq == 0xFFFF) { + preorder_ctrl->indicate_seq = seq_num; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_INIT indicate_seq:%d, seq_num:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, seq_num); + #endif + } + wend = (preorder_ctrl->indicate_seq + wsize - 1) & 0xFFF; /* % 4096; */ + + /* Drop out the packet which SeqNum is smaller than WinStart */ + if (SN_LESS(seq_num, preorder_ctrl->indicate_seq)) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO(FUNC_ADPT_FMT" tid:%u indicate_seq:%d > seq_num:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, seq_num); + #endif + return _FALSE; + } + + /* + * Sliding window manipulation. Conditions includes: + * 1. Incoming SeqNum is equal to WinStart =>Window shift 1 + * 2. Incoming SeqNum is larger than the WinEnd => Window shift N + */ + if (SN_EQUAL(seq_num, preorder_ctrl->indicate_seq)) { + preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_EQUAL indicate_seq:%d, seq_num:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, seq_num); + #endif + + } else if (SN_LESS(wend, seq_num)) { + /* boundary situation, when seq_num cross 0xFFF */ + if (seq_num >= (wsize - 1)) + preorder_ctrl->indicate_seq = seq_num + 1 - wsize; + else + preorder_ctrl->indicate_seq = 0xFFF - (wsize - (seq_num + 1)) + 1; + + precvpriv->dbg_rx_ampdu_window_shift_cnt++; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_LESS(wend, seq_num) indicate_seq:%d, seq_num:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, seq_num); + #endif + } + + return _TRUE; +} + +static int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe) +{ + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + _queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + _list *phead, *plist; + union recv_frame *pnextrframe; + struct rx_pkt_attrib *pnextattrib; + + /* DbgPrint("+enqueue_reorder_recvframe()\n"); */ + + /* _enter_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + /* _rtw_spinlock_ex(&ppending_recvframe_queue->lock); */ + + + phead = get_list_head(ppending_recvframe_queue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u); + pnextattrib = &pnextrframe->u.hdr.attrib; + + if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num)) + plist = get_next(plist); + else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num)) { + /* Duplicate entry is found!! Do not insert current entry. */ + + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + + return _FALSE; + } else + break; + + /* DbgPrint("enqueue_reorder_recvframe():while\n"); */ + + } + + + /* _enter_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + /* _rtw_spinlock_ex(&ppending_recvframe_queue->lock); */ + + rtw_list_delete(&(prframe->u.hdr.list)); + + rtw_list_insert_tail(&(prframe->u.hdr.list), plist); + + /* _rtw_spinunlock_ex(&ppending_recvframe_queue->lock); */ + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + + + return _TRUE; + +} + +static void recv_indicatepkts_pkt_loss_cnt(_adapter *padapter, u64 prev_seq, u64 current_seq) +{ + struct recv_priv *precvpriv = &padapter->recvpriv; + + if (current_seq < prev_seq) { + precvpriv->dbg_rx_ampdu_loss_count += (4096 + current_seq - prev_seq); + precvpriv->rx_drop += (4096 + current_seq - prev_seq); + } else { + precvpriv->dbg_rx_ampdu_loss_count += (current_seq - prev_seq); + precvpriv->rx_drop += (current_seq - prev_seq); + } +} + +static int recv_indicatepkts_in_order(_adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced) +{ + /* _irqL irql; */ + _list *phead, *plist; + union recv_frame *prframe; + struct rx_pkt_attrib *pattrib; + /* u8 index = 0; */ + int bPktInBuf = _FALSE; + struct recv_priv *precvpriv = &padapter->recvpriv; + _queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + + DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_in_oder); + + /* DbgPrint("+recv_indicatepkts_in_order\n"); */ + + /* _enter_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + /* _rtw_spinlock_ex(&ppending_recvframe_queue->lock); */ + + phead = get_list_head(ppending_recvframe_queue); + plist = get_next(phead); + +#if 0 + /* Check if there is any other indication thread running. */ + if (pTS->RxIndicateState == RXTS_INDICATE_PROCESSING) + return; +#endif + + /* Handling some condition for forced indicate case. */ + if (bforced == _TRUE) { + precvpriv->dbg_rx_ampdu_forced_indicate_count++; + if (rtw_is_list_empty(phead)) { + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + /* _rtw_spinunlock_ex(&ppending_recvframe_queue->lock); */ + return _TRUE; + } + + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + pattrib = &prframe->u.hdr.attrib; + + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u FORCE indicate_seq:%d, seq_num:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + recv_indicatepkts_pkt_loss_cnt(padapter, preorder_ctrl->indicate_seq, pattrib->seq_num); + preorder_ctrl->indicate_seq = pattrib->seq_num; + } + + /* Prepare indication list and indication. */ + /* Check if there is any packet need indicate. */ + while (!rtw_is_list_empty(phead)) { + + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + pattrib = &prframe->u.hdr.attrib; + + if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) { + +#if 0 + /* This protect buffer from overflow. */ + if (index >= REORDER_WIN_SIZE) { + RT_ASSERT(FALSE, ("IndicateRxReorderList(): Buffer overflow!!\n")); + bPktInBuf = TRUE; + break; + } +#endif + + plist = get_next(plist); + rtw_list_delete(&(prframe->u.hdr.list)); + + if (SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num)) { + preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_EQUAL indicate_seq:%d, seq_num:%d\n" + , FUNC_ADPT_ARG(padapter), preorder_ctrl->tid, preorder_ctrl->indicate_seq, pattrib->seq_num); + #endif + } + +#if 0 + index++; + if (index == 1) { + /* Cancel previous pending timer. */ + /* PlatformCancelTimer(Adapter, &pTS->RxPktPendingTimer); */ + if (bforced != _TRUE) { + /* RTW_INFO("_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);\n"); */ + _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer); + } + } +#endif + + /* Set this as a lock to make sure that only one thread is indicating packet. */ + /* pTS->RxIndicateState = RXTS_INDICATE_PROCESSING; */ + + /* Indicate packets */ + /* RT_ASSERT((index<=REORDER_WIN_SIZE), ("RxReorderIndicatePacket(): Rx Reorder buffer full!!\n")); */ + + + /* indicate this recv_frame */ + /* DbgPrint("recv_indicatepkts_in_order, indicate_seq=%d, seq_num=%d\n", precvpriv->indicate_seq, pattrib->seq_num); */ + if (recv_process_mpdu(padapter, prframe) != _SUCCESS) + precvpriv->dbg_rx_drop_count++; + + /* Update local variables. */ + bPktInBuf = _FALSE; + + } else { + bPktInBuf = _TRUE; + break; + } + + /* DbgPrint("recv_indicatepkts_in_order():while\n"); */ + + } + + /* _rtw_spinunlock_ex(&ppending_recvframe_queue->lock); */ + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + +#if 0 + /* Release the indication lock and set to new indication step. */ + if (bPktInBuf) { + /* Set new pending timer. */ + /* pTS->RxIndicateState = RXTS_INDICATE_REORDER; */ + /* PlatformSetTimer(Adapter, &pTS->RxPktPendingTimer, pHTInfo->RxReorderPendingTime); */ + + _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME); + } else { + /* pTS->RxIndicateState = RXTS_INDICATE_IDLE; */ + } +#endif + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + + /* return _TRUE; */ + return bPktInBuf; + +} + +static int recv_indicatepkt_reorder(_adapter *padapter, union recv_frame *prframe) +{ + _irqL irql; + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl; + _queue *ppending_recvframe_queue = preorder_ctrl ? &preorder_ctrl->pending_recvframe_queue : NULL; + struct recv_priv *precvpriv = &padapter->recvpriv; + + if (!pattrib->qos || !preorder_ctrl || preorder_ctrl->enable == _FALSE) + goto _success_exit; + + + DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_reoder); + + _enter_critical_bh(&ppending_recvframe_queue->lock, &irql); + + + if(rtw_test_and_clear_bit(RTW_RECV_ACK_OR_TIMEOUT, &preorder_ctrl->rec_abba_rsp_ack)) + preorder_ctrl->indicate_seq = 0xFFFF; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ %s:preorder_ctrl->rec_abba_rsp_ack = %u,indicate_seq = %d\n" + , __func__ + , preorder_ctrl->rec_abba_rsp_ack + , preorder_ctrl->indicate_seq); + #endif + + /* s2. check if winstart_b(indicate_seq) needs to been updated */ + if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num)) { + precvpriv->dbg_rx_ampdu_drop_count++; + /* pHTInfo->RxReorderDropCounter++; */ + /* ReturnRFDList(Adapter, pRfd); */ + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + /* return _FAIL; */ + + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" check_indicate_seq fail\n" + , FUNC_ADPT_ARG(padapter)); + #endif +#if 0 + rtw_recv_indicatepkt(padapter, prframe); + + _exit_critical_bh(&ppending_recvframe_queue->lock, &irql); + + goto _success_exit; +#else + goto _err_exit; +#endif + } + + + /* s3. Insert all packet into Reorder Queue to maintain its ordering. */ + if (!enqueue_reorder_recvframe(preorder_ctrl, prframe)) { + /* DbgPrint("recv_indicatepkt_reorder, enqueue_reorder_recvframe fail!\n"); */ + /* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */ + /* return _FAIL; */ + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" enqueue_reorder_recvframe fail\n" + , FUNC_ADPT_ARG(padapter)); + #endif + goto _err_exit; + } + + + /* s4. */ + /* Indication process. */ + /* After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets */ + /* with the SeqNum smaller than latest WinStart and buffer other packets. */ + /* */ + /* For Rx Reorder condition: */ + /* 1. All packets with SeqNum smaller than WinStart => Indicate */ + /* 2. All packets with SeqNum larger than or equal to WinStart => Buffer it. */ + /* */ + + /* recv_indicatepkts_in_order(padapter, preorder_ctrl, _TRUE); */ + if (recv_indicatepkts_in_order(padapter, preorder_ctrl, _FALSE) == _TRUE) { + if (!preorder_ctrl->bReorderWaiting) { + preorder_ctrl->bReorderWaiting = _TRUE; + _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME); + } + _exit_critical_bh(&ppending_recvframe_queue->lock, &irql); + } else { + preorder_ctrl->bReorderWaiting = _FALSE; + _exit_critical_bh(&ppending_recvframe_queue->lock, &irql); + _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer); + } + + return RTW_RX_HANDLED; + +_success_exit: + + return _SUCCESS; + +_err_exit: + + _exit_critical_bh(&ppending_recvframe_queue->lock, &irql); + + return _FAIL; +} + + +void rtw_reordering_ctrl_timeout_handler(void *pcontext) +{ + _irqL irql; + struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext; + _adapter *padapter = NULL; + _queue *ppending_recvframe_queue = NULL; + + + if (preorder_ctrl == NULL) + return; + + padapter = preorder_ctrl->padapter; + if (RTW_CANNOT_RUN(padapter)) + return; + + ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + + /* RTW_INFO("+rtw_reordering_ctrl_timeout_handler()=>\n"); */ + + _enter_critical_bh(&ppending_recvframe_queue->lock, &irql); + + preorder_ctrl->bReorderWaiting = _FALSE; + + if (recv_indicatepkts_in_order(padapter, preorder_ctrl, _TRUE) == _TRUE) + _set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME); + + _exit_critical_bh(&ppending_recvframe_queue->lock, &irql); + +} +#endif /* defined(CONFIG_80211N_HT) && defined(CONFIG_RECV_REORDERING_CTRL) */ + +static void recv_set_iseq_before_mpdu_process(union recv_frame *rframe, u16 seq_num, const char *caller) +{ +#if defined(CONFIG_80211N_HT) && defined(CONFIG_RECV_REORDERING_CTRL) + struct recv_reorder_ctrl *reorder_ctrl = rframe->u.hdr.preorder_ctrl; + + if (reorder_ctrl) { + reorder_ctrl->indicate_seq = seq_num; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ %s("ADPT_FMT")-B tid:%u indicate_seq:%d, seq_num:%d\n" + , caller, ADPT_ARG(reorder_ctrl->padapter) + , reorder_ctrl->tid, reorder_ctrl->indicate_seq, seq_num); + #endif + } +#endif +} + +static void recv_set_iseq_after_mpdu_process(union recv_frame *rframe, u16 seq_num, const char *caller) +{ +#if defined(CONFIG_80211N_HT) && defined(CONFIG_RECV_REORDERING_CTRL) + struct recv_reorder_ctrl *reorder_ctrl = rframe->u.hdr.preorder_ctrl; + + if (reorder_ctrl) { + reorder_ctrl->indicate_seq = (reorder_ctrl->indicate_seq + 1) % 4096; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ %s("ADPT_FMT")-A tid:%u indicate_seq:%d, seq_num:%d\n" + , caller, ADPT_ARG(reorder_ctrl->padapter) + , reorder_ctrl->tid, reorder_ctrl->indicate_seq, seq_num); + #endif + } +#endif +} + +#ifdef CONFIG_MP_INCLUDED +int validate_mp_recv_frame(_adapter *adapter, union recv_frame *precv_frame) +{ + int ret = _SUCCESS; + u8 *ptr = precv_frame->u.hdr.rx_data; + u8 type, subtype; + struct mp_priv *pmppriv = &adapter->mppriv; + struct mp_tx *pmptx; + unsigned char *sa , *da, *bs; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + u32 i = 0; + u8 rtk_prefix[]={0x52, 0x65, 0x61, 0x6C, 0x4C, 0x6F, 0x76, 0x65, 0x54, 0x65, 0x6B}; + u8 *prx_data; + pmptx = &pmppriv->tx; + + + if (pmppriv->mplink_brx == _FALSE) { + + u8 bDumpRxPkt = 0; + type = GetFrameType(ptr); + subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2) */ + + RTW_DBG("hdr len = %d iv_len=%d \n", pattrib->hdrlen , pattrib->iv_len); + prx_data = ptr + pattrib->hdrlen + pattrib->iv_len; + + for (i = 0; i < precv_frame->u.hdr.len; i++) { + if (precv_frame->u.hdr.len < (11 + i)) + break; + + if (_rtw_memcmp(prx_data + i, (void *)&rtk_prefix, 11) == _FALSE) { + bDumpRxPkt = 0; + RTW_DBG("prx_data = %02X != rtk_prefix[%d] = %02X \n", *(prx_data + i), i , rtk_prefix[i]); + } else { + bDumpRxPkt = 1; + RTW_DBG("prx_data = %02X = rtk_prefix[%d] = %02X \n", *(prx_data + i), i , rtk_prefix[i]); + break; + } + } + + if (bDumpRxPkt == 1) { /* dump all rx packets */ + int i; + RTW_INFO("############ type:0x%02x subtype:0x%02x #################\n", type, subtype); + + for (i = 0; i < precv_frame->u.hdr.len; i = i + 8) + RTW_INFO("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr + i), + *(ptr + i + 1), *(ptr + i + 2) , *(ptr + i + 3) , *(ptr + i + 4), *(ptr + i + 5), *(ptr + i + 6), *(ptr + i + 7)); + RTW_INFO("#############################\n"); + _rtw_memset(pmppriv->mplink_buf, '\0' , sizeof(pmppriv->mplink_buf)); + _rtw_memcpy(pmppriv->mplink_buf, ptr, precv_frame->u.hdr.len); + pmppriv->mplink_rx_len = precv_frame->u.hdr.len; + pmppriv->mplink_brx =_TRUE; + } + } + if (pmppriv->bloopback) { + if (_rtw_memcmp(ptr + 24, pmptx->buf + 24, precv_frame->u.hdr.len - 24) == _FALSE) { + RTW_INFO("Compare payload content Fail !!!\n"); + ret = _FAIL; + } + } + if (pmppriv->bSetRxBssid == _TRUE) { + + sa = get_addr2_ptr(ptr); + da = GetAddr1Ptr(ptr); + bs = GetAddr3Ptr(ptr); + type = GetFrameType(ptr); + subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2) */ + + if (_rtw_memcmp(bs, adapter->mppriv.network_macaddr, ETH_ALEN) == _FALSE) + ret = _FAIL; + + RTW_DBG("############ type:0x%02x subtype:0x%02x #################\n", type, subtype); + RTW_DBG("A2 sa %02X:%02X:%02X:%02X:%02X:%02X \n", *(sa) , *(sa + 1), *(sa+ 2), *(sa + 3), *(sa + 4), *(sa + 5)); + RTW_DBG("A1 da %02X:%02X:%02X:%02X:%02X:%02X \n", *(da) , *(da + 1), *(da+ 2), *(da + 3), *(da + 4), *(da + 5)); + RTW_DBG("A3 bs %02X:%02X:%02X:%02X:%02X:%02X \n --------------------------\n", *(bs) , *(bs + 1), *(bs+ 2), *(bs + 3), *(bs + 4), *(bs + 5)); + } + + if (!adapter->mppriv.bmac_filter) + return ret; + + if (_rtw_memcmp(get_addr2_ptr(ptr), adapter->mppriv.mac_filter, ETH_ALEN) == _FALSE) + ret = _FAIL; + + return ret; +} + +static sint MPwlanhdr_to_ethhdr(union recv_frame *precvframe) +{ + sint rmv_len; + u16 len; + u8 mcastheadermac[] = {0x01, 0x00, 0x5e}; + sint ret = _SUCCESS; + _adapter *adapter = precvframe->u.hdr.adapter; + + u8 *ptr = get_recvframe_data(precvframe) ; /* point to frame_ctrl field */ + struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib; + enum rtw_rx_llc_hdl llc_hdl; + + + if (pattrib->encrypt) + recvframe_pull_tail(precvframe, pattrib->icv_len); + + llc_hdl = rtw_recv_llc_parse(ptr + pattrib->hdrlen + pattrib->iv_len + , precvframe->u.hdr.len - pattrib->hdrlen - pattrib->iv_len); + + rmv_len = pattrib->hdrlen + pattrib->iv_len + (llc_hdl ? SNAP_SIZE : 0); + len = precvframe->u.hdr.len - rmv_len; + + ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + (llc_hdl ? 2 : 0))); + + _rtw_memcpy(ptr, pattrib->dst, ETH_ALEN); + _rtw_memcpy(ptr + ETH_ALEN, pattrib->src, ETH_ALEN); + + if (!llc_hdl) { + len = htons(len); + _rtw_memcpy(ptr + 12, &len, 2); + } + + + len = htons(pattrib->seq_num); + /* RTW_INFO("wlan seq = %d ,seq_num =%x\n",len,pattrib->seq_num); */ + _rtw_memcpy(ptr + 12, &len, 2); + if (adapter->mppriv.bRTWSmbCfg == _TRUE) { + /* if(_rtw_memcmp(mcastheadermac, pattrib->dst, 3) == _TRUE) */ /* SimpleConfig Dest. */ + /* _rtw_memcpy(ptr+ETH_ALEN, pattrib->bssid, ETH_ALEN); */ + + if (_rtw_memcmp(mcastheadermac, pattrib->bssid, 3) == _TRUE) /* SimpleConfig Dest. */ + _rtw_memcpy(ptr, pattrib->bssid, ETH_ALEN); + + } + + + return ret; + +} + + +int mp_recv_frame(_adapter *padapter, union recv_frame *rframe) +{ + int ret = _SUCCESS; + struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib; + _queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; +#ifdef CONFIG_MP_INCLUDED + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mp_priv *pmppriv = &padapter->mppriv; +#endif /* CONFIG_MP_INCLUDED */ + u8 type; + u8 *ptr = rframe->u.hdr.rx_data; + u8 *psa, *pda, *pbssid; + struct sta_info *psta = NULL; + DBG_COUNTER(padapter->rx_logs.core_rx_pre); + + if ((check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE)) { /* &&(padapter->mppriv.check_mp_pkt == 0)) */ + if (pattrib->crc_err == 1) + padapter->mppriv.rx_crcerrpktcount++; + else { + if (_SUCCESS == validate_mp_recv_frame(padapter, rframe)) + padapter->mppriv.rx_pktcount++; + else + padapter->mppriv.rx_pktcount_filter_out++; + } + + if (pmppriv->rx_bindicatePkt == _FALSE) { + ret = _FAIL; + rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */ + goto exit; + } else { + type = GetFrameType(ptr); + pattrib->to_fr_ds = get_tofr_ds(ptr); + pattrib->frag_num = GetFragNum(ptr); + pattrib->seq_num = GetSequence(ptr); + pattrib->pw_save = GetPwrMgt(ptr); + pattrib->mfrag = GetMFrag(ptr); + pattrib->mdata = GetMData(ptr); + pattrib->privacy = GetPrivacy(ptr); + pattrib->order = GetOrder(ptr); + + if (type == WIFI_DATA_TYPE) { + pda = get_da(ptr); + psa = get_sa(ptr); + pbssid = get_hdr_bssid(ptr); + + _rtw_memcpy(pattrib->dst, pda, ETH_ALEN); + _rtw_memcpy(pattrib->src, psa, ETH_ALEN); + _rtw_memcpy(pattrib->bssid, pbssid, ETH_ALEN); + + switch (pattrib->to_fr_ds) { + case 0: + _rtw_memcpy(pattrib->ra, pda, ETH_ALEN); + _rtw_memcpy(pattrib->ta, psa, ETH_ALEN); + ret = sta2sta_data_frame(padapter, rframe, &psta); + break; + case 1: + _rtw_memcpy(pattrib->ra, pbssid, ETH_ALEN); + _rtw_memcpy(pattrib->ta, psa, ETH_ALEN); + ret = sta2ap_data_frame(padapter, rframe, &psta); + break; + case 2: + _rtw_memcpy(pattrib->ra, pda, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pbssid, ETH_ALEN); + ret = ap2sta_data_frame(padapter, rframe, &psta); + break; + case 3: + _rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN); + _rtw_memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN); + ret = _FAIL; + break; + default: + ret = _FAIL; + break; + } + + if (ret != _SUCCESS) { +#ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" 2_data_frame fail: drop pkt\n" + , FUNC_ADPT_ARG(padapter)); +#endif + ret = _FAIL; + goto exit; + } + + ret = MPwlanhdr_to_ethhdr(rframe); + + if (ret != _SUCCESS) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" wlanhdr_to_ethhdr: drop pkt\n" + , FUNC_ADPT_ARG(padapter)); + #endif + ret = _FAIL; + goto exit; + } + if (!RTW_CANNOT_RUN(padapter)) { + /* indicate this recv_frame */ + ret = rtw_recv_indicatepkt(padapter, rframe); + if (ret != _SUCCESS) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" rtw_recv_indicatepkt fail!\n" + , FUNC_ADPT_ARG(padapter)); + #endif + ret = _FAIL; + goto exit; + } + } else { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" bDriverStopped(%s) OR bSurpriseRemoved(%s)\n" + , FUNC_ADPT_ARG(padapter) + , rtw_is_drv_stopped(padapter) ? "True" : "False" + , rtw_is_surprise_removed(padapter) ? "True" : "False"); + #endif + ret = _FAIL; + goto exit; + } + + } + } + } +exit: + rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */ + ret = _FAIL; + return ret; + +} +#endif + + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)) +int recv_frame_monitor(_adapter *padapter, union recv_frame *rframe) +{ + int ret = _SUCCESS; + _queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + +#ifdef CONFIG_WIFI_MONITOR + struct net_device *ndev = padapter->pnetdev; + _pkt *pskb = NULL; + + if (rframe == NULL) + goto exit; + + /* read skb information from recv frame */ + pskb = rframe->u.hdr.pkt; + pskb->len = rframe->u.hdr.len; + pskb->data = rframe->u.hdr.rx_data; + skb_set_tail_pointer(pskb, rframe->u.hdr.len); + + if (ndev->type == ARPHRD_IEEE80211_RADIOTAP) { + /* fill radiotap header */ + if (rtw_fill_radiotap_hdr(padapter, &rframe->u.hdr.attrib, (u8 *)pskb) == _FAIL) { + ret = _FAIL; + goto exit; + } + } + + /* write skb information to recv frame */ + skb_reset_mac_header(pskb); + rframe->u.hdr.len = pskb->len; + rframe->u.hdr.rx_data = pskb->data; + rframe->u.hdr.rx_head = pskb->head; + rframe->u.hdr.rx_tail = skb_tail_pointer(pskb); + rframe->u.hdr.rx_end = skb_end_pointer(pskb); + + if (!RTW_CANNOT_RUN(padapter)) { + /* indicate this recv_frame */ + ret = rtw_recv_monitor(padapter, rframe); + } else + ret = _FAIL; + +exit: +#endif /* CONFIG_WIFI_MONITOR */ + + if (rframe) /* free this recv_frame */ + rtw_free_recvframe(rframe, pfree_recv_queue); + + return ret; +} +#endif +int recv_func_prehandle(_adapter *padapter, union recv_frame *rframe) +{ + int ret = _SUCCESS; +#ifdef DBG_RX_COUNTER_DUMP + struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib; +#endif + _queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + +#ifdef DBG_RX_COUNTER_DUMP + if (padapter->dump_rx_cnt_mode & DUMP_DRV_RX_COUNTER) { + if (pattrib->crc_err == 1) + padapter->drv_rx_cnt_crcerror++; + else + padapter->drv_rx_cnt_ok++; + } +#endif + +#ifdef CONFIG_MP_INCLUDED + if (padapter->registrypriv.mp_mode == 1 || padapter->mppriv.bRTWSmbCfg == _TRUE) { + mp_recv_frame(padapter, rframe); + ret = _FAIL; + goto exit; + } else +#endif + { + /* check the frame crtl field and decache */ + ret = validate_recv_frame(padapter, rframe); + if (ret != _SUCCESS) { + rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */ + goto exit; + } + } +exit: + return ret; +} + +/*#define DBG_RX_BMC_FRAME*/ +int recv_func_posthandle(_adapter *padapter, union recv_frame *prframe) +{ + int ret = _SUCCESS; + union recv_frame *orig_prframe = prframe; + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; + struct recv_priv *precvpriv = &padapter->recvpriv; + _queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; +#ifdef CONFIG_TDLS + u8 *psnap_type, *pcategory; +#endif /* CONFIG_TDLS */ + + DBG_COUNTER(padapter->rx_logs.core_rx_post); + + prframe = decryptor(padapter, prframe); + if (prframe == NULL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" decryptor: drop pkt\n" + , FUNC_ADPT_ARG(padapter)); + #endif + ret = _FAIL; + DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_err); + goto _recv_data_drop; + } + +#ifdef DBG_RX_BMC_FRAME + if (IS_MCAST(pattrib->ra)) + RTW_INFO("%s =>"ADPT_FMT" Rx BC/MC from "MAC_FMT"\n", __func__, ADPT_ARG(padapter), MAC_ARG(pattrib->ta)); +#endif + +#if 0 + if (is_primary_adapter(padapter)) { + RTW_INFO("+++\n"); + { + int i; + u8 *ptr = get_recvframe_data(prframe); + for (i = 0; i < 140; i = i + 8) + RTW_INFO("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:", *(ptr + i), + *(ptr + i + 1), *(ptr + i + 2) , *(ptr + i + 3) , *(ptr + i + 4), *(ptr + i + 5), *(ptr + i + 6), *(ptr + i + 7)); + + } + RTW_INFO("---\n"); + } +#endif + +#ifdef CONFIG_TDLS + /* check TDLS frame */ + psnap_type = get_recvframe_data(orig_prframe) + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE; + pcategory = psnap_type + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + + if ((_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_TDLS, ETH_TYPE_LEN)) && + ((*pcategory == RTW_WLAN_CATEGORY_TDLS) || (*pcategory == RTW_WLAN_CATEGORY_P2P))) { + ret = OnTDLS(padapter, prframe); + if (ret == _FAIL) + goto _exit_recv_func; + } +#endif /* CONFIG_TDLS */ + + prframe = recvframe_chk_defrag(padapter, prframe); + if (prframe == NULL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" recvframe_chk_defrag: drop pkt\n" + , FUNC_ADPT_ARG(padapter)); + #endif + DBG_COUNTER(padapter->rx_logs.core_rx_post_defrag_err); + goto _recv_data_drop; + } + + prframe = portctrl(padapter, prframe); + if (prframe == NULL) { + #ifdef DBG_RX_DROP_FRAME + RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" portctrl: drop pkt\n" + , FUNC_ADPT_ARG(padapter)); + #endif + ret = _FAIL; + DBG_COUNTER(padapter->rx_logs.core_rx_post_portctrl_err); + goto _recv_data_drop; + } + + count_rx_stats(padapter, prframe, NULL); + +#ifdef CONFIG_WAPI_SUPPORT + rtw_wapi_update_info(padapter, prframe); +#endif + +#if defined(CONFIG_80211N_HT) && defined(CONFIG_RECV_REORDERING_CTRL) + /* including perform A-MPDU Rx Ordering Buffer Control */ + ret = recv_indicatepkt_reorder(padapter, prframe); + if (ret == _FAIL) { + rtw_free_recvframe(orig_prframe, pfree_recv_queue); + goto _recv_data_drop; + } else if (ret == RTW_RX_HANDLED) /* queued OR indicated in order */ + goto _exit_recv_func; +#endif + + recv_set_iseq_before_mpdu_process(prframe, pattrib->seq_num, __func__); + ret = recv_process_mpdu(padapter, prframe); + recv_set_iseq_after_mpdu_process(prframe, pattrib->seq_num, __func__); + if (ret == _FAIL) + goto _recv_data_drop; + +_exit_recv_func: + return ret; + +_recv_data_drop: + precvpriv->dbg_rx_drop_count++; + return ret; +} + +int recv_func(_adapter *padapter, union recv_frame *rframe) +{ + int ret; + struct rx_pkt_attrib *prxattrib = &rframe->u.hdr.attrib; + struct recv_priv *recvpriv = &padapter->recvpriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + u8 *ptr = rframe->u.hdr.rx_data; +#ifdef CONFIG_CUSTOMER_ALIBABA_GENERAL + u8 type; +#endif + + if (check_fwstate(mlmepriv, WIFI_MONITOR_STATE) +#ifdef RTW_SIMPLE_CONFIG + || (check_fwstate(mlmepriv, WIFI_AP_STATE) && padapter->rtw_simple_config == _TRUE && IS_MCAST(get_ra(ptr))) +#endif + ) { +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)) + recv_frame_monitor(padapter, rframe); +#endif + ret = _SUCCESS; + goto exit; + } + +#ifdef CONFIG_CUSTOMER_ALIBABA_GENERAL + type = GetFrameType(ptr); + if ((type == WIFI_DATA_TYPE)&& check_fwstate(mlmepriv, WIFI_STATION_STATE)) { + struct wlan_network *cur_network = &(mlmepriv->cur_network); + if ( _rtw_memcmp(get_addr2_ptr(ptr), cur_network->network.MacAddress, ETH_ALEN)==0) { + recv_frame_monitor(padapter, rframe); + ret = _SUCCESS; + goto exit; + } + } +#endif + /* check if need to handle uc_swdec_pending_queue*/ + if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && psecuritypriv->busetkipkey) { + union recv_frame *pending_frame; + int cnt = 0; + + while ((pending_frame = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue))) { + cnt++; + DBG_COUNTER(padapter->rx_logs.core_rx_dequeue); + recv_func_posthandle(padapter, pending_frame); + } + + if (cnt) + RTW_INFO(FUNC_ADPT_FMT" dequeue %d from uc_swdec_pending_queue\n", + FUNC_ADPT_ARG(padapter), cnt); + } + + DBG_COUNTER(padapter->rx_logs.core_rx); + ret = recv_func_prehandle(padapter, rframe); + + if (ret == _SUCCESS) { + + /* check if need to enqueue into uc_swdec_pending_queue*/ + if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && + !IS_MCAST(prxattrib->ra) && prxattrib->encrypt > 0 && + (prxattrib->bdecrypted == 0 || psecuritypriv->sw_decrypt == _TRUE) && + psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK && + !psecuritypriv->busetkipkey) { + DBG_COUNTER(padapter->rx_logs.core_rx_enqueue); + rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue); + /* RTW_INFO("%s: no key, enqueue uc_swdec_pending_queue\n", __func__); */ + + if (recvpriv->free_recvframe_cnt < NR_RECVFRAME / 4) { + /* to prevent from recvframe starvation, get recvframe from uc_swdec_pending_queue to free_recvframe_cnt */ + rframe = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue); + if (rframe) + goto do_posthandle; + } + goto exit; + } + +do_posthandle: + ret = recv_func_posthandle(padapter, rframe); + } + +exit: + return ret; +} + + +s32 rtw_recv_entry(union recv_frame *precvframe) +{ + _adapter *padapter; + struct recv_priv *precvpriv; + s32 ret = _SUCCESS; + + + + padapter = precvframe->u.hdr.adapter; + + precvpriv = &padapter->recvpriv; + + + ret = recv_func(padapter, precvframe); + if (ret == _FAIL) { + goto _recv_entry_drop; + } + + + precvpriv->rx_pkts++; + + + return ret; + +_recv_entry_drop: + +#ifdef CONFIG_MP_INCLUDED + if (padapter->registrypriv.mp_mode == 1) + padapter->mppriv.rx_pktloss = precvpriv->rx_drop; +#endif + + + + return ret; +} + +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS +static void rtw_signal_stat_timer_hdl(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; + struct recv_priv *recvpriv = &adapter->recvpriv; + + u32 tmp_s, tmp_q; + u8 avg_signal_strength = 0; + u8 avg_signal_qual = 0; + u32 num_signal_strength = 0; + u32 num_signal_qual = 0; + u8 ratio_pre_stat = 0, ratio_curr_stat = 0, ratio_total = 0, ratio_profile = SIGNAL_STAT_CALC_PROFILE_0; + + if (adapter->recvpriv.is_signal_dbg) { + /* update the user specific value, signal_strength_dbg, to signal_strength, rssi */ + adapter->recvpriv.signal_strength = adapter->recvpriv.signal_strength_dbg; + adapter->recvpriv.rssi = (s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg); + } else { + + if (recvpriv->signal_strength_data.update_req == 0) { /* update_req is clear, means we got rx */ + avg_signal_strength = recvpriv->signal_strength_data.avg_val; + num_signal_strength = recvpriv->signal_strength_data.total_num; + /* after avg_vals are accquired, we can re-stat the signal values */ + recvpriv->signal_strength_data.update_req = 1; + } + + if (recvpriv->signal_qual_data.update_req == 0) { /* update_req is clear, means we got rx */ + avg_signal_qual = recvpriv->signal_qual_data.avg_val; + num_signal_qual = recvpriv->signal_qual_data.total_num; + /* after avg_vals are accquired, we can re-stat the signal values */ + recvpriv->signal_qual_data.update_req = 1; + } + + if (num_signal_strength == 0) { + if (rtw_get_on_cur_ch_time(adapter) == 0 + || rtw_get_passing_time_ms(rtw_get_on_cur_ch_time(adapter)) < 2 * adapter->mlmeextpriv.mlmext_info.bcn_interval + ) + goto set_timer; + } + + if (check_fwstate(&adapter->mlmepriv, WIFI_UNDER_SURVEY) == _TRUE + || check_fwstate(&adapter->mlmepriv, WIFI_ASOC_STATE) == _FALSE + ) + goto set_timer; + +#ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_buddy_check_fwstate(adapter, WIFI_UNDER_SURVEY) == _TRUE) + goto set_timer; +#endif + if (adapter->registrypriv.mp_mode == 1) + ratio_profile = SIGNAL_STAT_CALC_PROFILE_2; + else if (RTW_SIGNAL_STATE_CALC_PROFILE < SIGNAL_STAT_CALC_PROFILE_MAX) + ratio_profile = RTW_SIGNAL_STATE_CALC_PROFILE; + + ratio_pre_stat = signal_stat_calc_profile[ratio_profile][0]; + ratio_curr_stat = signal_stat_calc_profile[ratio_profile][1]; + ratio_total = ratio_pre_stat + ratio_curr_stat; + + /* update value of signal_strength, rssi, signal_qual */ + tmp_s = (ratio_curr_stat * avg_signal_strength + ratio_pre_stat * recvpriv->signal_strength); + if (tmp_s % ratio_total) + tmp_s = tmp_s / ratio_total + 1; + else + tmp_s = tmp_s / ratio_total; + if (tmp_s > 100) + tmp_s = 100; + + tmp_q = (ratio_curr_stat * avg_signal_qual + ratio_pre_stat * recvpriv->signal_qual); + if (tmp_q % ratio_total) + tmp_q = tmp_q / ratio_total + 1; + else + tmp_q = tmp_q / ratio_total; + if (tmp_q > 100) + tmp_q = 100; + + recvpriv->signal_strength = tmp_s; + recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s); + recvpriv->signal_qual = tmp_q; + +#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1 + RTW_INFO(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u" + ", num_signal_strength:%u, num_signal_qual:%u" + ", on_cur_ch_ms:%d" + "\n" + , FUNC_ADPT_ARG(adapter) + , recvpriv->signal_strength + , recvpriv->rssi + , recvpriv->signal_qual + , num_signal_strength, num_signal_qual + , rtw_get_on_cur_ch_time(adapter) ? rtw_get_passing_time_ms(rtw_get_on_cur_ch_time(adapter)) : 0 + ); +#endif + } + +set_timer: + rtw_set_signal_stat_timer(recvpriv); + +} +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + +static void rx_process_rssi(_adapter *padapter, union recv_frame *prframe) +{ + struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + struct signal_stat *signal_stat = &padapter->recvpriv.signal_strength_data; +#else /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + u32 last_rssi, tmp_val; +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + + /* RTW_INFO("process_rssi=> pattrib->rssil(%d) signal_strength(%d)\n ",pattrib->recv_signal_power,pattrib->signal_strength); */ + /* if(pRfd->Status.bPacketToSelf || pRfd->Status.bPacketBeacon) */ + { +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + if (signal_stat->update_req) { + signal_stat->total_num = 0; + signal_stat->total_val = 0; + signal_stat->update_req = 0; + } + + signal_stat->total_num++; + signal_stat->total_val += pattrib->phy_info.signal_strength; + signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num; +#else /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + + /* Adapter->RxStats.RssiCalculateCnt++; */ /* For antenna Test */ + if (padapter->recvpriv.signal_strength_data.total_num++ >= PHY_RSSI_SLID_WIN_MAX) { + padapter->recvpriv.signal_strength_data.total_num = PHY_RSSI_SLID_WIN_MAX; + last_rssi = padapter->recvpriv.signal_strength_data.elements[padapter->recvpriv.signal_strength_data.index]; + padapter->recvpriv.signal_strength_data.total_val -= last_rssi; + } + padapter->recvpriv.signal_strength_data.total_val += pattrib->phy_info.signal_strength; + + padapter->recvpriv.signal_strength_data.elements[padapter->recvpriv.signal_strength_data.index++] = pattrib->phy_info.signal_strength; + if (padapter->recvpriv.signal_strength_data.index >= PHY_RSSI_SLID_WIN_MAX) + padapter->recvpriv.signal_strength_data.index = 0; + + + tmp_val = padapter->recvpriv.signal_strength_data.total_val / padapter->recvpriv.signal_strength_data.total_num; + + if (padapter->recvpriv.is_signal_dbg) { + padapter->recvpriv.signal_strength = padapter->recvpriv.signal_strength_dbg; + padapter->recvpriv.rssi = (s8)translate_percentage_to_dbm(padapter->recvpriv.signal_strength_dbg); + } else { + padapter->recvpriv.signal_strength = tmp_val; + padapter->recvpriv.rssi = (s8)translate_percentage_to_dbm(tmp_val); + } + +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + } +} + +static void rx_process_link_qual(_adapter *padapter, union recv_frame *prframe) +{ + struct rx_pkt_attrib *pattrib; +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + struct signal_stat *signal_stat; +#else /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + u32 last_evm = 0, tmpVal; +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + + if (prframe == NULL || padapter == NULL) + return; + + pattrib = &prframe->u.hdr.attrib; +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + signal_stat = &padapter->recvpriv.signal_qual_data; +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + + /* RTW_INFO("process_link_qual=> pattrib->signal_qual(%d)\n ",pattrib->signal_qual); */ + +#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS + if (signal_stat->update_req) { + signal_stat->total_num = 0; + signal_stat->total_val = 0; + signal_stat->update_req = 0; + } + + signal_stat->total_num++; + signal_stat->total_val += pattrib->phy_info.signal_quality; + signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num; + +#else /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ + if (pattrib->phy_info.signal_quality != 0) { + /* */ + /* 1. Record the general EVM to the sliding window. */ + /* */ + if (padapter->recvpriv.signal_qual_data.total_num++ >= PHY_LINKQUALITY_SLID_WIN_MAX) { + padapter->recvpriv.signal_qual_data.total_num = PHY_LINKQUALITY_SLID_WIN_MAX; + last_evm = padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index]; + padapter->recvpriv.signal_qual_data.total_val -= last_evm; + } + padapter->recvpriv.signal_qual_data.total_val += pattrib->phy_info.signal_quality; + + padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index++] = pattrib->phy_info.signal_quality; + if (padapter->recvpriv.signal_qual_data.index >= PHY_LINKQUALITY_SLID_WIN_MAX) + padapter->recvpriv.signal_qual_data.index = 0; + + + /* <1> Showed on UI for user, in percentage. */ + tmpVal = padapter->recvpriv.signal_qual_data.total_val / padapter->recvpriv.signal_qual_data.total_num; + padapter->recvpriv.signal_qual = (u8)tmpVal; + + } +#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */ +} + +void rx_process_phy_info(_adapter *padapter, union recv_frame *rframe) +{ + /* Check RSSI */ + rx_process_rssi(padapter, rframe); + + /* Check PWDB */ + /* process_PWDB(padapter, rframe); */ + + /* UpdateRxSignalStatistics8192C(Adapter, pRfd); */ + + /* Check EVM */ + rx_process_link_qual(padapter, rframe); + rtw_store_phy_info(padapter, rframe); +} + +void rx_query_phy_status( + union recv_frame *precvframe, + u8 *pphy_status) +{ + PADAPTER padapter = precvframe->u.hdr.adapter; + struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib; + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct phydm_phyinfo_struct *p_phy_info = &pattrib->phy_info; + u8 *wlanhdr; + struct phydm_perpkt_info_struct pkt_info; + u8 *ta, *ra; + u8 is_ra_bmc; + struct sta_priv *pstapriv; + struct sta_info *psta = NULL; + struct recv_priv *precvpriv = &padapter->recvpriv; + /* _irqL irqL; */ + + pkt_info.is_packet_match_bssid = _FALSE; + pkt_info.is_packet_to_self = _FALSE; + pkt_info.is_packet_beacon = _FALSE; + pkt_info.ppdu_cnt = pattrib->ppdu_cnt; + pkt_info.station_id = 0xFF; + + wlanhdr = get_recvframe_data(precvframe); + + ta = get_ta(wlanhdr); + ra = get_ra(wlanhdr); + is_ra_bmc = IS_MCAST(ra); + + if (_rtw_memcmp(adapter_mac_addr(padapter), ta, ETH_ALEN) == _TRUE) { + static systime start_time = 0; + +#if 0 /*For debug */ + if (IsFrameTypeCtrl(wlanhdr)) { + RTW_INFO("-->Control frame: Y\n"); + RTW_INFO("-->pkt_len: %d\n", pattrib->pkt_len); + RTW_INFO("-->Sub Type = 0x%X\n", get_frame_sub_type(wlanhdr)); + } + + /* Dump first 40 bytes of header */ + int i = 0; + + for (i = 0; i < 40; i++) + RTW_INFO("%d: %X\n", i, *((u8 *)wlanhdr + i)); + + RTW_INFO("\n"); +#endif + + if ((start_time == 0) || (rtw_get_passing_time_ms(start_time) > 5000)) { + RTW_PRINT("Warning!!! %s: Confilc mac addr!!\n", __func__); + start_time = rtw_get_current_time(); + } + precvpriv->dbg_rx_conflic_mac_addr_cnt++; + } else { + pstapriv = &padapter->stapriv; + psta = rtw_get_stainfo(pstapriv, ta); + if (psta) + pkt_info.station_id = psta->cmn.mac_id; + } + + pkt_info.is_packet_match_bssid = (!IsFrameTypeCtrl(wlanhdr)) + && (!pattrib->icv_err) && (!pattrib->crc_err) + && ((!MLME_IS_MESH(padapter) && _rtw_memcmp(get_hdr_bssid(wlanhdr), get_bssid(&padapter->mlmepriv), ETH_ALEN)) + || (MLME_IS_MESH(padapter) && psta)); + + pkt_info.is_to_self = (!pattrib->icv_err) && (!pattrib->crc_err) + && _rtw_memcmp(ra, adapter_mac_addr(padapter), ETH_ALEN); + + pkt_info.is_packet_to_self = pkt_info.is_packet_match_bssid + && _rtw_memcmp(ra, adapter_mac_addr(padapter), ETH_ALEN); + + pkt_info.is_packet_beacon = pkt_info.is_packet_match_bssid + && (get_frame_sub_type(wlanhdr) == WIFI_BEACON); + + if (psta && IsFrameTypeData(wlanhdr) + && !(get_frame_sub_type(wlanhdr) & BIT(6)) /* don't count NULL data */ + ) { + if (is_ra_bmc) + psta->curr_rx_rate_bmc = pattrib->data_rate; + else + psta->curr_rx_rate = pattrib->data_rate; + } + pkt_info.data_rate = pattrib->data_rate; + + odm_phy_status_query(&pHalData->odmpriv, p_phy_info, pphy_status, &pkt_info); + + /* If bw is initial value, get from phy status */ + if (pattrib->bw == CHANNEL_WIDTH_MAX) + pattrib->bw = p_phy_info->band_width; + + if (p_phy_info->physts_rpt_valid == _TRUE) { + precvframe->u.hdr.psta = NULL; + if (padapter->registrypriv.mp_mode != 1) { + if ((!MLME_IS_MESH(padapter) && pkt_info.is_packet_match_bssid) + || (MLME_IS_MESH(padapter) && psta)) { + if (psta) { + precvframe->u.hdr.psta = psta; + rx_process_phy_info(padapter, precvframe); + } + } else if (pkt_info.is_packet_to_self || pkt_info.is_packet_beacon) { + if (psta) + precvframe->u.hdr.psta = psta; + rx_process_phy_info(padapter, precvframe); + } + } else { +#ifdef CONFIG_MP_INCLUDED + if (padapter->mppriv.brx_filter_beacon == _TRUE) { + if (pkt_info.is_packet_beacon) { + RTW_INFO("in MP Rx is_packet_beacon\n"); + if (psta) + precvframe->u.hdr.psta = psta; + rx_process_phy_info(padapter, precvframe); + } + } else +#endif + { + if (psta) + precvframe->u.hdr.psta = psta; + rx_process_phy_info(padapter, precvframe); + } + } + } + + rtw_odm_parse_rx_phy_status_chinfo(precvframe, pphy_status); +} +/* +* Increase and check if the continual_no_rx_packet of this @param pmlmepriv is larger than MAX_CONTINUAL_NORXPACKET_COUNT +* @return _TRUE: +* @return _FALSE: +*/ +int rtw_inc_and_chk_continual_no_rx_packet(struct sta_info *sta, int tid_index) +{ + + int ret = _FALSE; + int value = ATOMIC_INC_RETURN(&sta->continual_no_rx_packet[tid_index]); + + if (value >= MAX_CONTINUAL_NORXPACKET_COUNT) + ret = _TRUE; + + return ret; +} + +/* +* Set the continual_no_rx_packet of this @param pmlmepriv to 0 +*/ +void rtw_reset_continual_no_rx_packet(struct sta_info *sta, int tid_index) +{ + ATOMIC_SET(&sta->continual_no_rx_packet[tid_index], 0); +} + +u8 adapter_allow_bmc_data_rx(_adapter *adapter) +{ + if (check_fwstate(&adapter->mlmepriv, WIFI_MONITOR_STATE | WIFI_MP_STATE) == _TRUE) + return 1; + +#ifdef RTW_SIMPLE_CONFIG + /* allow AP to receive multicast packet for RtwSimpleConfigV4 */ + if (MLME_IS_AP(adapter) && adapter->rtw_simple_config) + return 1; +#endif + + if (MLME_IS_AP(adapter)) + return 0; + + if (rtw_linked_check(adapter) == _FALSE) + return 0; + + return 1; +} + +s32 pre_recv_entry(union recv_frame *precvframe, u8 *pphy_status) +{ + s32 ret = _SUCCESS; + u8 *pbuf = precvframe->u.hdr.rx_data; + u8 *ra = get_ra(pbuf); + u8 ra_is_bmc = IS_MCAST(ra); + bool phy_queried = 0; + _adapter *primary_padapter = precvframe->u.hdr.adapter; + _adapter *iface = NULL; + +#ifdef CONFIG_MP_INCLUDED + if (rtw_mp_mode_check(primary_padapter)) + goto query_phy_status; +#endif +#ifdef CONFIG_WIFI_MONITOR + if (MLME_IS_MONITOR(primary_padapter)) + goto query_phy_status; +#endif + + if (ra_is_bmc == _FALSE) { + /* UC frame */ + iface = rtw_get_iface_by_macddr(primary_padapter , ra); + if (!iface) { + #if defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) + if (_rtw_memcmp(ra, adapter_pno_mac_addr(primary_padapter), ETH_ALEN)) + goto query_phy_status; + #endif + + #ifdef CONFIG_RTW_MULTI_AP + /* unasoc STA RCPI */ + if (rtw_unassoc_sta_src_chk(primary_padapter, UNASOC_STA_SRC_RX_NMY_UC)) { + if (pphy_status) { + rx_query_phy_status(precvframe, pphy_status); + rtw_rx_add_unassoc_sta(primary_padapter, UNASOC_STA_SRC_RX_NMY_UC, get_ta(pbuf) + , precvframe->u.hdr.attrib.phy_info.recv_signal_power); + } + } else + #endif + RTW_INFO("%s [WARN] Cannot find appropriate adapter - mac_addr : "MAC_FMT"\n" + , __func__, MAC_ARG(ra)); + + rtw_free_recvframe(precvframe, &precvframe->u.hdr.adapter->recvpriv.free_recv_queue); + goto exit; + } + #ifdef CONFIG_CONCURRENT_MODE + else + precvframe->u.hdr.adapter = iface; + #endif + + } else { + /* BMC frame */ + #ifdef CONFIG_CONCURRENT_MODE + rtw_mi_buddy_clone_bcmc_packet(primary_padapter, precvframe, pphy_status); + #endif + + #ifdef CONFIG_RTW_MULTI_AP + /* unasoc STA RCPI */ + if (pphy_status + && rtw_unassoc_sta_src_chk(primary_padapter, UNASOC_STA_SRC_RX_BMC) + ) { + phy_queried = 1; + rx_query_phy_status(precvframe, pphy_status); + rtw_rx_add_unassoc_sta(primary_padapter, UNASOC_STA_SRC_RX_BMC, get_ta(pbuf) + , precvframe->u.hdr.attrib.phy_info.recv_signal_power); + } + #endif + + /* skip unnecessary BMC data frame for primary adapter */ + if (GetFrameType(pbuf) == WIFI_DATA_TYPE + && !adapter_allow_bmc_data_rx(precvframe->u.hdr.adapter) + ) { + rtw_free_recvframe(precvframe, &precvframe->u.hdr.adapter->recvpriv.free_recv_queue); + goto exit; + } + } +#if defined(CONFIG_MP_INCLUDED) || defined(CONFIG_WIFI_MONITOR) || defined(CONFIG_RTW_CFGVENDOR_RANDOM_MAC_OUI) || defined(CONFIG_RTW_SCAN_RAND) +query_phy_status: +#endif + if (pphy_status) { + if (!phy_queried) + rx_query_phy_status(precvframe, pphy_status); + #ifdef CONFIG_WIFI_MONITOR + if (MLME_IS_MONITOR(primary_padapter)) + rx_query_moinfo(&precvframe->u.hdr.attrib, pphy_status); + #endif + } + + ret = rtw_recv_entry(precvframe); + +exit: + return ret; +} + +#ifdef CONFIG_RECV_THREAD_MODE +thread_return rtw_recv_thread(thread_context context) +{ + _adapter *adapter = (_adapter *)context; + struct recv_priv *recvpriv = &adapter->recvpriv; + s32 err = _SUCCESS; +#ifdef RTW_RECV_THREAD_HIGH_PRIORITY +#ifdef PLATFORM_LINUX + struct sched_param param = { .sched_priority = 1 }; + + sched_setscheduler(current, SCHED_FIFO, ¶m); +#endif /* PLATFORM_LINUX */ +#endif /*RTW_RECV_THREAD_HIGH_PRIORITY*/ + thread_enter("RTW_RECV_THREAD"); + + RTW_INFO(FUNC_ADPT_FMT" enter\n", FUNC_ADPT_ARG(adapter)); + + do { + err = _rtw_down_sema(&recvpriv->recv_sema); + if (_FAIL == err) { + RTW_ERR(FUNC_ADPT_FMT" down recv_sema fail!\n", FUNC_ADPT_ARG(adapter)); + goto exit; + } + + if (RTW_CANNOT_RUN(adapter)) { + RTW_DBG(FUNC_ADPT_FMT "- bDriverStopped(%s) bSurpriseRemoved(%s)\n", + FUNC_ADPT_ARG(adapter), + rtw_is_drv_stopped(adapter) ? "True" : "False", + rtw_is_surprise_removed(adapter) ? "True" : "False"); + goto exit; + } + + err = rtw_hal_recv_hdl(adapter); + + if (err == RTW_RFRAME_UNAVAIL + || err == RTW_RFRAME_PKT_UNAVAIL + ) { + rtw_msleep_os(1); + _rtw_up_sema(&recvpriv->recv_sema); + } + + flush_signals_thread(); + + } while (err != _FAIL); + +exit: + + RTW_INFO(FUNC_ADPT_FMT " Exit\n", FUNC_ADPT_ARG(adapter)); + + rtw_thread_wait_stop(); + + return 0; +} +#endif /* CONFIG_RECV_THREAD_MODE */ + +#if DBG_RX_BH_TRACKING +void rx_bh_tk_set_stage(struct recv_priv *recv, u32 s) +{ + recv->rx_bh_stage = s; +} + +void rx_bh_tk_set_buf(struct recv_priv *recv, void *buf, void *data, u32 dlen) +{ + if (recv->rx_bh_cbuf) + recv->rx_bh_lbuf = recv->rx_bh_cbuf; + recv->rx_bh_cbuf = buf; + if (buf) { + recv->rx_bh_cbuf_data = data; + recv->rx_bh_cbuf_dlen = dlen; + recv->rx_bh_buf_dq_cnt++; + } else { + recv->rx_bh_cbuf_data = NULL; + recv->rx_bh_cbuf_dlen = 0; + } +} + +void rx_bh_tk_set_buf_pos(struct recv_priv *recv, void *pos) +{ + if (recv->rx_bh_cbuf) { + recv->rx_bh_cbuf_pos = pos - recv->rx_bh_cbuf_data; + } else { + rtw_warn_on(1); + recv->rx_bh_cbuf_pos = 0; + } +} + +void rx_bh_tk_set_frame(struct recv_priv *recv, void *frame) +{ + recv->rx_bh_cframe = frame; +} + +void dump_rx_bh_tk(void *sel, struct recv_priv *recv) +{ + RTW_PRINT_SEL(sel, "[RXBHTK]s:%u, buf_dqc:%u, lbuf:%p, cbuf:%p, dlen:%u, pos:%u, cframe:%p\n" + , recv->rx_bh_stage + , recv->rx_bh_buf_dq_cnt + , recv->rx_bh_lbuf + , recv->rx_bh_cbuf + , recv->rx_bh_cbuf_dlen + , recv->rx_bh_cbuf_pos + , recv->rx_bh_cframe + ); +} +#endif /* DBG_RX_BH_TRACKING */ + diff --git a/core/rtw_rf.c b/core/rtw_rf.c new file mode 100644 index 0000000..60c95c7 --- /dev/null +++ b/core/rtw_rf.c @@ -0,0 +1,2437 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_RF_C_ + +#include +#include + +u8 center_ch_2g[CENTER_CH_2G_NUM] = { +/* G00 */1, 2, +/* G01 */3, 4, 5, +/* G02 */6, 7, 8, +/* G03 */9, 10, 11, +/* G04 */12, 13, +/* G05 */14 +}; + +#define ch_to_cch_2g_idx(ch) ((ch) - 1) + +u8 center_ch_2g_40m[CENTER_CH_2G_40M_NUM] = { + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, +}; + +u8 op_chs_of_cch_2g_40m[CENTER_CH_2G_40M_NUM][2] = { + {1, 5}, /* 3 */ + {2, 6}, /* 4 */ + {3, 7}, /* 5 */ + {4, 8}, /* 6 */ + {5, 9}, /* 7 */ + {6, 10}, /* 8 */ + {7, 11}, /* 9 */ + {8, 12}, /* 10 */ + {9, 13}, /* 11 */ +}; + +u8 center_ch_5g_all[CENTER_CH_5G_ALL_NUM] = { +/* G00 */36, 38, 40, + 42, +/* G01 */44, 46, 48, + /* 50, */ +/* G02 */52, 54, 56, + 58, +/* G03 */60, 62, 64, +/* G04 */100, 102, 104, + 106, +/* G05 */108, 110, 112, + /* 114, */ +/* G06 */116, 118, 120, + 122, +/* G07 */124, 126, 128, +/* G08 */132, 134, 136, + 138, +/* G09 */140, 142, 144, +/* G10 */149, 151, 153, + 155, +/* G11 */157, 159, 161, + /* 163, */ +/* G12 */165, 167, 169, + 171, +/* G13 */173, 175, 177 +}; + +u8 center_ch_5g_20m[CENTER_CH_5G_20M_NUM] = { +/* G00 */36, 40, +/* G01 */44, 48, +/* G02 */52, 56, +/* G03 */60, 64, +/* G04 */100, 104, +/* G05 */108, 112, +/* G06 */116, 120, +/* G07 */124, 128, +/* G08 */132, 136, +/* G09 */140, 144, +/* G10 */149, 153, +/* G11 */157, 161, +/* G12 */165, 169, +/* G13 */173, 177 +}; + +#define ch_to_cch_5g_20m_idx(ch) \ + ( \ + ((ch) >= 36 && (ch) <= 64) ? (((ch) - 36) >> 2) : \ + ((ch) >= 100 && (ch) <= 144) ? 8 + (((ch) - 100) >> 2) : \ + ((ch) >= 149 && (ch) <= 177) ? 20 + (((ch) - 149) >> 2) : 255 \ + ) + +u8 center_ch_5g_40m[CENTER_CH_5G_40M_NUM] = { +/* G00 */38, +/* G01 */46, +/* G02 */54, +/* G03 */62, +/* G04 */102, +/* G05 */110, +/* G06 */118, +/* G07 */126, +/* G08 */134, +/* G09 */142, +/* G10 */151, +/* G11 */159, +/* G12 */167, +/* G13 */175 +}; + +u8 center_ch_5g_20m_40m[CENTER_CH_5G_20M_NUM + CENTER_CH_5G_40M_NUM] = { +/* G00 */36, 38, 40, +/* G01 */44, 46, 48, +/* G02 */52, 54, 56, +/* G03 */60, 62, 64, +/* G04 */100, 102, 104, +/* G05 */108, 110, 112, +/* G06 */116, 118, 120, +/* G07 */124, 126, 128, +/* G08 */132, 134, 136, +/* G09 */140, 142, 144, +/* G10 */149, 151, 153, +/* G11 */157, 159, 161, +/* G12 */165, 167, 169, +/* G13 */173, 175, 177 +}; + +u8 op_chs_of_cch_5g_40m[CENTER_CH_5G_40M_NUM][2] = { + {36, 40}, /* 38 */ + {44, 48}, /* 46 */ + {52, 56}, /* 54 */ + {60, 64}, /* 62 */ + {100, 104}, /* 102 */ + {108, 112}, /* 110 */ + {116, 120}, /* 118 */ + {124, 128}, /* 126 */ + {132, 136}, /* 134 */ + {140, 144}, /* 142 */ + {149, 153}, /* 151 */ + {157, 161}, /* 159 */ + {165, 169}, /* 167 */ + {173, 177}, /* 175 */ +}; + +u8 center_ch_5g_80m[CENTER_CH_5G_80M_NUM] = { +/* G00 ~ G01*/42, +/* G02 ~ G03*/58, +/* G04 ~ G05*/106, +/* G06 ~ G07*/122, +/* G08 ~ G09*/138, +/* G10 ~ G11*/155, +/* G12 ~ G13*/171 +}; + +u8 op_chs_of_cch_5g_80m[CENTER_CH_5G_80M_NUM][4] = { + {36, 40, 44, 48}, /* 42 */ + {52, 56, 60, 64}, /* 58 */ + {100, 104, 108, 112}, /* 106 */ + {116, 120, 124, 128}, /* 122 */ + {132, 136, 140, 144}, /* 138 */ + {149, 153, 157, 161}, /* 155 */ + {165, 169, 173, 177}, /* 171 */ +}; + +u8 center_ch_5g_160m[CENTER_CH_5G_160M_NUM] = { +/* G00 ~ G03*/50, +/* G04 ~ G07*/114, +/* G10 ~ G13*/163 +}; + +u8 op_chs_of_cch_5g_160m[CENTER_CH_5G_160M_NUM][8] = { + {36, 40, 44, 48, 52, 56, 60, 64}, /* 50 */ + {100, 104, 108, 112, 116, 120, 124, 128}, /* 114 */ + {149, 153, 157, 161, 165, 169, 173, 177}, /* 163 */ +}; + +struct center_chs_ent_t { + u8 ch_num; + u8 *chs; +}; + +struct center_chs_ent_t center_chs_2g_by_bw[] = { + {CENTER_CH_2G_NUM, center_ch_2g}, + {CENTER_CH_2G_40M_NUM, center_ch_2g_40m}, +}; + +struct center_chs_ent_t center_chs_5g_by_bw[] = { + {CENTER_CH_5G_20M_NUM, center_ch_5g_20m}, + {CENTER_CH_5G_40M_NUM, center_ch_5g_40m}, + {CENTER_CH_5G_80M_NUM, center_ch_5g_80m}, + {CENTER_CH_5G_160M_NUM, center_ch_5g_160m}, +}; + +/* + * Get center channel of smaller bandwidth by @param cch, @param bw, @param offset + * @cch: the given center channel + * @bw: the given bandwidth + * @offset: the given primary SC offset of the given bandwidth + * + * return center channel of smaller bandiwdth if valid, or 0 + */ +u8 rtw_get_scch_by_cch_offset(u8 cch, u8 bw, u8 offset) +{ + u8 t_cch = 0; + + if (bw == CHANNEL_WIDTH_20) { + t_cch = cch; + goto exit; + } + + if (offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) { + rtw_warn_on(1); + goto exit; + } + + /* 2.4G, 40MHz */ + if (cch >= 3 && cch <= 11 && bw == CHANNEL_WIDTH_40) { + t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 2 : cch - 2; + goto exit; + } + + /* 5G, 160MHz */ + if (cch >= 50 && cch <= 163 && bw == CHANNEL_WIDTH_160) { + t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 8 : cch - 8; + goto exit; + + /* 5G, 80MHz */ + } else if (cch >= 42 && cch <= 171 && bw == CHANNEL_WIDTH_80) { + t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 4 : cch - 4; + goto exit; + + /* 5G, 40MHz */ + } else if (cch >= 38 && cch <= 175 && bw == CHANNEL_WIDTH_40) { + t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 2 : cch - 2; + goto exit; + + } else { + rtw_warn_on(1); + goto exit; + } + +exit: + return t_cch; +} + +/* + * Get center channel of smaller bandwidth by @param cch, @param bw, @param opch + * @cch: the given center channel + * @bw: the given bandwidth + * @opch: the given operating channel + * + * return center channel of smaller bandiwdth if valid, or 0 + */ +u8 rtw_get_scch_by_cch_opch(u8 cch, u8 bw, u8 opch) +{ + u8 offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + if (opch > cch) + offset = HAL_PRIME_CHNL_OFFSET_UPPER; + else if (opch < cch) + offset = HAL_PRIME_CHNL_OFFSET_LOWER; + + return rtw_get_scch_by_cch_offset(cch, bw, offset); +} + +struct op_chs_ent_t { + u8 ch_num; + u8 *chs; +}; + +struct op_chs_ent_t op_chs_of_cch_2g_by_bw[] = { + {1, center_ch_2g}, + {2, (u8 *)op_chs_of_cch_2g_40m}, +}; + +struct op_chs_ent_t op_chs_of_cch_5g_by_bw[] = { + {1, center_ch_5g_20m}, + {2, (u8 *)op_chs_of_cch_5g_40m}, + {4, (u8 *)op_chs_of_cch_5g_80m}, + {8, (u8 *)op_chs_of_cch_5g_160m}, +}; + +inline u8 center_chs_2g_num(u8 bw) +{ + if (bw > CHANNEL_WIDTH_40) + return 0; + + return center_chs_2g_by_bw[bw].ch_num; +} + +inline u8 center_chs_2g(u8 bw, u8 id) +{ + if (bw > CHANNEL_WIDTH_40) + return 0; + + if (id >= center_chs_2g_num(bw)) + return 0; + + return center_chs_2g_by_bw[bw].chs[id]; +} + +inline u8 center_chs_5g_num(u8 bw) +{ + if (bw > CHANNEL_WIDTH_160) + return 0; + + return center_chs_5g_by_bw[bw].ch_num; +} + +inline u8 center_chs_5g(u8 bw, u8 id) +{ + if (bw > CHANNEL_WIDTH_160) + return 0; + + if (id >= center_chs_5g_num(bw)) + return 0; + + return center_chs_5g_by_bw[bw].chs[id]; +} + +/* + * Get available op channels by @param cch, @param bw + * @cch: the given center channel + * @bw: the given bandwidth + * @op_chs: the pointer to return pointer of op channel array + * @op_ch_num: the pointer to return pointer of op channel number + * + * return valid (1) or not (0) + */ +u8 rtw_get_op_chs_by_cch_bw(u8 cch, u8 bw, u8 **op_chs, u8 *op_ch_num) +{ + int i; + struct center_chs_ent_t *c_chs_ent = NULL; + struct op_chs_ent_t *op_chs_ent = NULL; + u8 valid = 1; + + if (cch <= 14 + && bw <= CHANNEL_WIDTH_40 + ) { + c_chs_ent = ¢er_chs_2g_by_bw[bw]; + op_chs_ent = &op_chs_of_cch_2g_by_bw[bw]; + } else if (cch >= 36 && cch <= 177 + && bw <= CHANNEL_WIDTH_160 + ) { + c_chs_ent = ¢er_chs_5g_by_bw[bw]; + op_chs_ent = &op_chs_of_cch_5g_by_bw[bw]; + } else { + valid = 0; + goto exit; + } + + for (i = 0; i < c_chs_ent->ch_num; i++) + if (cch == *(c_chs_ent->chs + i)) + break; + + if (i == c_chs_ent->ch_num) { + valid = 0; + goto exit; + } + + *op_chs = op_chs_ent->chs + op_chs_ent->ch_num * i; + *op_ch_num = op_chs_ent->ch_num; + +exit: + return valid; +} + +u8 rtw_get_offset_by_chbw(u8 ch, u8 bw, u8 *r_offset) +{ + u8 valid = 1; + u8 offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + + if (bw == CHANNEL_WIDTH_20) + goto exit; + + if (bw >= CHANNEL_WIDTH_80 && ch <= 14) { + valid = 0; + goto exit; + } + + if (ch >= 1 && ch <= 4) + offset = HAL_PRIME_CHNL_OFFSET_LOWER; + else if (ch >= 5 && ch <= 9) { + if (*r_offset == HAL_PRIME_CHNL_OFFSET_LOWER || *r_offset == HAL_PRIME_CHNL_OFFSET_UPPER) + offset = *r_offset; /* both lower and upper is valid, obey input value */ + else + offset = HAL_PRIME_CHNL_OFFSET_UPPER; /* default use upper */ + } else if (ch >= 10 && ch <= 13) + offset = HAL_PRIME_CHNL_OFFSET_UPPER; + else if (ch == 14) { + valid = 0; /* ch14 doesn't support 40MHz bandwidth */ + goto exit; + } else if (ch >= 36 && ch <= 177) { + switch (ch) { + case 36: + case 44: + case 52: + case 60: + case 100: + case 108: + case 116: + case 124: + case 132: + case 140: + case 149: + case 157: + case 165: + case 173: + offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + case 40: + case 48: + case 56: + case 64: + case 104: + case 112: + case 120: + case 128: + case 136: + case 144: + case 153: + case 161: + case 169: + case 177: + offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + default: + valid = 0; + break; + } + } else + valid = 0; + +exit: + if (valid && r_offset) + *r_offset = offset; + return valid; +} + +u8 rtw_get_center_ch(u8 ch, u8 bw, u8 offset) +{ + u8 cch = ch; + + if (bw == CHANNEL_WIDTH_160) { + if (ch % 4 == 0) { + if (ch >= 36 && ch <= 64) + cch = 50; + else if (ch >= 100 && ch <= 128) + cch = 114; + } else if (ch % 4 == 1) { + if (ch >= 149 && ch <= 177) + cch = 163; + } + + } else if (bw == CHANNEL_WIDTH_80) { + if (ch <= 14) + cch = 7; /* special case for 2.4G */ + else if (ch % 4 == 0) { + if (ch >= 36 && ch <= 48) + cch = 42; + else if (ch >= 52 && ch <= 64) + cch = 58; + else if (ch >= 100 && ch <= 112) + cch = 106; + else if (ch >= 116 && ch <= 128) + cch = 122; + else if (ch >= 132 && ch <= 144) + cch = 138; + } else if (ch % 4 == 1) { + if (ch >= 149 && ch <= 161) + cch = 155; + else if (ch >= 165 && ch <= 177) + cch = 171; + } + + } else if (bw == CHANNEL_WIDTH_40) { + if (offset == HAL_PRIME_CHNL_OFFSET_LOWER) + cch = ch + 2; + else if (offset == HAL_PRIME_CHNL_OFFSET_UPPER) + cch = ch - 2; + + } else if (bw == CHANNEL_WIDTH_20 + || bw == CHANNEL_WIDTH_10 + || bw == CHANNEL_WIDTH_5 + ) + ; /* same as ch */ + else + rtw_warn_on(1); + + return cch; +} + +u8 rtw_get_ch_group(u8 ch, u8 *group, u8 *cck_group) +{ + BAND_TYPE band = BAND_MAX; + s8 gp = -1, cck_gp = -1; + + if (ch <= 14) { + band = BAND_ON_2_4G; + + if (1 <= ch && ch <= 2) + gp = 0; + else if (3 <= ch && ch <= 5) + gp = 1; + else if (6 <= ch && ch <= 8) + gp = 2; + else if (9 <= ch && ch <= 11) + gp = 3; + else if (12 <= ch && ch <= 14) + gp = 4; + else + band = BAND_MAX; + + if (ch == 14) + cck_gp = 5; + else + cck_gp = gp; + } else { + band = BAND_ON_5G; + + if (36 <= ch && ch <= 42) + gp = 0; + else if (44 <= ch && ch <= 48) + gp = 1; + else if (50 <= ch && ch <= 58) + gp = 2; + else if (60 <= ch && ch <= 64) + gp = 3; + else if (100 <= ch && ch <= 106) + gp = 4; + else if (108 <= ch && ch <= 114) + gp = 5; + else if (116 <= ch && ch <= 122) + gp = 6; + else if (124 <= ch && ch <= 130) + gp = 7; + else if (132 <= ch && ch <= 138) + gp = 8; + else if (140 <= ch && ch <= 144) + gp = 9; + else if (149 <= ch && ch <= 155) + gp = 10; + else if (157 <= ch && ch <= 161) + gp = 11; + else if (165 <= ch && ch <= 171) + gp = 12; + else if (173 <= ch && ch <= 177) + gp = 13; + else + band = BAND_MAX; + } + + if (band == BAND_MAX + || (band == BAND_ON_2_4G && cck_gp == -1) + || gp == -1 + ) { + RTW_WARN("%s invalid channel:%u", __func__, ch); + rtw_warn_on(1); + goto exit; + } + + if (group) + *group = gp; + if (cck_group && band == BAND_ON_2_4G) + *cck_group = cck_gp; + +exit: + return band; +} + +int rtw_ch2freq(int chan) +{ + /* see 802.11 17.3.8.3.2 and Annex J + * there are overlapping channel numbers in 5GHz and 2GHz bands */ + + /* + * RTK: don't consider the overlapping channel numbers: 5G channel <= 14, + * because we don't support it. simply judge from channel number + */ + + if (chan >= 1 && chan <= 14) { + if (chan == 14) + return 2484; + else if (chan < 14) + return 2407 + chan * 5; + } else if (chan >= 36 && chan <= 177) + return 5000 + chan * 5; + + return 0; /* not supported */ +} + +int rtw_freq2ch(int freq) +{ + /* see 802.11 17.3.8.3.2 and Annex J */ + if (freq == 2484) + return 14; + else if (freq < 2484) + return (freq - 2407) / 5; + else if (freq >= 4910 && freq <= 4980) + return (freq - 4000) / 5; + else if (freq <= 45000) /* DMG band lower limit */ + return (freq - 5000) / 5; + else if (freq >= 58320 && freq <= 64800) + return (freq - 56160) / 2160; + else + return 0; +} + +bool rtw_chbw_to_freq_range(u8 ch, u8 bw, u8 offset, u32 *hi, u32 *lo) +{ + u8 c_ch; + u32 freq; + u32 hi_ret = 0, lo_ret = 0; + bool valid = _FALSE; + + if (hi) + *hi = 0; + if (lo) + *lo = 0; + + c_ch = rtw_get_center_ch(ch, bw, offset); + freq = rtw_ch2freq(c_ch); + + if (!freq) { + rtw_warn_on(1); + goto exit; + } + + if (bw == CHANNEL_WIDTH_160) { + hi_ret = freq + 80; + lo_ret = freq - 80; + } else if (bw == CHANNEL_WIDTH_80) { + hi_ret = freq + 40; + lo_ret = freq - 40; + } else if (bw == CHANNEL_WIDTH_40) { + hi_ret = freq + 20; + lo_ret = freq - 20; + } else if (bw == CHANNEL_WIDTH_20) { + hi_ret = freq + 10; + lo_ret = freq - 10; + } else + rtw_warn_on(1); + + if (hi) + *hi = hi_ret; + if (lo) + *lo = lo_ret; + + valid = _TRUE; + +exit: + return valid; +} + +const char *const _ch_width_str[CHANNEL_WIDTH_MAX] = { + [CHANNEL_WIDTH_20] = "20MHz", + [CHANNEL_WIDTH_40] = "40MHz", + [CHANNEL_WIDTH_80] = "80MHz", + [CHANNEL_WIDTH_160] = "160MHz", + [CHANNEL_WIDTH_80_80] = "80_80MHz", + [CHANNEL_WIDTH_5] = "5MHz", + [CHANNEL_WIDTH_10] = "10MHz", +}; + +const u8 _ch_width_to_bw_cap[CHANNEL_WIDTH_MAX] = { + [CHANNEL_WIDTH_20] = BW_CAP_20M, + [CHANNEL_WIDTH_40] = BW_CAP_40M, + [CHANNEL_WIDTH_80] = BW_CAP_80M, + [CHANNEL_WIDTH_160] = BW_CAP_160M, + [CHANNEL_WIDTH_80_80] = BW_CAP_80_80M, + [CHANNEL_WIDTH_5] = BW_CAP_5M, + [CHANNEL_WIDTH_10] = BW_CAP_10M, +}; + +const char *const _band_str[] = { + "2.4G", + "5G", + "BAND_MAX", +}; + +const u8 _band_to_band_cap[] = { + BAND_CAP_2G, + BAND_CAP_5G, + 0, +}; + +const char *const _opc_bw_str[OPC_BW_NUM] = { + "20M ", /* OPC_BW20 */ + "40M+", /* OPC_BW40PLUS */ + "40M-", /* OPC_BW40MINUS */ + "80M ", /* OPC_BW80 */ + "160M ", /* OPC_BW160 */ + "80+80M ", /* OPC_BW80P80 */ +}; + +const u8 _opc_bw_to_ch_width[OPC_BW_NUM] = { + CHANNEL_WIDTH_20, /* OPC_BW20 */ + CHANNEL_WIDTH_40, /* OPC_BW40PLUS */ + CHANNEL_WIDTH_40, /* OPC_BW40MINUS */ + CHANNEL_WIDTH_80, /* OPC_BW80 */ + CHANNEL_WIDTH_160, /* OPC_BW160 */ + CHANNEL_WIDTH_80_80, /* OPC_BW80P80 */ +}; + +/* global operating class database */ + +struct op_class_t { + u8 class_id; + BAND_TYPE band; + enum opc_bw bw; + u8 *len_ch_attr; +}; + +#define OPC_CH_LIST_LEN(_opc) (_opc.len_ch_attr[0]) +#define OPC_CH_LIST_CH(_opc, _i) (_opc.len_ch_attr[_i + 1]) + +#define OP_CLASS_ENT(_class, _band, _bw, _len, arg...) \ + {.class_id = _class, .band = _band, .bw = _bw, .len_ch_attr = (uint8_t[_len + 1]) {_len, ##arg},} + +/* 802.11-2016 Table E-4, partial */ +static const struct op_class_t global_op_class[] = { + /* 2G ch1~13, 20M */ + OP_CLASS_ENT(81, BAND_ON_2_4G, OPC_BW20, 13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13), + /* 2G ch14, 20M */ + OP_CLASS_ENT(82, BAND_ON_2_4G, OPC_BW20, 1, 14), + /* 2G, 40M */ + OP_CLASS_ENT(83, BAND_ON_2_4G, OPC_BW40PLUS, 9, 1, 2, 3, 4, 5, 6, 7, 8, 9), + OP_CLASS_ENT(84, BAND_ON_2_4G, OPC_BW40MINUS, 9, 5, 6, 7, 8, 9, 10, 11, 12, 13), + /* 5G band 1, 20M & 40M */ + OP_CLASS_ENT(115, BAND_ON_5G, OPC_BW20, 4, 36, 40, 44, 48), + OP_CLASS_ENT(116, BAND_ON_5G, OPC_BW40PLUS, 2, 36, 44), + OP_CLASS_ENT(117, BAND_ON_5G, OPC_BW40MINUS, 2, 40, 48), + /* 5G band 2, 20M & 40M */ + OP_CLASS_ENT(118, BAND_ON_5G, OPC_BW20, 4, 52, 56, 60, 64), + OP_CLASS_ENT(119, BAND_ON_5G, OPC_BW40PLUS, 2, 52, 60), + OP_CLASS_ENT(120, BAND_ON_5G, OPC_BW40MINUS, 2, 56, 64), + /* 5G band 3, 20M & 40M */ + OP_CLASS_ENT(121, BAND_ON_5G, OPC_BW20, 12, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144), + OP_CLASS_ENT(122, BAND_ON_5G, OPC_BW40PLUS, 6, 100, 108, 116, 124, 132, 140), + OP_CLASS_ENT(123, BAND_ON_5G, OPC_BW40MINUS, 6, 104, 112, 120, 128, 136, 144), + /* 5G band 4, 20M & 40M */ + OP_CLASS_ENT(124, BAND_ON_5G, OPC_BW20, 4, 149, 153, 157, 161), + OP_CLASS_ENT(125, BAND_ON_5G, OPC_BW20, 6, 149, 153, 157, 161, 165, 169), + OP_CLASS_ENT(126, BAND_ON_5G, OPC_BW40PLUS, 2, 149, 157), + OP_CLASS_ENT(127, BAND_ON_5G, OPC_BW40MINUS, 2, 153, 161), + /* 5G, 80M & 160M */ + OP_CLASS_ENT(128, BAND_ON_5G, OPC_BW80, 24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, 149, 153, 157, 161), + OP_CLASS_ENT(129, BAND_ON_5G, OPC_BW160, 16, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128), + #if 0 /* TODO */ + /* 5G, 80+80M */ + {130, BAND_ON_5G, OPC_BW80P80, 0x0FFFFFF}, + #endif +}; + +static const int global_op_class_num = sizeof(global_op_class) / sizeof(struct op_class_t); + +static const struct op_class_t *get_global_op_class_by_id(u8 gid) +{ + int i; + + for (i = 0; i < global_op_class_num; i++) + if (global_op_class[i].class_id == gid) + break; + + return i < global_op_class_num ? &global_op_class[i] : NULL; +} + +bool is_valid_global_op_class_id(u8 gid) +{ + return get_global_op_class_by_id(gid) ? 1 : 0; +} + +static bool is_valid_global_op_class_ch(const struct op_class_t *opc, u8 ch) +{ + int array_idx; + int i; + + if (opc < global_op_class + || (((u8 *)opc) - ((u8 *)global_op_class)) % sizeof(struct op_class_t) + ) { + RTW_ERR("Invalid opc pointer:%p (global_op_class:%p, sizeof(struct op_class_t):%zu, %zu)\n" + , opc, global_op_class, sizeof(struct op_class_t), (((u8 *)opc) - ((u8 *)global_op_class)) % sizeof(struct op_class_t)); + return 0; + } + + array_idx = (((u8 *)opc) - ((u8 *)global_op_class)) / sizeof(struct op_class_t); + + for (i = 0; i < OPC_CH_LIST_LEN(global_op_class[array_idx]); i++) + if (OPC_CH_LIST_CH(global_op_class[array_idx], i) == ch) + break; + + return i < OPC_CH_LIST_LEN(global_op_class[array_idx]); +} + +static enum opc_bw get_global_opc_bw_by_id(u8 gid) +{ + int i; + + for (i = 0; i < global_op_class_num; i++) + if (global_op_class[i].class_id == gid) + break; + + return i < global_op_class_num ? global_op_class[i].bw : OPC_BW_NUM; +} + +/* -2: logic error, -1: error, 0: is already BW20 */ +s16 get_sub_op_class(u8 gid, u8 ch) +{ + const struct op_class_t *opc = get_global_op_class_by_id(gid); + int i; + enum channel_width bw; + + if (!opc) + return -1; + + if (!is_valid_global_op_class_ch(opc, ch)) { + return -1; + } + + if (opc->bw == OPC_BW20) + return 0; + + bw = opc_bw_to_ch_width(opc->bw); + + for (i = 0; i < global_op_class_num; i++) { + if (bw != opc_bw_to_ch_width(global_op_class[i].bw) + 1) + continue; + if (is_valid_global_op_class_ch(&global_op_class[i], ch)) + break; + } + + return i < global_op_class_num ? global_op_class[i].class_id : -2; +} + +static void dump_op_class_ch_title(void *sel) +{ + RTW_PRINT_SEL(sel, "%-5s %-4s %-7s ch_list\n" + , "class", "band", "bw"); +} + +static void dump_global_op_class_ch_single(void *sel, u8 gid) +{ + u8 i; + char buf[100]; + char *pos = buf; + + for (i = 0; i < OPC_CH_LIST_LEN(global_op_class[gid]); i++) + pos += snprintf(pos, 100 - (pos - buf), " %u", OPC_CH_LIST_CH(global_op_class[gid], i)); + + RTW_PRINT_SEL(sel, "%5u %4s %7s%s\n" + , global_op_class[gid].class_id + , band_str(global_op_class[gid].band) + , opc_bw_str(global_op_class[gid].bw), buf); +} + +#ifdef CONFIG_RTW_DEBUG +static bool dbg_global_op_class_validate(u8 gid) +{ + u8 i; + u8 ch, bw, offset, cch; + bool ret = 1; + + switch (global_op_class[gid].bw) { + case OPC_BW20: + bw = CHANNEL_WIDTH_20; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + case OPC_BW40PLUS: + bw = CHANNEL_WIDTH_40; + offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + case OPC_BW40MINUS: + bw = CHANNEL_WIDTH_40; + offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + case OPC_BW80: + bw = CHANNEL_WIDTH_80; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + case OPC_BW160: + bw = CHANNEL_WIDTH_160; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + case OPC_BW80P80: /* TODO */ + default: + RTW_ERR("%s class:%u unsupported opc_bw:%u\n" + , __func__, global_op_class[gid].class_id, global_op_class[gid].bw); + ret = 0; + goto exit; + } + + for (i = 0; i < OPC_CH_LIST_LEN(global_op_class[gid]); i++) { + u8 *op_chs; + u8 op_ch_num; + u8 k; + + ch = OPC_CH_LIST_CH(global_op_class[gid], i); + cch = rtw_get_center_ch(ch ,bw, offset); + if (!cch) { + RTW_ERR("%s can't get cch from class:%u ch:%u\n" + , __func__, global_op_class[gid].class_id, ch); + ret = 0; + continue; + } + + if (!rtw_get_op_chs_by_cch_bw(cch, bw, &op_chs, &op_ch_num)) { + RTW_ERR("%s can't get op chs from class:%u cch:%u\n" + , __func__, global_op_class[gid].class_id, cch); + ret = 0; + continue; + } + + for (k = 0; k < op_ch_num; k++) { + if (*(op_chs + k) == ch) + break; + } + if (k >= op_ch_num) { + RTW_ERR("%s can't get ch:%u from op_chs class:%u cch:%u\n" + , __func__, ch, global_op_class[i].class_id, cch); + ret = 0; + } + } + +exit: + return ret; +} +#endif /* CONFIG_RTW_DEBUG */ + +void dump_global_op_class(void *sel) +{ + u8 i; + + dump_op_class_ch_title(sel); + + for (i = 0; i < global_op_class_num; i++) + dump_global_op_class_ch_single(sel, i); +} + +u8 rtw_get_op_class_by_chbw(u8 ch, u8 bw, u8 offset) +{ + BAND_TYPE band = BAND_MAX; + int i; + u8 gid = 0; /* invalid */ + + if (rtw_is_2g_ch(ch)) + band = BAND_ON_2_4G; + else if (rtw_is_5g_ch(ch)) + band = BAND_ON_5G; + else + goto exit; + + switch (bw) { + case CHANNEL_WIDTH_20: + case CHANNEL_WIDTH_40: + case CHANNEL_WIDTH_80: + case CHANNEL_WIDTH_160: + #if 0 /* TODO */ + case CHANNEL_WIDTH_80_80: + #endif + break; + default: + goto exit; + } + + for (i = 0; i < global_op_class_num; i++) { + if (band != global_op_class[i].band) + continue; + + if (opc_bw_to_ch_width(global_op_class[i].bw) != bw) + continue; + + if ((global_op_class[i].bw == OPC_BW40PLUS + && offset != HAL_PRIME_CHNL_OFFSET_LOWER) + || (global_op_class[i].bw == OPC_BW40MINUS + && offset != HAL_PRIME_CHNL_OFFSET_UPPER) + ) + continue; + + if (is_valid_global_op_class_ch(&global_op_class[i], ch)) + goto get; + } + +get: + if (i < global_op_class_num) { + #if 0 /* TODO */ + if (bw == CHANNEL_WIDTH_80_80) { + /* search another ch */ + if (!is_valid_global_op_class_ch(&global_op_class[i], ch2)) + goto exit; + } + #endif + + gid = global_op_class[i].class_id; + } + +exit: + return gid; +} + +u8 rtw_get_bw_offset_by_op_class_ch(u8 gid, u8 ch, u8 *bw, u8 *offset) +{ + enum opc_bw opc_bw; + u8 valid = 0; + int i; + + opc_bw = get_global_opc_bw_by_id(gid); + if (opc_bw == OPC_BW_NUM) + goto exit; + + *bw = opc_bw_to_ch_width(opc_bw); + + if (opc_bw == OPC_BW40PLUS) + *offset = HAL_PRIME_CHNL_OFFSET_LOWER; + else if (opc_bw == OPC_BW40MINUS) + *offset = HAL_PRIME_CHNL_OFFSET_UPPER; + + if (rtw_get_offset_by_chbw(ch, *bw, offset)) + valid = 1; + +exit: + return valid; +} + +static struct op_class_pref_t *opc_pref_alloc(u8 class_id) +{ + int i, j; + struct op_class_pref_t *opc_pref = NULL; + + for (i = 0; i < global_op_class_num; i++) + if (global_op_class[i].class_id == class_id) + break; + + if (i >= global_op_class_num) + goto exit; + + opc_pref = rtw_zmalloc(sizeof(*opc_pref)); + if (!opc_pref) + goto exit; + + opc_pref->class_id = global_op_class[i].class_id; + opc_pref->band = global_op_class[i].band; + opc_pref->bw = global_op_class[i].bw; + + for (j = 0; j < OPC_CH_LIST_LEN(global_op_class[i]); j++) { + opc_pref->chs[j].ch = OPC_CH_LIST_CH(global_op_class[i], j); + opc_pref->chs[j].static_non_op = 1; + opc_pref->chs[j].no_ir = 1; + opc_pref->chs[j].max_txpwr = UNSPECIFIED_MBM; + } + opc_pref->ch_num = OPC_CH_LIST_LEN(global_op_class[i]); + +exit: + return opc_pref; +} + +static void opc_pref_free(struct op_class_pref_t *opc_pref) +{ + rtw_mfree(opc_pref, sizeof(*opc_pref)); +} + +int op_class_pref_init(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + struct registry_priv *regsty = adapter_to_regsty(adapter); + u8 bw; + struct op_class_pref_t *opc_pref; + int i; + u8 op_class_num = 0; + u8 band_bmp = 0; + u8 bw_bmp[BAND_MAX] = {0}; + int ret = _FAIL; + + rfctl->spt_op_class_ch = rtw_zmalloc(sizeof(struct op_class_pref_t *) * global_op_class_num); + if (!rfctl->spt_op_class_ch) { + RTW_ERR("%s alloc rfctl->spt_op_class_ch fail\n", __func__); + goto exit; + } + + if (IsSupported24G(regsty->wireless_mode) && hal_chk_band_cap(adapter, BAND_CAP_2G)) + band_bmp |= BAND_CAP_2G; + if (is_supported_5g(regsty->wireless_mode) && hal_chk_band_cap(adapter, BAND_CAP_5G)) + band_bmp |= BAND_CAP_5G; + + bw_bmp[BAND_ON_2_4G] = (ch_width_to_bw_cap(REGSTY_BW_2G(regsty) + 1) - 1) & (GET_HAL_SPEC(adapter)->bw_cap); + bw_bmp[BAND_ON_5G] = (ch_width_to_bw_cap(REGSTY_BW_5G(regsty) + 1) - 1) & (GET_HAL_SPEC(adapter)->bw_cap); + if (!REGSTY_IS_11AC_ENABLE(regsty) + || !is_supported_vht(regsty->wireless_mode) + ) + bw_bmp[BAND_ON_5G] &= ~(BW_CAP_80M | BW_CAP_160M); + + if (0) { + RTW_INFO("REGSTY_BW_2G(regsty):%u\n", REGSTY_BW_2G(regsty)); + RTW_INFO("REGSTY_BW_5G(regsty):%u\n", REGSTY_BW_5G(regsty)); + RTW_INFO("GET_HAL_SPEC(adapter)->bw_cap:0x%x\n", GET_HAL_SPEC(adapter)->bw_cap); + RTW_INFO("band_bmp:0x%x\n", band_bmp); + RTW_INFO("bw_bmp[2G]:0x%x\n", bw_bmp[BAND_ON_2_4G]); + RTW_INFO("bw_bmp[5G]:0x%x\n", bw_bmp[BAND_ON_5G]); + } + + for (i = 0; i < global_op_class_num; i++) { + #ifdef CONFIG_RTW_DEBUG + rtw_warn_on(!dbg_global_op_class_validate(i)); + #endif + + if (!(band_bmp & band_to_band_cap(global_op_class[i].band))) + continue; + + bw = opc_bw_to_ch_width(global_op_class[i].bw); + if (bw == CHANNEL_WIDTH_MAX + || bw == CHANNEL_WIDTH_80_80 /* TODO */ + ) + continue; + + if (!(bw_bmp[global_op_class[i].band] & ch_width_to_bw_cap(bw))) + continue; + + opc_pref = opc_pref_alloc(global_op_class[i].class_id); + if (!opc_pref) { + RTW_ERR("%s opc_pref_alloc(%u) fail\n", __func__, global_op_class[i].class_id); + goto exit; + } + + if (opc_pref->ch_num) { + rfctl->spt_op_class_ch[i] = opc_pref; + op_class_num++; + } else + opc_pref_free(opc_pref); + } + + rfctl->cap_spt_op_class_num = op_class_num; + ret = _SUCCESS; + +exit: + return ret; +} + +void op_class_pref_deinit(_adapter *adapter) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + int i; + + if (!rfctl->spt_op_class_ch) + return; + + for (i = 0; i < global_op_class_num; i++) { + if (rfctl->spt_op_class_ch[i]) { + opc_pref_free(rfctl->spt_op_class_ch[i]); + rfctl->spt_op_class_ch[i] = NULL; + } + } + + rtw_mfree(rfctl->spt_op_class_ch, sizeof(struct op_class_pref_t *) * global_op_class_num); + rfctl->spt_op_class_ch = NULL; +} + +void op_class_pref_apply_regulatory(_adapter *adapter, u8 reason) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + struct registry_priv *regsty = adapter_to_regsty(adapter); + u8 ch, bw, offset, cch; + struct op_class_pref_t *opc_pref; + int i, j; + u8 reg_op_class_num = 0; + u8 op_class_num = 0; + + for (i = 0; i < global_op_class_num; i++) { + if (!rfctl->spt_op_class_ch[i]) + continue; + opc_pref = rfctl->spt_op_class_ch[i]; + + /* reset all channel */ + for (j = 0; opc_pref->chs[j].ch != 0; j++) { + if (reason >= REG_CHANGE) + opc_pref->chs[j].static_non_op = 1; + if (reason != REG_TXPWR_CHANGE) + opc_pref->chs[j].no_ir = 1; + if (reason >= REG_TXPWR_CHANGE) + opc_pref->chs[j].max_txpwr = UNSPECIFIED_MBM; + } + if (reason >= REG_CHANGE) + opc_pref->op_ch_num = 0; + if (reason != REG_TXPWR_CHANGE) + opc_pref->ir_ch_num = 0; + + switch (opc_pref->bw) { + case OPC_BW20: + bw = CHANNEL_WIDTH_20; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + case OPC_BW40PLUS: + bw = CHANNEL_WIDTH_40; + offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + case OPC_BW40MINUS: + bw = CHANNEL_WIDTH_40; + offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + case OPC_BW80: + bw = CHANNEL_WIDTH_80; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + case OPC_BW160: + bw = CHANNEL_WIDTH_160; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + case OPC_BW80P80: /* TODO */ + default: + continue; + } + + if (rfctl->country_ent && !COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent) + && (bw == CHANNEL_WIDTH_80 || bw == CHANNEL_WIDTH_160)) + continue; + + for (j = 0; opc_pref->chs[j].ch != 0; j++) { + u8 *op_chs; + u8 op_ch_num; + u8 k, l; + int chset_idx; + + ch = opc_pref->chs[j].ch; + + if (reason >= REG_TXPWR_CHANGE) + opc_pref->chs[j].max_txpwr = rtw_rfctl_get_reg_max_txpwr_mbm(rfctl, ch, bw, offset, 1); + + if (reason == REG_TXPWR_CHANGE) + continue; + + cch = rtw_get_center_ch(ch ,bw, offset); + if (!cch) + continue; + + if (!rtw_get_op_chs_by_cch_bw(cch, bw, &op_chs, &op_ch_num)) + continue; + + for (k = 0, l = 0; k < op_ch_num; k++) { + chset_idx = rtw_chset_search_ch(chset, *(op_chs + k)); + if (chset_idx == -1) + break; + if (bw >= CHANNEL_WIDTH_40) { + if ((chset[chset_idx].flags & RTW_CHF_NO_HT40U) && k % 2 == 0) + break; + if ((chset[chset_idx].flags & RTW_CHF_NO_HT40L) && k % 2 == 1) + break; + } + if (bw >= CHANNEL_WIDTH_80 && (chset[chset_idx].flags & RTW_CHF_NO_80MHZ)) + break; + if (bw >= CHANNEL_WIDTH_160 && (chset[chset_idx].flags & RTW_CHF_NO_160MHZ)) + break; + if ((chset[chset_idx].flags & RTW_CHF_DFS) && rtw_rfctl_dfs_domain_unknown(rfctl)) + continue; + if (chset[chset_idx].flags & RTW_CHF_NO_IR) + continue; + l++; + } + if (k < op_ch_num) + continue; + + if (reason >= REG_CHANGE) { + opc_pref->chs[j].static_non_op = 0; + opc_pref->op_ch_num++; + } + + if (l >= op_ch_num) { + opc_pref->chs[j].no_ir = 0; + opc_pref->ir_ch_num++; + } + } + + if (opc_pref->op_ch_num) + reg_op_class_num++; + if (opc_pref->ir_ch_num) + op_class_num++; + } + + rfctl->reg_spt_op_class_num = reg_op_class_num; + rfctl->cur_spt_op_class_num = op_class_num; +} + +static void dump_opc_pref_single(void *sel, struct op_class_pref_t *opc_pref, bool show_snon_ocp, bool show_no_ir, bool detail) +{ + u8 i; + u8 ch_num = 0; + char buf[256]; + char *pos = buf; + + if (!show_snon_ocp && !opc_pref->op_ch_num) + return; + if (!show_no_ir && !opc_pref->ir_ch_num) + return; + + for (i = 0; opc_pref->chs[i].ch != 0; i++) { + if ((show_snon_ocp || !opc_pref->chs[i].static_non_op) + && (show_no_ir || !opc_pref->chs[i].no_ir) + ) { + if (detail) + pos += snprintf(pos, 256 - (pos - buf), " %4u", opc_pref->chs[i].ch); + else + pos += snprintf(pos, 256 - (pos - buf), " %u", opc_pref->chs[i].ch); + } + } + + RTW_PRINT_SEL(sel, "%5u %4s %7s%s\n" + , opc_pref->class_id + , band_str(opc_pref->band) + , opc_bw_str(opc_pref->bw), buf); + + if (!detail) + return; + + pos = buf; + for (i = 0; opc_pref->chs[i].ch != 0; i++) { + if ((show_snon_ocp || !opc_pref->chs[i].static_non_op) + && (show_no_ir || !opc_pref->chs[i].no_ir) + ) { + pos += snprintf(pos, 256 - (pos - buf), " %c%c" + , opc_pref->chs[i].no_ir ? ' ' : 'I' + , opc_pref->chs[i].static_non_op ? ' ' : 'E' + ); + } + } + RTW_PRINT_SEL(sel, " %s\n", buf); + + pos = buf; + for (i = 0; opc_pref->chs[i].ch != 0; i++) { + if ((show_snon_ocp || !opc_pref->chs[i].static_non_op) + && (show_no_ir || !opc_pref->chs[i].no_ir) + ) { + if (opc_pref->chs[i].max_txpwr == UNSPECIFIED_MBM) + pos += snprintf(pos, 256 - (pos - buf), " "); + else + pos += snprintf(pos, 256 - (pos - buf), " %4d", opc_pref->chs[i].max_txpwr); + } + } + RTW_PRINT_SEL(sel, " %s\n", buf); +} + +void dump_cap_spt_op_class_ch(void *sel, struct rf_ctl_t *rfctl, bool detail) +{ + u8 i; + + dump_op_class_ch_title(sel); + + for (i = 0; i < global_op_class_num; i++) { + if (!rfctl->spt_op_class_ch[i]) + continue; + dump_opc_pref_single(sel, rfctl->spt_op_class_ch[i], 1, 1, detail); + } + + RTW_PRINT_SEL(sel, "op_class number:%d\n", rfctl->cap_spt_op_class_num); +} + +void dump_reg_spt_op_class_ch(void *sel, struct rf_ctl_t *rfctl, bool detail) +{ + u8 i; + + dump_op_class_ch_title(sel); + + for (i = 0; i < global_op_class_num; i++) { + if (!rfctl->spt_op_class_ch[i]) + continue; + dump_opc_pref_single(sel, rfctl->spt_op_class_ch[i], 0, 1, detail); + } + + RTW_PRINT_SEL(sel, "op_class number:%d\n", rfctl->reg_spt_op_class_num); +} + +void dump_cur_spt_op_class_ch(void *sel, struct rf_ctl_t *rfctl, bool detail) +{ + u8 i; + + dump_op_class_ch_title(sel); + + for (i = 0; i < global_op_class_num; i++) { + if (!rfctl->spt_op_class_ch[i]) + continue; + dump_opc_pref_single(sel, rfctl->spt_op_class_ch[i], 0, 0, detail); + } + + RTW_PRINT_SEL(sel, "op_class number:%d\n", rfctl->cur_spt_op_class_num); +} + +const u8 _rf_type_to_rf_tx_cnt[RF_TYPE_MAX] = { + [RF_1T1R] = 1, + [RF_1T2R] = 1, + [RF_1T3R] = 1, + [RF_1T4R] = 1, + [RF_2T1R] = 2, + [RF_2T2R] = 2, + [RF_2T3R] = 2, + [RF_2T4R] = 2, + [RF_3T1R] = 3, + [RF_3T2R] = 3, + [RF_3T3R] = 3, + [RF_3T4R] = 3, + [RF_4T1R] = 4, + [RF_4T2R] = 4, + [RF_4T3R] = 4, + [RF_4T4R] = 4, +}; + +const u8 _rf_type_to_rf_rx_cnt[RF_TYPE_MAX] = { + [RF_1T1R] = 1, + [RF_1T2R] = 2, + [RF_1T3R] = 3, + [RF_1T4R] = 4, + [RF_2T1R] = 1, + [RF_2T2R] = 2, + [RF_2T3R] = 3, + [RF_2T4R] = 4, + [RF_3T1R] = 1, + [RF_3T2R] = 2, + [RF_3T3R] = 3, + [RF_3T4R] = 4, + [RF_4T1R] = 1, + [RF_4T2R] = 2, + [RF_4T3R] = 3, + [RF_4T4R] = 4, +}; + +const char *const _rf_type_to_rfpath_str[RF_TYPE_MAX] = { + [RF_1T1R] = "RF_1T1R", + [RF_1T2R] = "RF_1T2R", + [RF_1T3R] = "RF_1T3R", + [RF_1T4R] = "RF_1T4R", + [RF_2T1R] = "RF_2T1R", + [RF_2T2R] = "RF_2T2R", + [RF_2T3R] = "RF_2T3R", + [RF_2T4R] = "RF_2T4R", + [RF_3T1R] = "RF_3T1R", + [RF_3T2R] = "RF_3T2R", + [RF_3T3R] = "RF_3T3R", + [RF_3T4R] = "RF_3T4R", + [RF_4T1R] = "RF_4T1R", + [RF_4T2R] = "RF_4T2R", + [RF_4T3R] = "RF_4T3R", + [RF_4T4R] = "RF_4T4R", +}; + +void rf_type_to_default_trx_bmp(enum rf_type rf, enum bb_path *tx, enum bb_path *rx) +{ + u8 tx_num = rf_type_to_rf_tx_cnt(rf); + u8 rx_num = rf_type_to_rf_rx_cnt(rf); + int i; + + *tx = *rx = 0; + + for (i = 0; i < tx_num; i++) + *tx |= BIT(i); + for (i = 0; i < rx_num; i++) + *rx |= BIT(i); +} + +static const u8 _trx_num_to_rf_type[RF_PATH_MAX][RF_PATH_MAX] = { + {RF_1T1R, RF_1T2R, RF_1T3R, RF_1T4R}, + {RF_2T1R, RF_2T2R, RF_2T3R, RF_2T4R}, + {RF_3T1R, RF_3T2R, RF_3T3R, RF_3T4R}, + {RF_4T1R, RF_4T2R, RF_4T3R, RF_4T4R}, +}; + +enum rf_type trx_num_to_rf_type(u8 tx_num, u8 rx_num) +{ + if (tx_num > 0 && tx_num <= RF_PATH_MAX && rx_num > 0 && rx_num <= RF_PATH_MAX) + return _trx_num_to_rf_type[tx_num - 1][rx_num - 1]; + return RF_TYPE_MAX; +} + +enum rf_type trx_bmp_to_rf_type(u8 tx_bmp, u8 rx_bmp) +{ + u8 tx_num = 0; + u8 rx_num = 0; + int i; + + for (i = 0; i < RF_PATH_MAX; i++) { + if (tx_bmp >> i & BIT0) + tx_num++; + if (rx_bmp >> i & BIT0) + rx_num++; + } + + return trx_num_to_rf_type(tx_num, rx_num); +} + +bool rf_type_is_a_in_b(enum rf_type a, enum rf_type b) +{ + return rf_type_to_rf_tx_cnt(a) <= rf_type_to_rf_tx_cnt(b) + && rf_type_to_rf_rx_cnt(a) <= rf_type_to_rf_rx_cnt(b); +} + +static void rtw_path_bmp_limit_from_higher(u8 *bmp, u8 *bmp_bit_cnt, u8 bit_cnt_lmt) +{ + int i; + + for (i = RF_PATH_MAX - 1; *bmp_bit_cnt > bit_cnt_lmt && i >= 0; i--) { + if (*bmp & BIT(i)) { + *bmp &= ~BIT(i); + (*bmp_bit_cnt)--; + } + } +} + +u8 rtw_restrict_trx_path_bmp_by_trx_num_lmt(u8 trx_path_bmp, u8 tx_num_lmt, u8 rx_num_lmt, u8 *tx_num, u8 *rx_num) +{ + u8 bmp_tx = (trx_path_bmp & 0xF0) >> 4; + u8 bmp_rx = trx_path_bmp & 0x0F; + u8 bmp_tx_num = 0, bmp_rx_num = 0; + enum rf_type ret_type = RF_TYPE_MAX; + int i, j; + + for (i = 0; i < RF_PATH_MAX; i++) { + if (bmp_tx & BIT(i)) + bmp_tx_num++; + if (bmp_rx & BIT(i)) + bmp_rx_num++; + } + + /* limit higher bit first according to input type */ + if (tx_num_lmt) + rtw_path_bmp_limit_from_higher(&bmp_tx, &bmp_tx_num, tx_num_lmt); + if (rx_num_lmt) + rtw_path_bmp_limit_from_higher(&bmp_rx, &bmp_rx_num, rx_num_lmt); + + /* search for valid rf_type (larger RX prefer) */ + for (j = bmp_rx_num; j > 0; j--) { + for (i = bmp_tx_num; i > 0; i--) { + ret_type = trx_num_to_rf_type(i, j); + if (RF_TYPE_VALID(ret_type)) { + rtw_path_bmp_limit_from_higher(&bmp_tx, &bmp_tx_num, i); + rtw_path_bmp_limit_from_higher(&bmp_rx, &bmp_rx_num, j); + if (tx_num) + *tx_num = bmp_tx_num; + if (rx_num) + *rx_num = bmp_rx_num; + goto exit; + } + } + } + +exit: + return RF_TYPE_VALID(ret_type) ? ((bmp_tx << 4) | bmp_rx) : 0x00; +} + +u8 rtw_restrict_trx_path_bmp_by_rftype(u8 trx_path_bmp, enum rf_type type, u8 *tx_num, u8 *rx_num) +{ + return rtw_restrict_trx_path_bmp_by_trx_num_lmt(trx_path_bmp + , rf_type_to_rf_tx_cnt(type), rf_type_to_rf_rx_cnt(type), tx_num, rx_num); +} + +/* config to non N-TX value, path with lower index prefer */ +void tx_path_nss_set_default(enum bb_path txpath_nss[], u8 txpath_num_nss[], u8 txpath) +{ + int i, j; + u8 cnt; + + for (i = 4; i > 0; i--) { + cnt = 0; + txpath_nss[i - 1] = 0; + for (j = 0; j < RF_PATH_MAX; j++) { + if (txpath & BIT(j)) { + txpath_nss[i - 1] |= BIT(j); + if (++cnt == i) + break; + } + } + txpath_num_nss[i - 1] = i; + } +} + +/* config to full N-TX value */ +void tx_path_nss_set_full_tx(enum bb_path txpath_nss[], u8 txpath_num_nss[], u8 txpath) +{ + u8 tx_num = 0; + int i; + + for (i = 0; i < RF_PATH_MAX; i++) + if (txpath & BIT(i)) + tx_num++; + + for (i = 4; i > 0; i--) { + txpath_nss[i - 1] = txpath; + txpath_num_nss[i - 1] = tx_num; + } +} + +const char *const _regd_str[] = { + "NONE", + "FCC", + "MKK", + "ETSI", + "IC", + "KCC", + "NCC", + "ACMA", + "CHILE", + "UKRAINE", + "MEXICO", + "CN", + "WW", +}; + +/* +* input with txpwr value in unit of txpwr index +* return string in length 6 at least (for -xx.xx) +*/ +void txpwr_idx_get_dbm_str(s8 idx, u8 txgi_max, u8 txgi_pdbm, SIZE_T cwidth, char dbm_str[], u8 dbm_str_len) +{ + char fmt[16]; + + if (idx == txgi_max) { + snprintf(fmt, 16, "%%%zus", cwidth >= 6 ? cwidth + 1 : 6); + snprintf(dbm_str, dbm_str_len, fmt, "NA"); + } else if (idx > -txgi_pdbm && idx < 0) { /* -0.xx */ + snprintf(fmt, 16, "%%%zus-0.%%02d", cwidth >= 6 ? cwidth - 4 : 1); + snprintf(dbm_str, dbm_str_len, fmt, "", (rtw_abs(idx) % txgi_pdbm) * 100 / txgi_pdbm); + } else if (idx % txgi_pdbm) { /* d.xx */ + snprintf(fmt, 16, "%%%zud.%%02d", cwidth >= 6 ? cwidth - 2 : 3); + snprintf(dbm_str, dbm_str_len, fmt, idx / txgi_pdbm, (rtw_abs(idx) % txgi_pdbm) * 100 / txgi_pdbm); + } else { /* d */ + snprintf(fmt, 16, "%%%zud", cwidth >= 6 ? cwidth + 1 : 6); + snprintf(dbm_str, dbm_str_len, fmt, idx / txgi_pdbm); + } +} + +/* +* input with txpwr value in unit of mbm +* return string in length 6 at least (for -xx.xx) +*/ +void txpwr_mbm_get_dbm_str(s16 mbm, SIZE_T cwidth, char dbm_str[], u8 dbm_str_len) +{ + char fmt[16]; + + if (mbm == UNSPECIFIED_MBM) { + snprintf(fmt, 16, "%%%zus", cwidth >= 6 ? cwidth + 1 : 6); + snprintf(dbm_str, dbm_str_len, fmt, "NA"); + } else if (mbm > -MBM_PDBM && mbm < 0) { /* -0.xx */ + snprintf(fmt, 16, "%%%zus-0.%%02d", cwidth >= 6 ? cwidth - 4 : 1); + snprintf(dbm_str, dbm_str_len, fmt, "", (rtw_abs(mbm) % MBM_PDBM) * 100 / MBM_PDBM); + } else if (mbm % MBM_PDBM) { /* d.xx */ + snprintf(fmt, 16, "%%%zud.%%02d", cwidth >= 6 ? cwidth - 2 : 3); + snprintf(dbm_str, dbm_str_len, fmt, mbm / MBM_PDBM, (rtw_abs(mbm) % MBM_PDBM) * 100 / MBM_PDBM); + } else { /* d */ + snprintf(fmt, 16, "%%%zud", cwidth >= 6 ? cwidth + 1 : 6); + snprintf(dbm_str, dbm_str_len, fmt, mbm / MBM_PDBM); + } +} + +static const s16 _mb_of_ntx[] = { + 0, /* 1TX */ + 301, /* 2TX */ + 477, /* 3TX */ + 602, /* 4TX */ + 699, /* 5TX */ + 778, /* 6TX */ + 845, /* 7TX */ + 903, /* 8TX */ +}; + +/* get mB(100 *dB) for specifc TX count relative to 1TX */ +s16 mb_of_ntx(u8 ntx) +{ + if (ntx == 0 || ntx > 8) { + RTW_ERR("ntx=%u, out of range\n", ntx); + rtw_warn_on(1); + } + + return _mb_of_ntx[ntx - 1]; +} + +#if CONFIG_TXPWR_LIMIT +void _dump_regd_exc_list(void *sel, struct rf_ctl_t *rfctl) +{ + struct regd_exc_ent *ent; + _list *cur, *head; + + RTW_PRINT_SEL(sel, "regd_exc_num:%u\n", rfctl->regd_exc_num); + + if (!rfctl->regd_exc_num) + goto exit; + + RTW_PRINT_SEL(sel, "%-7s %-6s %-9s\n", "country", "domain", "regd_name"); + + head = &rfctl->reg_exc_list; + cur = get_next(head); + + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + u8 has_country; + + ent = LIST_CONTAINOR(cur, struct regd_exc_ent, list); + cur = get_next(cur); + has_country = (ent->country[0] == '\0' && ent->country[1] == '\0') ? 0 : 1; + + RTW_PRINT_SEL(sel, " %c%c 0x%02x %s\n" + , has_country ? ent->country[0] : '0' + , has_country ? ent->country[1] : '0' + , ent->domain + , ent->regd_name + ); + } + +exit: + return; +} + +inline void dump_regd_exc_list(void *sel, struct rf_ctl_t *rfctl) +{ + _irqL irqL; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + _dump_regd_exc_list(sel, rfctl); + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); +} + +void rtw_regd_exc_add_with_nlen(struct rf_ctl_t *rfctl, const char *country, u8 domain, const char *regd_name, u32 nlen) +{ + struct regd_exc_ent *ent; + _irqL irqL; + + if (!regd_name || !nlen) { + rtw_warn_on(1); + goto exit; + } + + ent = (struct regd_exc_ent *)rtw_zmalloc(sizeof(struct regd_exc_ent) + nlen + 1); + if (!ent) + goto exit; + + _rtw_init_listhead(&ent->list); + if (country) + _rtw_memcpy(ent->country, country, 2); + ent->domain = domain; + _rtw_memcpy(ent->regd_name, regd_name, nlen); + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + rtw_list_insert_tail(&ent->list, &rfctl->reg_exc_list); + rfctl->regd_exc_num++; + + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + +exit: + return; +} + +inline void rtw_regd_exc_add(struct rf_ctl_t *rfctl, const char *country, u8 domain, const char *regd_name) +{ + rtw_regd_exc_add_with_nlen(rfctl, country, domain, regd_name, strlen(regd_name)); +} + +struct regd_exc_ent *_rtw_regd_exc_search(struct rf_ctl_t *rfctl, const char *country, u8 domain) +{ + struct regd_exc_ent *ent; + _list *cur, *head; + u8 match = 0; + + head = &rfctl->reg_exc_list; + cur = get_next(head); + + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + u8 has_country; + + ent = LIST_CONTAINOR(cur, struct regd_exc_ent, list); + cur = get_next(cur); + has_country = (ent->country[0] == '\0' && ent->country[1] == '\0') ? 0 : 1; + + /* entry has country condition to match */ + if (has_country) { + if (!country) + continue; + if (ent->country[0] != country[0] + || ent->country[1] != country[1]) + continue; + } + + /* entry has domain condition to match */ + if (ent->domain != 0xFF) { + if (domain == 0xFF) + continue; + if (ent->domain != domain) + continue; + } + + match = 1; + break; + } + + if (match) + return ent; + else + return NULL; +} + +inline struct regd_exc_ent *rtw_regd_exc_search(struct rf_ctl_t *rfctl, const char *country, u8 domain) +{ + struct regd_exc_ent *ent; + _irqL irqL; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + ent = _rtw_regd_exc_search(rfctl, country, domain); + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + return ent; +} + +void rtw_regd_exc_list_free(struct rf_ctl_t *rfctl) +{ + struct regd_exc_ent *ent; + _irqL irqL; + _list *cur, *head; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + head = &rfctl->reg_exc_list; + cur = get_next(head); + + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct regd_exc_ent, list); + cur = get_next(cur); + rtw_list_delete(&ent->list); + rtw_mfree((u8 *)ent, sizeof(struct regd_exc_ent) + strlen(ent->regd_name) + 1); + } + rfctl->regd_exc_num = 0; + + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); +} + +void dump_txpwr_lmt(void *sel, _adapter *adapter) +{ +#define TMP_STR_LEN 16 + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter); + struct hal_spec_t *hal_spec = GET_HAL_SPEC(adapter); + _irqL irqL; + char fmt[16]; + char tmp_str[TMP_STR_LEN]; + s8 *lmt_idx = NULL; + int bw, band, ch_num, tlrs, ntx_idx, rs, i, path; + u8 ch, n, rfpath_num; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + _dump_regd_exc_list(sel, rfctl); + RTW_PRINT_SEL(sel, "\n"); + + if (!rfctl->txpwr_regd_num) + goto release_lock; + + lmt_idx = rtw_malloc(sizeof(s8) * RF_PATH_MAX * rfctl->txpwr_regd_num); + if (!lmt_idx) { + RTW_ERR("%s alloc fail\n", __func__); + goto release_lock; + } + + RTW_PRINT_SEL(sel, "txpwr_lmt_2g_cck_ofdm_state:0x%02x\n", rfctl->txpwr_lmt_2g_cck_ofdm_state); + #if CONFIG_IEEE80211_BAND_5GHZ + if (IS_HARDWARE_TYPE_JAGUAR_ALL(adapter)) { + RTW_PRINT_SEL(sel, "txpwr_lmt_5g_cck_ofdm_state:0x%02x\n", rfctl->txpwr_lmt_5g_cck_ofdm_state); + RTW_PRINT_SEL(sel, "txpwr_lmt_5g_20_40_ref:0x%02x\n", rfctl->txpwr_lmt_5g_20_40_ref); + } + #endif + RTW_PRINT_SEL(sel, "\n"); + + for (band = BAND_ON_2_4G; band <= BAND_ON_5G; band++) { + if (!hal_is_band_support(adapter, band)) + continue; + + rfpath_num = (band == BAND_ON_2_4G ? hal_spec->rfpath_num_2g : hal_spec->rfpath_num_5g); + + for (bw = 0; bw < MAX_5G_BANDWIDTH_NUM; bw++) { + + if (bw >= CHANNEL_WIDTH_160) + break; + if (band == BAND_ON_2_4G && bw >= CHANNEL_WIDTH_80) + break; + + if (band == BAND_ON_2_4G) + ch_num = CENTER_CH_2G_NUM; + else + ch_num = center_chs_5g_num(bw); + + if (ch_num == 0) { + rtw_warn_on(1); + break; + } + + for (tlrs = TXPWR_LMT_RS_CCK; tlrs < TXPWR_LMT_RS_NUM; tlrs++) { + + if (band == BAND_ON_2_4G && tlrs == TXPWR_LMT_RS_VHT) + continue; + if (band == BAND_ON_5G && tlrs == TXPWR_LMT_RS_CCK) + continue; + if (bw > CHANNEL_WIDTH_20 && (tlrs == TXPWR_LMT_RS_CCK || tlrs == TXPWR_LMT_RS_OFDM)) + continue; + if (bw > CHANNEL_WIDTH_40 && tlrs == TXPWR_LMT_RS_HT) + continue; + if (tlrs == TXPWR_LMT_RS_VHT && !IS_HARDWARE_TYPE_JAGUAR_ALL(adapter)) + continue; + + for (ntx_idx = RF_1TX; ntx_idx < MAX_TX_COUNT; ntx_idx++) { + struct txpwr_lmt_ent *ent; + _list *cur, *head; + + if (ntx_idx + 1 > hal_data->max_tx_cnt) + continue; + + /* bypass CCK multi-TX is not defined */ + if (tlrs == TXPWR_LMT_RS_CCK && ntx_idx > RF_1TX) { + if (band == BAND_ON_2_4G + && !(rfctl->txpwr_lmt_2g_cck_ofdm_state & (TXPWR_LMT_HAS_CCK_1T << ntx_idx))) + continue; + } + + /* bypass OFDM multi-TX is not defined */ + if (tlrs == TXPWR_LMT_RS_OFDM && ntx_idx > RF_1TX) { + if (band == BAND_ON_2_4G + && !(rfctl->txpwr_lmt_2g_cck_ofdm_state & (TXPWR_LMT_HAS_OFDM_1T << ntx_idx))) + continue; + #if CONFIG_IEEE80211_BAND_5GHZ + if (band == BAND_ON_5G + && !(rfctl->txpwr_lmt_5g_cck_ofdm_state & (TXPWR_LMT_HAS_OFDM_1T << ntx_idx))) + continue; + #endif + } + + /* bypass 5G 20M, 40M pure reference */ + #if CONFIG_IEEE80211_BAND_5GHZ + if (band == BAND_ON_5G && (bw == CHANNEL_WIDTH_20 || bw == CHANNEL_WIDTH_40)) { + if (rfctl->txpwr_lmt_5g_20_40_ref == TXPWR_LMT_REF_HT_FROM_VHT) { + if (tlrs == TXPWR_LMT_RS_HT) + continue; + } else if (rfctl->txpwr_lmt_5g_20_40_ref == TXPWR_LMT_REF_VHT_FROM_HT) { + if (tlrs == TXPWR_LMT_RS_VHT && bw <= CHANNEL_WIDTH_40) + continue; + } + } + #endif + + /* choose n-SS mapping rate section to get lmt diff value */ + if (tlrs == TXPWR_LMT_RS_CCK) + rs = CCK; + else if (tlrs == TXPWR_LMT_RS_OFDM) + rs = OFDM; + else if (tlrs == TXPWR_LMT_RS_HT) + rs = HT_1SS + ntx_idx; + else if (tlrs == TXPWR_LMT_RS_VHT) + rs = VHT_1SS + ntx_idx; + else { + RTW_ERR("%s invalid tlrs %u\n", __func__, tlrs); + continue; + } + + RTW_PRINT_SEL(sel, "[%s][%s][%s][%uT]\n" + , band_str(band) + , ch_width_str(bw) + , txpwr_lmt_rs_str(tlrs) + , ntx_idx + 1 + ); + + /* header for limit in db */ + RTW_PRINT_SEL(sel, "%3s ", "ch"); + + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + + sprintf(fmt, "%%%zus%%s ", strlen(ent->regd_name) >= 6 ? 1 : 6 - strlen(ent->regd_name)); + snprintf(tmp_str, TMP_STR_LEN, fmt + , strcmp(ent->regd_name, rfctl->regd_name) == 0 ? "*" : "" + , ent->regd_name); + _RTW_PRINT_SEL(sel, "%s", tmp_str); + } + sprintf(fmt, "%%%zus%%s ", strlen(regd_str(TXPWR_LMT_WW)) >= 6 ? 1 : 6 - strlen(regd_str(TXPWR_LMT_WW))); + snprintf(tmp_str, TMP_STR_LEN, fmt + , strcmp(rfctl->regd_name, regd_str(TXPWR_LMT_WW)) == 0 ? "*" : "" + , regd_str(TXPWR_LMT_WW)); + _RTW_PRINT_SEL(sel, "%s", tmp_str); + + /* header for limit offset */ + for (path = 0; path < RF_PATH_MAX; path++) { + if (path >= rfpath_num) + break; + _RTW_PRINT_SEL(sel, "|"); + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + _RTW_PRINT_SEL(sel, "%3c " + , strcmp(ent->regd_name, rfctl->regd_name) == 0 ? rf_path_char(path) : ' '); + } + _RTW_PRINT_SEL(sel, "%3c " + , strcmp(rfctl->regd_name, regd_str(TXPWR_LMT_WW)) == 0 ? rf_path_char(path) : ' '); + } + _RTW_PRINT_SEL(sel, "\n"); + + for (n = 0; n < ch_num; n++) { + s8 lmt; + s8 lmt_offset; + u8 base; + + if (band == BAND_ON_2_4G) + ch = n + 1; + else + ch = center_chs_5g(bw, n); + + if (ch == 0) { + rtw_warn_on(1); + break; + } + + /* dump limit in dBm */ + RTW_PRINT_SEL(sel, "%3u ", ch); + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + lmt = phy_get_txpwr_lmt(adapter, ent->regd_name, band, bw, tlrs, ntx_idx, ch, 0); + txpwr_idx_get_dbm_str(lmt, hal_spec->txgi_max, hal_spec->txgi_pdbm, strlen(ent->regd_name), tmp_str, TMP_STR_LEN); + _RTW_PRINT_SEL(sel, "%s ", tmp_str); + } + lmt = phy_get_txpwr_lmt(adapter, regd_str(TXPWR_LMT_WW), band, bw, tlrs, ntx_idx, ch, 0); + txpwr_idx_get_dbm_str(lmt, hal_spec->txgi_max, hal_spec->txgi_pdbm, strlen(regd_str(TXPWR_LMT_WW)), tmp_str, TMP_STR_LEN); + _RTW_PRINT_SEL(sel, "%s ", tmp_str); + + /* dump limit offset of each path */ + for (path = RF_PATH_A; path < RF_PATH_MAX; path++) { + if (path >= rfpath_num) + break; + + base = phy_get_target_txpwr(adapter, band, path, rs); + + _RTW_PRINT_SEL(sel, "|"); + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + i = 0; + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + lmt_offset = phy_get_txpwr_lmt_diff(adapter, ent->regd_name, band, bw, path, rs, tlrs, ntx_idx, ch, 0); + if (lmt_offset == hal_spec->txgi_max) { + *(lmt_idx + i * RF_PATH_MAX + path) = hal_spec->txgi_max; + _RTW_PRINT_SEL(sel, "%3s ", "NA"); + } else { + *(lmt_idx + i * RF_PATH_MAX + path) = lmt_offset + base; + _RTW_PRINT_SEL(sel, "%3d ", lmt_offset); + } + i++; + } + lmt_offset = phy_get_txpwr_lmt_diff(adapter, regd_str(TXPWR_LMT_WW), band, bw, path, rs, tlrs, ntx_idx, ch, 0); + if (lmt_offset == hal_spec->txgi_max) + _RTW_PRINT_SEL(sel, "%3s ", "NA"); + else + _RTW_PRINT_SEL(sel, "%3d ", lmt_offset); + + } + + /* compare limit_idx of each path, print 'x' when mismatch */ + if (rfpath_num > 1) { + for (i = 0; i < rfctl->txpwr_regd_num; i++) { + for (path = 0; path < RF_PATH_MAX; path++) { + if (path >= rfpath_num) + break; + if (*(lmt_idx + i * RF_PATH_MAX + path) != *(lmt_idx + i * RF_PATH_MAX + ((path + 1) % rfpath_num))) + break; + } + if (path >= rfpath_num) + _RTW_PRINT_SEL(sel, " "); + else + _RTW_PRINT_SEL(sel, "x"); + } + } + _RTW_PRINT_SEL(sel, "\n"); + + } + RTW_PRINT_SEL(sel, "\n"); + } + } /* loop for rate sections */ + } /* loop for bandwidths */ + } /* loop for bands */ + + if (lmt_idx) + rtw_mfree(lmt_idx, sizeof(s8) * RF_PATH_MAX * rfctl->txpwr_regd_num); + +release_lock: + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); +} + +/* search matcing first, if not found, alloc one */ +void rtw_txpwr_lmt_add_with_nlen(struct rf_ctl_t *rfctl, const char *regd_name, u32 nlen + , u8 band, u8 bw, u8 tlrs, u8 ntx_idx, u8 ch_idx, s8 lmt) +{ + struct hal_spec_t *hal_spec = GET_HAL_SPEC(dvobj_get_primary_adapter(rfctl_to_dvobj(rfctl))); + struct txpwr_lmt_ent *ent; + _irqL irqL; + _list *cur, *head; + s8 pre_lmt; + + if (!regd_name || !nlen) { + rtw_warn_on(1); + goto exit; + } + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + /* search for existed entry */ + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + + if (strlen(ent->regd_name) == nlen + && _rtw_memcmp(ent->regd_name, regd_name, nlen) == _TRUE) + goto chk_lmt_val; + } + + /* alloc new one */ + ent = (struct txpwr_lmt_ent *)rtw_zvmalloc(sizeof(struct txpwr_lmt_ent) + nlen + 1); + if (!ent) + goto release_lock; + + _rtw_init_listhead(&ent->list); + _rtw_memcpy(ent->regd_name, regd_name, nlen); + { + u8 j, k, l, m; + + for (j = 0; j < MAX_2_4G_BANDWIDTH_NUM; ++j) + for (k = 0; k < TXPWR_LMT_RS_NUM_2G; ++k) + for (m = 0; m < CENTER_CH_2G_NUM; ++m) + for (l = 0; l < MAX_TX_COUNT; ++l) + ent->lmt_2g[j][k][m][l] = hal_spec->txgi_max; + #if CONFIG_IEEE80211_BAND_5GHZ + for (j = 0; j < MAX_5G_BANDWIDTH_NUM; ++j) + for (k = 0; k < TXPWR_LMT_RS_NUM_5G; ++k) + for (m = 0; m < CENTER_CH_5G_ALL_NUM; ++m) + for (l = 0; l < MAX_TX_COUNT; ++l) + ent->lmt_5g[j][k][m][l] = hal_spec->txgi_max; + #endif + } + + rtw_list_insert_tail(&ent->list, &rfctl->txpwr_lmt_list); + rfctl->txpwr_regd_num++; + +chk_lmt_val: + if (band == BAND_ON_2_4G) + pre_lmt = ent->lmt_2g[bw][tlrs][ch_idx][ntx_idx]; + #if CONFIG_IEEE80211_BAND_5GHZ + else if (band == BAND_ON_5G) + pre_lmt = ent->lmt_5g[bw][tlrs - 1][ch_idx][ntx_idx]; + #endif + else + goto release_lock; + + if (pre_lmt != hal_spec->txgi_max) + RTW_PRINT("duplicate txpwr_lmt for [%s][%s][%s][%s][%uT][%d]\n" + , regd_name, band_str(band), ch_width_str(bw), txpwr_lmt_rs_str(tlrs), ntx_idx + 1 + , band == BAND_ON_2_4G ? ch_idx + 1 : center_ch_5g_all[ch_idx]); + + lmt = rtw_min(pre_lmt, lmt); + if (band == BAND_ON_2_4G) + ent->lmt_2g[bw][tlrs][ch_idx][ntx_idx] = lmt; + #if CONFIG_IEEE80211_BAND_5GHZ + else if (band == BAND_ON_5G) + ent->lmt_5g[bw][tlrs - 1][ch_idx][ntx_idx] = lmt; + #endif + + if (0) + RTW_PRINT("%s, %4s, %6s, %7s, %uT, ch%3d = %d\n" + , regd_name, band_str(band), ch_width_str(bw), txpwr_lmt_rs_str(tlrs), ntx_idx + 1 + , band == BAND_ON_2_4G ? ch_idx + 1 : center_ch_5g_all[ch_idx] + , lmt); + +release_lock: + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + +exit: + return; +} + +inline void rtw_txpwr_lmt_add(struct rf_ctl_t *rfctl, const char *regd_name + , u8 band, u8 bw, u8 tlrs, u8 ntx_idx, u8 ch_idx, s8 lmt) +{ + rtw_txpwr_lmt_add_with_nlen(rfctl, regd_name, strlen(regd_name) + , band, bw, tlrs, ntx_idx, ch_idx, lmt); +} + +struct txpwr_lmt_ent *_rtw_txpwr_lmt_get_by_name(struct rf_ctl_t *rfctl, const char *regd_name) +{ + struct txpwr_lmt_ent *ent; + _list *cur, *head; + u8 found = 0; + + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + + if (strcmp(ent->regd_name, regd_name) == 0) { + found = 1; + break; + } + } + + if (found) + return ent; + return NULL; +} + +inline struct txpwr_lmt_ent *rtw_txpwr_lmt_get_by_name(struct rf_ctl_t *rfctl, const char *regd_name) +{ + struct txpwr_lmt_ent *ent; + _irqL irqL; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + ent = _rtw_txpwr_lmt_get_by_name(rfctl, regd_name); + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + return ent; +} + +void rtw_txpwr_lmt_list_free(struct rf_ctl_t *rfctl) +{ + struct txpwr_lmt_ent *ent; + _irqL irqL; + _list *cur, *head; + + _enter_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); + + head = &rfctl->txpwr_lmt_list; + cur = get_next(head); + + while ((rtw_end_of_queue_search(head, cur)) == _FALSE) { + ent = LIST_CONTAINOR(cur, struct txpwr_lmt_ent, list); + cur = get_next(cur); + if (ent->regd_name == rfctl->regd_name) + rfctl->regd_name = regd_str(TXPWR_LMT_NONE); + rtw_list_delete(&ent->list); + rtw_vmfree((u8 *)ent, sizeof(struct txpwr_lmt_ent) + strlen(ent->regd_name) + 1); + } + rfctl->txpwr_regd_num = 0; + + _exit_critical_mutex(&rfctl->txpwr_lmt_mutex, &irqL); +} +#endif /* CONFIG_TXPWR_LIMIT */ + +int rtw_ch_to_bb_gain_sel(int ch) +{ + int sel = -1; + + if (ch >= 1 && ch <= 14) + sel = BB_GAIN_2G; +#if CONFIG_IEEE80211_BAND_5GHZ + else if (ch >= 36 && ch < 48) + sel = BB_GAIN_5GLB1; + else if (ch >= 52 && ch <= 64) + sel = BB_GAIN_5GLB2; + else if (ch >= 100 && ch <= 120) + sel = BB_GAIN_5GMB1; + else if (ch >= 124 && ch <= 144) + sel = BB_GAIN_5GMB2; + else if (ch >= 149 && ch <= 177) + sel = BB_GAIN_5GHB; +#endif + + return sel; +} + +s8 rtw_rf_get_kfree_tx_gain_offset(_adapter *padapter, u8 path, u8 ch) +{ + s8 kfree_offset = 0; + +#ifdef CONFIG_RF_POWER_TRIM + struct kfree_data_t *kfree_data = GET_KFREE_DATA(padapter); + s8 bb_gain_sel = rtw_ch_to_bb_gain_sel(ch); + + if (bb_gain_sel < BB_GAIN_2G || bb_gain_sel >= BB_GAIN_NUM) { + rtw_warn_on(1); + goto exit; + } + + if (kfree_data->flag & KFREE_FLAG_ON) { + kfree_offset = kfree_data->bb_gain[bb_gain_sel][path]; + if (IS_HARDWARE_TYPE_8723D(padapter)) + RTW_INFO("%s path:%s, ch:%u, bb_gain_sel:%d, kfree_offset:%d\n" + , __func__, (path == 0)?"S1":"S0", + ch, bb_gain_sel, kfree_offset); + else + RTW_INFO("%s path:%u, ch:%u, bb_gain_sel:%d, kfree_offset:%d\n" + , __func__, path, ch, bb_gain_sel, kfree_offset); + } +exit: +#endif /* CONFIG_RF_POWER_TRIM */ + return kfree_offset; +} + +void rtw_rf_set_tx_gain_offset(_adapter *adapter, u8 path, s8 offset) +{ +#if !defined(CONFIG_RTL8814A) && !defined(CONFIG_RTL8822B) && !defined(CONFIG_RTL8821C) && !defined(CONFIG_RTL8822C) \ + && !defined(CONFIG_RTL8723F) + u8 write_value; +#endif + u8 target_path = 0; + u32 val32 = 0; + + if (IS_HARDWARE_TYPE_8723D(adapter)) { + target_path = RF_PATH_A; /*in 8723D case path means S0/S1*/ + if (path == PPG_8723D_S1) + RTW_INFO("kfree gain_offset 0x55:0x%x ", + rtw_hal_read_rfreg(adapter, target_path, 0x55, 0xffffffff)); + else if (path == PPG_8723D_S0) + RTW_INFO("kfree gain_offset 0x65:0x%x ", + rtw_hal_read_rfreg(adapter, target_path, 0x65, 0xffffffff)); + } else { + target_path = path; + RTW_INFO("kfree gain_offset 0x55:0x%x ", rtw_hal_read_rfreg(adapter, target_path, 0x55, 0xffffffff)); + } + + switch (rtw_get_chip_type(adapter)) { +#ifdef CONFIG_RTL8723D + case RTL8723D: + write_value = RF_TX_GAIN_OFFSET_8723D(offset); + if (path == PPG_8723D_S1) + rtw_hal_write_rfreg(adapter, target_path, 0x55, 0x0f8000, write_value); + else if (path == PPG_8723D_S0) + rtw_hal_write_rfreg(adapter, target_path, 0x65, 0x0f8000, write_value); + break; +#endif /* CONFIG_RTL8723D */ +#ifdef CONFIG_RTL8703B + case RTL8703B: + write_value = RF_TX_GAIN_OFFSET_8703B(offset); + rtw_hal_write_rfreg(adapter, target_path, 0x55, 0x0fc000, write_value); + break; +#endif /* CONFIG_RTL8703B */ +#ifdef CONFIG_RTL8188F + case RTL8188F: + write_value = RF_TX_GAIN_OFFSET_8188F(offset); + rtw_hal_write_rfreg(adapter, target_path, 0x55, 0x0fc000, write_value); + break; +#endif /* CONFIG_RTL8188F */ +#ifdef CONFIG_RTL8188GTV + case RTL8188GTV: + write_value = RF_TX_GAIN_OFFSET_8188GTV(offset); + rtw_hal_write_rfreg(adapter, target_path, 0x55, 0x0fc000, write_value); + break; +#endif /* CONFIG_RTL8188GTV */ +#ifdef CONFIG_RTL8192E + case RTL8192E: + write_value = RF_TX_GAIN_OFFSET_8192E(offset); + rtw_hal_write_rfreg(adapter, target_path, 0x55, 0x0f8000, write_value); + break; +#endif /* CONFIG_RTL8188F */ + +#ifdef CONFIG_RTL8821A + case RTL8821: + write_value = RF_TX_GAIN_OFFSET_8821A(offset); + rtw_hal_write_rfreg(adapter, target_path, 0x55, 0x0f8000, write_value); + break; +#endif /* CONFIG_RTL8821A */ +#if defined(CONFIG_RTL8814A) || defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8192F) || defined(CONFIG_RTL8822C) \ + || defined(CONFIG_RTL8723F) + case RTL8814A: + case RTL8822B: + case RTL8822C: + case RTL8821C: + case RTL8192F: + case RTL8723F: + RTW_INFO("\nkfree by PhyDM on the sw CH. path %d\n", path); + break; +#endif /* CONFIG_RTL8814A || CONFIG_RTL8822B || CONFIG_RTL8821C || CONFIG_RTL8723F */ + + default: + rtw_warn_on(1); + break; + } + + if (IS_HARDWARE_TYPE_8723D(adapter)) { + if (path == PPG_8723D_S1) + val32 = rtw_hal_read_rfreg(adapter, target_path, 0x55, 0xffffffff); + else if (path == PPG_8723D_S0) + val32 = rtw_hal_read_rfreg(adapter, target_path, 0x65, 0xffffffff); + } else { + val32 = rtw_hal_read_rfreg(adapter, target_path, 0x55, 0xffffffff); + } + RTW_INFO(" after :0x%x\n", val32); +} + +void rtw_rf_apply_tx_gain_offset(_adapter *adapter, u8 ch) +{ + struct hal_spec_t *hal_spec = GET_HAL_SPEC(adapter); + s8 kfree_offset = 0; + s8 tx_pwr_track_offset = 0; /* TODO: 8814A should consider tx pwr track when setting tx gain offset */ + s8 total_offset; + int i, total = 0; + + if (IS_HARDWARE_TYPE_8723D(adapter)) + total = 2; /* S1 and S0 */ + else + total = hal_spec->rf_reg_path_num; + + for (i = 0; i < total; i++) { + kfree_offset = rtw_rf_get_kfree_tx_gain_offset(adapter, i, ch); + total_offset = kfree_offset + tx_pwr_track_offset; + rtw_rf_set_tx_gain_offset(adapter, i, total_offset); + } +} + +bool rtw_is_long_cac_range(u32 hi, u32 lo, u8 dfs_region) +{ + return (dfs_region == RTW_DFS_REGD_ETSI && rtw_is_range_overlap(hi, lo, 5650, 5600)) ? _TRUE : _FALSE; +} + +bool rtw_is_long_cac_ch(u8 ch, u8 bw, u8 offset, u8 dfs_region) +{ + u32 hi, lo; + + if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == _FALSE) + return _FALSE; + + return rtw_is_long_cac_range(hi, lo, dfs_region) ? _TRUE : _FALSE; +} diff --git a/core/rtw_rm.c b/core/rtw_rm.c new file mode 100644 index 0000000..38ceb2e --- /dev/null +++ b/core/rtw_rm.c @@ -0,0 +1,2825 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include +#ifdef CONFIG_RTW_80211K +#include "rtw_rm_fsm.h" +#include "rtw_rm_util.h" +#endif + +#define pstr(s) s+strlen(s) +#ifndef MIN +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) +#endif + +u8 rm_post_event_hdl(_adapter *padapter, u8 *pbuf) +{ +#ifdef CONFIG_RTW_80211K + struct rm_event *pev = (struct rm_event *)pbuf; + + _rm_post_event(padapter, pev->rmid, pev->evid); + rm_handler(padapter, pev); +#endif + return H2C_SUCCESS; +} + +#ifdef CONFIG_RTW_80211K +struct cmd_meas_type_ { + u8 id; + char *name; +}; + +char *rm_type_req_name(u8 meas_type) { + + switch (meas_type) { + case basic_req: + return "basic_req"; + case cca_req: + return "cca_req"; + case rpi_histo_req: + return "rpi_histo_req"; + case ch_load_req: + return "ch_load_req"; + case noise_histo_req: + return "noise_histo_req"; + case bcn_req: + return "bcn_req"; + case frame_req: + return "frame_req"; + case sta_statis_req: + return "sta_statis_req"; + } + return "unknown_req"; +}; + +char *rm_type_rep_name(u8 meas_type) { + + switch (meas_type) { + case basic_rep: + return "basic_rep"; + case cca_rep: + return "cca_rep"; + case rpi_histo_rep: + return "rpi_histo_rep"; + case ch_load_rep: + return "ch_load_rep"; + case noise_histo_rep: + return "noise_histo_rep"; + case bcn_rep: + return "bcn_rep"; + case frame_rep: + return "frame_rep"; + case sta_statis_rep: + return "sta_statis_rep"; + } + return "unknown_rep"; +}; + +char *rm_en_cap_name(enum rm_cap_en en) +{ + switch (en) { + case RM_LINK_MEAS_CAP_EN: + return "RM_LINK_MEAS_CAP_EN"; + case RM_NB_REP_CAP_EN: + return "RM_NB_REP_CAP_EN"; + case RM_PARAL_MEAS_CAP_EN: + return "RM_PARAL_MEAS_CAP_EN"; + case RM_REPEAT_MEAS_CAP_EN: + return "RM_REPEAT_MEAS_CAP_EN"; + case RM_BCN_PASSIVE_MEAS_CAP_EN: + return "RM_BCN_PASSIVE_MEAS_CAP_EN"; + case RM_BCN_ACTIVE_MEAS_CAP_EN: + return "RM_BCN_ACTIVE_MEAS_CAP_EN"; + case RM_BCN_TABLE_MEAS_CAP_EN: + return "RM_BCN_TABLE_MEAS_CAP_EN"; + case RM_BCN_MEAS_REP_COND_CAP_EN: + return "RM_BCN_MEAS_REP_COND_CAP_EN"; + + case RM_FRAME_MEAS_CAP_EN: + return "RM_FRAME_MEAS_CAP_EN"; + case RM_CH_LOAD_CAP_EN: + return "RM_CH_LOAD_CAP_EN"; + case RM_NOISE_HISTO_CAP_EN: + return "RM_NOISE_HISTO_CAP_EN"; + case RM_STATIS_MEAS_CAP_EN: + return "RM_STATIS_MEAS_CAP_EN"; + case RM_LCI_MEAS_CAP_EN: + return "RM_LCI_MEAS_CAP_EN"; + case RM_LCI_AMIMUTH_CAP_EN: + return "RM_LCI_AMIMUTH_CAP_EN"; + case RM_TRANS_STREAM_CAT_MEAS_CAP_EN: + return "RM_TRANS_STREAM_CAT_MEAS_CAP_EN"; + case RM_TRIG_TRANS_STREAM_CAT_MEAS_CAP_EN: + return "RM_TRIG_TRANS_STREAM_CAT_MEAS_CAP_EN"; + + case RM_AP_CH_REP_CAP_EN: + return "RM_AP_CH_REP_CAP_EN"; + case RM_RM_MIB_CAP_EN: + return "RM_RM_MIB_CAP_EN"; + case RM_OP_CH_MAX_MEAS_DUR0: + return "RM_OP_CH_MAX_MEAS_DUR0"; + case RM_OP_CH_MAX_MEAS_DUR1: + return "RM_OP_CH_MAX_MEAS_DUR1"; + case RM_OP_CH_MAX_MEAS_DUR2: + return "RM_OP_CH_MAX_MEAS_DUR2"; + case RM_NONOP_CH_MAX_MEAS_DUR0: + return "RM_NONOP_CH_MAX_MEAS_DUR0"; + case RM_NONOP_CH_MAX_MEAS_DUR1: + return "RM_NONOP_CH_MAX_MEAS_DUR1"; + case RM_NONOP_CH_MAX_MEAS_DUR2: + return "RM_NONOP_CH_MAX_MEAS_DUR2"; + + case RM_MEAS_PILOT_CAP0: + return "RM_MEAS_PILOT_CAP0"; /* 24-26 */ + case RM_MEAS_PILOT_CAP1: + return "RM_MEAS_PILOT_CAP1"; + case RM_MEAS_PILOT_CAP2: + return "RM_MEAS_PILOT_CAP2"; + case RM_MEAS_PILOT_TRANS_INFO_CAP_EN: + return "RM_MEAS_PILOT_TRANS_INFO_CAP_EN"; + case RM_NB_REP_TSF_OFFSET_CAP_EN: + return "RM_NB_REP_TSF_OFFSET_CAP_EN"; + case RM_RCPI_MEAS_CAP_EN: + return "RM_RCPI_MEAS_CAP_EN"; /* 29 */ + case RM_RSNI_MEAS_CAP_EN: + return "RM_RSNI_MEAS_CAP_EN"; + case RM_BSS_AVG_ACCESS_DELAY_CAP_EN: + return "RM_BSS_AVG_ACCESS_DELAY_CAP_EN"; + + case RM_AVALB_ADMIS_CAPACITY_CAP_EN: + return "RM_AVALB_ADMIS_CAPACITY_CAP_EN"; + case RM_ANT_CAP_EN: + return "RM_ANT_CAP_EN"; + case RM_RSVD: + case RM_MAX: + default: + break; + } + return "unknown"; +} + +int rm_en_cap_chk_and_set(struct rm_obj *prm, enum rm_cap_en en) +{ + int idx; + u8 cap; + + + if (en >= RM_MAX) + return _FALSE; + + idx = en / 8; + cap = prm->psta->padapter->rmpriv.rm_en_cap_def[idx]; + + if (!(cap & BIT(en - (idx*8)))) { + RTW_INFO("RM: %s incapable\n",rm_en_cap_name(en)); + rm_set_rep_mode(prm, MEAS_REP_MOD_INCAP); + return _FALSE; + } + return _SUCCESS; +} + +/* for caller outside rm */ +u8 rm_add_nb_req(_adapter *padapter, struct sta_info *psta) +{ + struct rm_obj *prm; + + + prm = rm_alloc_rmobj(padapter); + + if (prm == NULL) { + RTW_ERR("RM: unable to alloc rm obj for requeset\n"); + return _FALSE; + } + + prm->psta = psta; + prm->q.category = RTW_WLAN_CATEGORY_RADIO_MEAS; + prm->q.diag_token = rm_gen_dialog_token(padapter); + prm->q.m_token = rm_gen_meas_token(padapter); + prm->rmid = rm_gen_rmid(padapter, prm, RM_MASTER); + + prm->q.action_code = RM_ACT_NB_REP_REQ; + + #if 0 + if (pmac) { /* find sta_info according to bssid */ + pmac += 4; /* skip mac= */ + if (hwaddr_parse(pmac, bssid) == NULL) { + sprintf(pstr(s), "Err: \nincorrect mac format\n"); + return _FAIL; + } + psta = rm_get_sta(padapter, 0xff, bssid); + } + #endif + + /* enquee rmobj */ + rm_enqueue_rmobj(padapter, prm, _FALSE); + + RTW_INFO("RM: rmid=%x add req to " MAC_FMT "\n", + prm->rmid, MAC_ARG(psta->cmn.mac_addr)); + + return _SUCCESS; +} + +static u8 *build_wlan_hdr(_adapter *padapter, struct xmit_frame *pmgntframe, + struct sta_info *psta, u16 frame_type) +{ + u8 *pframe; + u16 *fctrl; + struct pkt_attrib *pattr; + struct rtw_ieee80211_hdr *pwlanhdr; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + + /* update attribute */ + pattr = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattr); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, psta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, + get_my_bssid(&(pmlmeinfo->network)),ETH_ALEN); + + RTW_INFO("RM: dst = " MAC_FMT "\n", MAC_ARG(pwlanhdr->addr1)); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFragNum(pframe, 0); + + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattr->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + return pframe; +} + +void rm_set_rep_mode(struct rm_obj *prm, u8 mode) +{ + + RTW_INFO("RM: rmid=%x set %s\n", + prm->rmid, + mode|MEAS_REP_MOD_INCAP?"INCAP": + mode|MEAS_REP_MOD_REFUSE?"REFUSE": + mode|MEAS_REP_MOD_LATE?"LATE":""); + + prm->p.m_mode |= mode; +} + +int issue_null_reply(struct rm_obj *prm) +{ + int len=0, my_len; + u8 *pframe, m_mode; + _adapter *padapter = prm->psta->padapter; + struct pkt_attrib *pattr; + struct xmit_frame *pmgntframe; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + + + m_mode = prm->p.m_mode; + if (m_mode || prm->p.rpt == 0) { + RTW_INFO("RM: rmid=%x reply (%s repeat=%d)\n", + prm->rmid, + m_mode&MEAS_REP_MOD_INCAP?"INCAP": + m_mode&MEAS_REP_MOD_REFUSE?"REFUSE": + m_mode&MEAS_REP_MOD_LATE?"LATE":"no content", + prm->p.rpt); + } + + switch (prm->p.action_code) { + case RM_ACT_RADIO_MEAS_REQ: + len = 8; + break; + case RM_ACT_NB_REP_REQ: + len = 3; + break; + case RM_ACT_LINK_MEAS_REQ: + len = 3; + break; + default: + break; + } + + if (len==0) + return _FALSE; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_ERR("RM: %s alloc xmit_frame fail\n",__func__); + return _FALSE; + } + pattr = &pmgntframe->attrib; + pframe = build_wlan_hdr(padapter, pmgntframe, prm->psta, WIFI_ACTION); + pframe = rtw_set_fixed_ie(pframe, 3, &prm->p.category, &pattr->pktlen); + + my_len = 0; + if (len>5) { + prm->p.len = len - 3 - 2; + pframe = rtw_set_fixed_ie(pframe, len - 3, + &prm->p.e_id, &my_len); + } + + pattr->pktlen += my_len; + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + + return _SUCCESS; +} + +int ready_for_scan(struct rm_obj *prm) +{ + _adapter *padapter = prm->psta->padapter; + u8 ssc_chk; + + if (!rtw_is_adapter_up(padapter)) + return _FALSE; + + ssc_chk = rtw_sitesurvey_condition_check(padapter, _FALSE); + + if (ssc_chk == SS_ALLOW) + return _SUCCESS; + + return _FALSE; +} + +int rm_sitesurvey(struct rm_obj *prm) +{ + int meas_ch_amount=0; + u8 op_class=0, val8; + struct rtw_ieee80211_channel *pch_set; + struct sitesurvey_parm parm; + + + RTW_INFO("RM: rmid=%x %s\n",prm->rmid, __func__); + + pch_set = &prm->q.ch_set[0]; + + _rtw_memset(pch_set, 0, + sizeof(struct rtw_ieee80211_channel) * RTW_CHANNEL_SCAN_AMOUNT); + + op_class = prm->q.op_class; + if (prm->q.ch_num == 0) { + /* ch_num=0 : scan all ch in operating class */ + meas_ch_amount = rm_get_ch_set(pch_set, + op_class, prm->q.ch_num); + + } else if (prm->q.ch_num == 255) { + /* 802.11 p.1066 */ + /* ch_num=255 : If the Channel Number is 255 and includes + * AP Channel Report subelements + */ + meas_ch_amount = rm_get_ch_set_from_bcn_req_opt(pch_set, &prm->q.opt.bcn); + } else + meas_ch_amount = rm_get_ch_set(pch_set, op_class, prm->q.ch_num); + + /* get means channel */ + prm->q.ch_set_ch_amount = meas_ch_amount; + +#if (RM_MORE_DBG_MSG) + RTW_INFO("survey (%d) chaannels\n", meas_ch_amount); +#endif + + _rtw_memset(&parm, 0, sizeof(struct sitesurvey_parm)); + _rtw_memcpy(parm.ch, pch_set, + sizeof(struct rtw_ieee80211_channel) * + MIN(meas_ch_amount, RTW_CHANNEL_SCAN_AMOUNT)); + + _rtw_memcpy(&parm.ssid[0], &prm->q.opt.bcn.ssid, IW_ESSID_MAX_SIZE); + + parm.ssid_num = 1; + parm.scan_mode = prm->q.m_mode; + parm.ch_num = meas_ch_amount; + parm.igi = 0; + parm.token = prm->rmid; + parm.duration = prm->q.meas_dur; + /* parm.bw = BW_20M; */ + + rtw_sitesurvey_cmd(prm->psta->padapter, &parm); + + return _SUCCESS; +} + +static int rm_parse_ch_load_s_elem(struct rm_obj *prm, u8 *pbody, int req_len) +{ + u8 *popt_id; + int i, p=0; /* position */ + int len = req_len; + + + prm->q.opt_s_elem_len = len; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: opt_s_elem_len=%d\n", len); +#endif + while (len) { + + switch (pbody[p]) { + case ch_load_rep_info: + /* check RM_EN */ + rm_en_cap_chk_and_set(prm, RM_CH_LOAD_CAP_EN); + + _rtw_memcpy(&(prm->q.opt.clm.rep_cond), + &pbody[p+2], sizeof(prm->q.opt.clm.rep_cond)); + + RTW_INFO("RM: ch_load_rep_info=%u:%u\n", + prm->q.opt.clm.rep_cond.cond, + prm->q.opt.clm.rep_cond.threshold); + break; + default: + break; + + } + len = len - (int)pbody[p+1] - 2; + p = p + (int)pbody[p+1] + 2; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: opt_s_elem_len=%d\n",len); +#endif + } + return _SUCCESS; +} + +static int rm_parse_noise_histo_s_elem(struct rm_obj *prm, + u8 *pbody, int req_len) +{ + u8 *popt_id; + int i, p=0; /* position */ + int len = req_len; + + + prm->q.opt_s_elem_len = len; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: opt_s_elem_len=%d\n", len); +#endif + + while (len) { + + switch (pbody[p]) { + case noise_histo_rep_info: + /* check RM_EN */ + rm_en_cap_chk_and_set(prm, RM_NOISE_HISTO_CAP_EN); + + _rtw_memcpy(&(prm->q.opt.nhm.rep_cond), + &pbody[p+2], sizeof(prm->q.opt.nhm.rep_cond)); + + RTW_INFO("RM: noise_histo_rep_info=%u:%u\n", + prm->q.opt.nhm.rep_cond.cond, + prm->q.opt.nhm.rep_cond.threshold); + break; + default: + break; + + } + len = len - (int)pbody[p+1] - 2; + p = p + (int)pbody[p+1] + 2; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: opt_s_elem_len=%d\n",len); +#endif + } + return _SUCCESS; +} + +static int rm_parse_bcn_req_s_elem(struct rm_obj *prm, u8 *pbody, int req_len) +{ + u8 *popt_id; + int i, p=0; /* position */ + int len = req_len; + int ap_ch_rpt_idx = 0; + struct _RT_OPERATING_CLASS *op; + + + /* opt length,2:pbody[0]+ pbody[1] */ + /* first opt id : pbody[18] */ + + prm->q.opt_s_elem_len = len; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: opt_s_elem_len=%d\n", len); +#endif + + popt_id = prm->q.opt.bcn.opt_id; + while (len && prm->q.opt.bcn.opt_id_num < BCN_REQ_OPT_MAX_NUM) { + + switch (pbody[p]) { + case bcn_req_ssid: + RTW_INFO("bcn_req_ssid\n"); + +#if (DBG_BCN_REQ_WILDCARD) + RTW_INFO("DBG set ssid to WILDCARD\n"); +#else +#if (DBG_BCN_REQ_SSID) + RTW_INFO("DBG set ssid to %s\n",DBG_BCN_REQ_SSID_NAME); + i = strlen(DBG_BCN_REQ_SSID_NAME); + prm->q.opt.bcn.ssid.SsidLength = i; + _rtw_memcpy(&(prm->q.opt.bcn.ssid.Ssid), + DBG_BCN_REQ_SSID_NAME, i); + +#else /* original */ + prm->q.opt.bcn.ssid.SsidLength = pbody[p+1]; + _rtw_memcpy(&(prm->q.opt.bcn.ssid.Ssid), + &pbody[p+2], pbody[p+1]); +#endif +#endif + RTW_INFO("RM: bcn_req_ssid=%s\n", + prm->q.opt.bcn.ssid.Ssid); + + popt_id[prm->q.opt.bcn.opt_id_num++] = pbody[p]; + break; + + case bcn_req_rep_info: + /* check RM_EN */ + rm_en_cap_chk_and_set(prm, RM_BCN_MEAS_REP_COND_CAP_EN); + + _rtw_memcpy(&(prm->q.opt.bcn.rep_cond), + &pbody[p+2], sizeof(prm->q.opt.bcn.rep_cond)); + + RTW_INFO("bcn_req_rep_info=%u:%u\n", + prm->q.opt.bcn.rep_cond.cond, + prm->q.opt.bcn.rep_cond.threshold); + + /*popt_id[prm->q.opt.bcn.opt_id_num++] = pbody[p];*/ + break; + + case bcn_req_rep_detail: +#if DBG_BCN_REQ_DETAIL + prm->q.opt.bcn.rep_detail = 2; /* all IE in beacon */ +#else + prm->q.opt.bcn.rep_detail = pbody[p+2]; +#endif + popt_id[prm->q.opt.bcn.opt_id_num++] = pbody[p]; + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: report_detail=%d\n", + prm->q.opt.bcn.rep_detail); +#endif + break; + + case bcn_req_req: + RTW_INFO("RM: bcn_req_req\n"); + + prm->q.opt.bcn.req_start = rtw_malloc(pbody[p+1]); + + if (prm->q.opt.bcn.req_start == NULL) { + RTW_ERR("RM: req_start malloc fail!!\n"); + break; + } + + for (i = 0; i < pbody[p+1]; i++) + *((prm->q.opt.bcn.req_start)+i) = + pbody[p+2+i]; + + prm->q.opt.bcn.req_len = pbody[p+1]; + popt_id[prm->q.opt.bcn.opt_id_num++] = pbody[p]; + break; + + case bcn_req_ap_ch_rep: +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: bcn_req_ap_ch_rep\n"); +#endif + if (ap_ch_rpt_idx > BCN_REQ_OPT_AP_CH_RPT_MAX_NUM) { + RTW_ERR("RM: bcn_req_ap_ch_rep over size\n"); + break; + } + + popt_id[prm->q.opt.bcn.opt_id_num++] = pbody[p]; + /* get channel list + * EID:len:op-class:ch-list + */ + op = rtw_malloc(sizeof (*op)); + op->global_op_class = pbody[p + 2]; + i = pbody[p + 1] - 1; /* ch list len; (-1) is op class */ + +#if (RM_MORE_DBG_MSG) + RTW_INFO("%d op class %d has %d ch\n", + ap_ch_rpt_idx,op->global_op_class,i); +#endif + op->Len = i; + memcpy(op->Channel, &pbody[p + 3], + MIN(i, MAX_CH_NUM_IN_OP_CLASS)); + prm->q.opt.bcn.ap_ch_rpt[ap_ch_rpt_idx++] = op; + prm->q.opt.bcn.ap_ch_rpt_num = ap_ch_rpt_idx; + break; + + default: + break; + + } + len = len - (int)pbody[p+1] - 2; + p = p + (int)pbody[p+1] + 2; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: opt_s_elem_len=%d\n",len); +#endif + } + + return _SUCCESS; +} + +static int rm_parse_meas_req(struct rm_obj *prm, u8 *pbody) +{ + int p; /* position */ + int req_len; + + + req_len = (int)pbody[1]; + p = 5; + + prm->q.op_class = pbody[p++]; + prm->q.ch_num = pbody[p++]; + prm->q.rand_intvl = le16_to_cpu(*(u16*)(&pbody[p])); + p+=2; + prm->q.meas_dur = le16_to_cpu(*(u16*)(&pbody[p])); + p+=2; + + if (prm->q.m_type == bcn_req) { + /* + * 0: passive + * 1: active + * 2: bcn_table + */ + prm->q.m_mode = pbody[p++]; + + /* BSSID */ + _rtw_memcpy(&(prm->q.bssid), &pbody[p], 6); + p+=6; + + /* + * default, used when Reporting detail subelement + * is not included in Beacon Request + */ + prm->q.opt.bcn.rep_detail = 2; + } + + if (req_len-(p-2) <= 0) /* without sub-element */ + return _SUCCESS; + + switch (prm->q.m_type) { + case bcn_req: + rm_parse_bcn_req_s_elem(prm, &pbody[p], req_len-(p-2)); + break; + case ch_load_req: + rm_parse_ch_load_s_elem(prm, &pbody[p], req_len-(p-2)); + break; + case noise_histo_req: + rm_parse_noise_histo_s_elem(prm, &pbody[p], req_len-(p-2)); + break; + default: + break; + } + + return _SUCCESS; +} + +/* receive measurement request */ +int rm_recv_radio_mens_req(_adapter *padapter, + union recv_frame *precv_frame, struct sta_info *psta) +{ + struct rm_obj *prm; + struct rm_priv *prmpriv = &padapter->rmpriv; + u8 *pdiag_body = (u8 *)(precv_frame->u.hdr.rx_data + + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 *pmeas_body = &pdiag_body[5]; + u8 rmid, update = 0; + + +#if 0 + /* search existing rm_obj */ + rmid = psta->cmn.aid << 16 + | pdiag_body[2] << 8 + | RM_SLAVE; + + prm = rm_get_rmobj(padapter, rmid); + if (prm) { + RTW_INFO("RM: Found an exist meas rmid=%u\n", rmid); + update = 1; + } else +#endif + prm = rm_alloc_rmobj(padapter); + + if (prm == NULL) { + RTW_ERR("RM: unable to alloc rm obj for requeset\n"); + return _FALSE; + } + + prm->psta = psta; + prm->q.diag_token = pdiag_body[2]; + prm->q.rpt = le16_to_cpu(*(u16*)(&pdiag_body[3])); + + /* Figure 8-104 Measurement Requested format */ + prm->q.e_id = pmeas_body[0]; + prm->q.m_token = pmeas_body[2]; + prm->q.m_mode = pmeas_body[3]; + prm->q.m_type = pmeas_body[4]; + prm->rmid = rm_gen_rmid(padapter, prm, RM_SLAVE); + + RTW_INFO("RM: rmid=%x, bssid " MAC_FMT "\n", prm->rmid, + MAC_ARG(prm->psta->cmn.mac_addr)); + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: element_id = %d\n", prm->q.e_id); + RTW_INFO("RM: length = %d\n", (int)pmeas_body[1]); + RTW_INFO("RM: meas_token = %d\n", prm->q.m_token); + RTW_INFO("RM: meas_mode = %d\n", prm->q.m_mode); + RTW_INFO("RM: meas_type = %d\n", prm->q.m_type); +#endif + + if (prm->q.e_id != _MEAS_REQ_IE_) /* 38 */ + return _FALSE; + + switch (prm->q.m_type) { + case bcn_req: + RTW_INFO("RM: recv beacon_request\n"); + switch (prm->q.m_mode) { + case bcn_req_passive: + rm_en_cap_chk_and_set(prm, RM_BCN_PASSIVE_MEAS_CAP_EN); + break; + case bcn_req_active: + rm_en_cap_chk_and_set(prm, RM_BCN_ACTIVE_MEAS_CAP_EN); + break; + case bcn_req_bcn_table: + rm_en_cap_chk_and_set(prm, RM_BCN_TABLE_MEAS_CAP_EN); + break; + default: + rm_set_rep_mode(prm, MEAS_REP_MOD_INCAP); + break; + } + break; + case ch_load_req: + RTW_INFO("RM: recv ch_load_request\n"); + rm_en_cap_chk_and_set(prm, RM_CH_LOAD_CAP_EN); + break; + case noise_histo_req: + RTW_INFO("RM: recv noise_histogram_request\n"); + rm_en_cap_chk_and_set(prm, RM_NOISE_HISTO_CAP_EN); + break; + default: + RTW_INFO("RM: recv unknown request type 0x%02x\n", + prm->q.m_type); + rm_set_rep_mode(prm, MEAS_REP_MOD_INCAP); + goto done; + } + rm_parse_meas_req(prm, pmeas_body); +done: + if (!update) + rm_enqueue_rmobj(padapter, prm, _FALSE); + + return _SUCCESS; +} + +/* receive measurement report */ +int rm_recv_radio_mens_rep(_adapter *padapter, + union recv_frame *precv_frame, struct sta_info *psta) +{ + int len, ret = _FALSE; + struct rm_obj *prm; + u32 rmid; + u8 *pdiag_body = (u8 *)(precv_frame->u.hdr.rx_data + + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 *pmeas_body = &pdiag_body[3]; + + + rmid = psta->cmn.aid << 16 + | pdiag_body[2] << 8 + | RM_MASTER; + + prm = rm_get_rmobj(padapter, rmid); + if (prm == NULL) { + /* not belong to us, report to upper */ + rtw_cfg80211_rx_rrm_action(psta->padapter, precv_frame); + return _TRUE; + } + + prm->p.action_code = pdiag_body[1]; + prm->p.diag_token = pdiag_body[2]; + + /* Figure 8-140 Measuremnt Report format */ + prm->p.e_id = pmeas_body[0]; + prm->p.m_token = pmeas_body[2]; + prm->p.m_mode = pmeas_body[3]; + prm->p.m_type = pmeas_body[4]; + + RTW_INFO("RM: rmid=%x, bssid " MAC_FMT "\n", prm->rmid, + MAC_ARG(prm->psta->cmn.mac_addr)); + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: element_id = %d\n", prm->p.e_id); + RTW_INFO("RM: length = %d\n", (int)pmeas_body[1]); + RTW_INFO("RM: meas_token = %d\n", prm->p.m_token); + RTW_INFO("RM: meas_mode = %d\n", prm->p.m_mode); + RTW_INFO("RM: meas_type = %d\n", prm->p.m_type); +#endif + if (prm->p.e_id != _MEAS_RSP_IE_) /* 39 */ + return _FALSE; + + RTW_INFO("RM: recv %s\n", rm_type_rep_name(prm->p.m_type)); + rm_post_event(padapter, prm->rmid, RM_EV_recv_rep); + + /* report to upper via ioctl */ + if ((prm->from_ioctl == true) && + prm->q.m_type == bcn_req) { + len = pmeas_body[1] + 2; /* 2 : EID(1B) length(1B) */ + indicate_beacon_report(prm->psta->cmn.mac_addr, + 1, len, pmeas_body); + } + return ret; +} + +/* receive link measurement request */ +int rm_recv_link_mens_req(_adapter *padapter, + union recv_frame *precv_frame, struct sta_info *psta) +{ + struct rm_obj *prm; + struct rm_priv *prmpriv = &padapter->rmpriv; + u8 *pdiag_body = (u8 *)(precv_frame->u.hdr.rx_data + + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 *pmeas_body = &pdiag_body[3]; + u8 rmid, update = 0; + int i; + + + prm = rm_alloc_rmobj(padapter); + + if (prm == NULL) { + RTW_ERR("RM: unable to alloc rm obj for requeset\n"); + return _FALSE; + } + + prm->psta = psta; + prm->q.action_code = pdiag_body[1]; + prm->q.diag_token = pdiag_body[2]; + + prm->q.tx_pwr_used = pmeas_body[0]; + prm->q.tx_pwr_max = pmeas_body[1]; + prm->q.rx_pwr = precv_frame->u.hdr.attrib.phy_info.rx_power; + prm->q.rx_rate = hw_rate_to_m_rate(precv_frame->u.hdr.attrib.data_rate); + prm->q.rx_bw = precv_frame->u.hdr.attrib.bw; + prm->q.rx_rsni = rm_get_frame_rsni(prm, precv_frame); + prm->rmid = rm_gen_rmid(padapter, prm, RM_SLAVE); + + RTW_INFO("RM: rmid=%x, bssid" MAC_FMT " rx_pwr=%ddBm, rate=%s\n", + prm->rmid, MAC_ARG(prm->psta->cmn.mac_addr), prm->q.rx_pwr, + MGN_RATE_STR(prm->q.rx_rate)); + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: tx_pwr_used =%d dBm\n", prm->q.tx_pwr_used); + RTW_INFO("RM: tx_pwr_max =%d dBm\n", prm->q.tx_pwr_max); +#endif + + if (!update) + rm_enqueue_rmobj(padapter, prm, _FALSE); + + return _SUCCESS; +} + +/* receive link measurement report */ +int rm_recv_link_mens_rep(_adapter *padapter, + union recv_frame *precv_frame, struct sta_info *psta) +{ + int ret = _FALSE; + struct rm_obj *prm; + u32 rmid; + u8 *pdiag_body = (u8 *)(precv_frame->u.hdr.rx_data + + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 *pmeas_body = pdiag_body + 3; + s8 val; + + + rmid = psta->cmn.aid << 16 + | pdiag_body[2] << 8 + | RM_MASTER; + + prm = rm_get_rmobj(padapter, rmid); + if (prm == NULL) { + /* not belong to us, report to upper */ + rtw_cfg80211_rx_rrm_action(psta->padapter, precv_frame); + return _TRUE; + } + + RTW_INFO("RM: rmid=%x, bssid " MAC_FMT "\n", prm->rmid, + MAC_ARG(prm->psta->cmn.mac_addr)); + + prm->p.action_code = pdiag_body[1]; + prm->p.diag_token = pdiag_body[2]; + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: action_code = %d\n", prm->p.action_code); + RTW_INFO("RM: diag_token = %d\n", prm->p.diag_token); + RTW_INFO("RM: xmit_power = %d dBm\n", pmeas_body[2]); + RTW_INFO("RM: link_margin = %d dBm\n", pmeas_body[3]); + RTW_INFO("RM: xmit_ant = %d\n", pmeas_body[4]); + RTW_INFO("RM: recv_ant = %d\n", pmeas_body[5]); + RTW_INFO("RM: RCPI = %d\n", pmeas_body[6]); + RTW_INFO("RM: RSNI = %d\n", pmeas_body[7]); +#endif + RTW_INFO("RM: recv link meas report ...\n"); + ret = rm_post_event(padapter, prm->rmid, RM_EV_recv_rep); + + return ret; +} + + +int rm_radio_mens_nb_rep(_adapter *padapter, + union recv_frame *precv_frame, struct sta_info *psta) +{ + u8 *pdiag_body = (u8 *)(precv_frame->u.hdr.rx_data + + sizeof(struct rtw_ieee80211_hdr_3addr)); + u8 *pmeas_body = &pdiag_body[3]; + u32 len = precv_frame->u.hdr.len; + u32 rmid; + struct rm_obj *prm; + + + rmid = psta->cmn.aid << 16 + | pdiag_body[2] << 8 + | RM_MASTER; + + prm = rm_get_rmobj(padapter, rmid); + + if (prm == NULL) { + /* not belong to us, report to upper */ + rtw_cfg80211_rx_rrm_action(psta->padapter, precv_frame); + return _TRUE; + } + + prm->p.action_code = pdiag_body[1]; + prm->p.diag_token = pdiag_body[2]; + prm->p.e_id = pmeas_body[0]; + + RTW_INFO("RM: rmid=%x, bssid " MAC_FMT "\n", prm->rmid, + MAC_ARG(prm->psta->cmn.mac_addr)); + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: element_id = %d\n", prm->p.e_id); + RTW_INFO("RM: length = %d\n", (int)pmeas_body[1]); +#endif + rm_post_event(padapter, prm->rmid, RM_EV_recv_rep); + +#ifdef CONFIG_LAYER2_ROAMING + if (rtw_wnm_btm_candidates_survey(padapter + ,(pdiag_body + 3) + ,(len - sizeof(struct rtw_ieee80211_hdr_3addr)) + ,_FALSE) == _FAIL) + return _FALSE; +#endif + rtw_cfg80211_rx_rrm_action(padapter, precv_frame); + + return _TRUE; +} + +unsigned int rm_on_action(_adapter *padapter, union recv_frame *precv_frame) +{ + u32 ret = _FAIL; + u8 *pframe = NULL; + u8 *pframe_body = NULL; + u8 action_code = 0; + u8 diag_token = 0; + struct rtw_ieee80211_hdr_3addr *whdr; + struct sta_info *psta; + + + pframe = precv_frame->u.hdr.rx_data; + + /* check RA matches or not */ + if (!_rtw_memcmp(adapter_mac_addr(padapter), + GetAddr1Ptr(pframe), ETH_ALEN)) + goto exit; + + whdr = (struct rtw_ieee80211_hdr_3addr *)pframe; + RTW_INFO("RM: %s bssid = " MAC_FMT "\n", + __func__, MAC_ARG(whdr->addr2)); + + psta = rtw_get_stainfo(&padapter->stapriv, whdr->addr2); + + if (!psta) { + RTW_ERR("RM: psta not found\n"); + goto exit; + } + + pframe_body = (unsigned char *)(pframe + + sizeof(struct rtw_ieee80211_hdr_3addr)); + + /* Figure 8-438 radio measurement request frame Action field format */ + /* Category = pframe_body[0] = 5 (Radio Measurement) */ + action_code = pframe_body[1]; + diag_token = pframe_body[2]; + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: %s radio_action=%x, diag_token=%x\n", __func__, + action_code, diag_token); +#endif + + switch (action_code) { + + case RM_ACT_RADIO_MEAS_REQ: + RTW_INFO("RM: RM_ACT_RADIO_MEAS_REQ\n"); + ret = rm_recv_radio_mens_req(padapter, precv_frame, psta); + break; + + case RM_ACT_RADIO_MEAS_REP: + RTW_INFO("RM: RM_ACT_RADIO_MEAS_REP\n"); + ret = rm_recv_radio_mens_rep(padapter, precv_frame, psta); + break; + + case RM_ACT_LINK_MEAS_REQ: + RTW_INFO("RM: RM_ACT_LINK_MEAS_REQ\n"); + ret = rm_recv_link_mens_req(padapter, precv_frame, psta); + break; + + case RM_ACT_LINK_MEAS_REP: + RTW_INFO("RM: RM_ACT_LINK_MEAS_REP\n"); + ret = rm_recv_link_mens_rep(padapter, precv_frame, psta); + break; + + case RM_ACT_NB_REP_REQ: + RTW_INFO("RM: RM_ACT_NB_REP_REQ\n"); + break; + + case RM_ACT_NB_REP_RESP: + RTW_INFO("RM: RM_ACT_NB_REP_RESP\n"); + ret = rm_radio_mens_nb_rep(padapter, precv_frame, psta); + break; + + default: + /* TODO reply incabable */ + RTW_ERR("RM: unknown specturm management action %2x\n", + action_code); + break; + } +exit: + return ret; +} + +static u8 *rm_gen_bcn_detail_elem(_adapter *padapter, u8 *pframe, + struct rm_obj *prm, struct wlan_network *pnetwork, + unsigned int *fr_len) +{ + WLAN_BSSID_EX *pbss = &pnetwork->network; + unsigned int my_len; + int j, k, len; + u8 *plen; + u8 *ptr; + u8 val8, eid; + + + my_len = 0; + /* Reporting Detail values + * 0: No fixed length fields or elements + * 1: All fixed length fields and any requested elements + * in the Request info element if present + * 2: All fixed length fields and elements + * 3-255: Reserved + */ + + /* report_detail = 0 */ + if (prm->q.opt.bcn.rep_detail == 0 + || prm->q.opt.bcn.rep_detail > 2) { + return pframe; + } + + /* ID */ + val8 = 1; /* 1:reported frame body */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + plen = pframe; + val8 = 0; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* report_detail = 2 */ + if (prm->q.opt.bcn.rep_detail == 2) { + pframe = rtw_set_fixed_ie(pframe, pbss->IELength - 4, + pbss->IEs, &my_len); /* -4 remove FCS */ + goto done; + } + + /* report_detail = 1 */ + /* all fixed lenght fields */ + pframe = rtw_set_fixed_ie(pframe, + _FIXED_IE_LENGTH_, pbss->IEs, &my_len); + + for (j = 0; j < prm->q.opt.bcn.opt_id_num; j++) { + switch (prm->q.opt.bcn.opt_id[j]) { + case bcn_req_ssid: + /* SSID */ +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: bcn_req_ssid\n"); +#endif + pframe = rtw_set_ie(pframe, _SSID_IE_, + pbss->Ssid.SsidLength, + pbss->Ssid.Ssid, &my_len); + break; + case bcn_req_req: + if (prm->q.opt.bcn.req_start == NULL) + break; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: bcn_req_req"); +#endif + for (k=0; kq.opt.bcn.req_len; k++) { + eid = prm->q.opt.bcn.req_start[k]; + + val8 = pbss->IELength - _FIXED_IE_LENGTH_; + ptr = rtw_get_ie(pbss->IEs + _FIXED_IE_LENGTH_, + eid, &len, val8); + + if (!ptr) + continue; +#if (RM_MORE_DBG_MSG) + switch (eid) { + case EID_SsId: + RTW_INFO("RM: EID_SSID\n"); + break; + case EID_QBSSLoad: + RTW_INFO("RM: EID_QBSSLoad\n"); + break; + case EID_HTCapability: + RTW_INFO("RM: EID_HTCapability\n"); + break; + case _MDIE_: + RTW_INFO("RM: EID_MobilityDomain\n"); + break; + case EID_Vendor: + RTW_INFO("RM: EID_Vendor\n"); + break; + default: + RTW_INFO("RM: EID %d todo\n",eid); + break; + } +#endif + pframe = rtw_set_ie(pframe, eid, + len, ptr+2, &my_len); + } /* for() */ + break; + case bcn_req_rep_detail: + RTW_INFO("RM: bcn_req_rep_detail\n"); + break; + case bcn_req_ap_ch_rep: + RTW_INFO("RM: bcn_req_ap_ch_rep\n"); + break; + default: + RTW_INFO("RM: OPT %d TODO\n",prm->q.opt.bcn.opt_id[j]); + break; + } + } +done: + /* + * update my length + * content length does NOT include ID and LEN + */ + val8 = my_len - 2; + rtw_set_fixed_ie(plen, 1, &val8, &j); + + /* update length to caller */ + *fr_len += my_len; + + return pframe; +} + +u8 rm_bcn_req_cond_mach(struct rm_obj *prm, struct wlan_network *pnetwork) +{ + u8 val8; + + + switch(prm->q.opt.bcn.rep_cond.cond) { + case bcn_rep_cond_immediately: + return _SUCCESS; + case bcn_req_cond_rcpi_greater: + val8 = rm_get_bcn_rcpi(prm, pnetwork); + if (val8 > prm->q.opt.bcn.rep_cond.threshold) + return _SUCCESS; + break; + case bcn_req_cond_rcpi_less: + val8 = rm_get_bcn_rcpi(prm, pnetwork); + if (val8 < prm->q.opt.bcn.rep_cond.threshold) + return _SUCCESS; + break; + case bcn_req_cond_rsni_greater: + val8 = rm_get_bcn_rsni(prm, pnetwork); + if (val8 != 255 && val8 > prm->q.opt.bcn.rep_cond.threshold) + return _SUCCESS; + break; + case bcn_req_cond_rsni_less: + val8 = rm_get_bcn_rsni(prm, pnetwork); + if (val8 != 255 && val8 < prm->q.opt.bcn.rep_cond.threshold) + return _SUCCESS; + break; + default: + RTW_ERR("RM: bcn_req cond %u not support\n", + prm->q.opt.bcn.rep_cond.cond); + break; + } + return _FALSE; +} + +static u8 *rm_gen_bcn_rep_ie (struct rm_obj *prm, + u8 *pframe, struct wlan_network *pnetwork, unsigned int *fr_len) +{ + int snr, i; + u8 val8, *plen; + u16 val16; + u32 val32; + u64 val64; + unsigned int my_len; + _adapter *padapter = prm->psta->padapter; + + + my_len = 0; + plen = pframe + 1; + pframe = rtw_set_fixed_ie(pframe, 7, &prm->p.e_id, &my_len); + + /* Actual Measurement StartTime */ + val64 = cpu_to_le64(prm->meas_start_time); + pframe = rtw_set_fixed_ie(pframe, 8, (u8 *)&val64, &my_len); + + /* Measurement Duration */ + val16 = prm->meas_end_time - prm->meas_start_time; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, 2, (u8*)&val16, &my_len); + + /* TODO + * ReportedFrameInformation: + * 0 :beacon or probe rsp + * 1 :pilot frame + */ + val8 = 0; /* report frame info */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* RCPI */ + val8 = rm_get_bcn_rcpi(prm, pnetwork); + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* RSNI */ + val8 = rm_get_bcn_rsni(prm, pnetwork); + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* BSSID */ + pframe = rtw_set_fixed_ie(pframe, 6, + (u8 *)&pnetwork->network.MacAddress, &my_len); + + /* + * AntennaID + * 0: unknown + * 255: multiple antenna (Diversity) + */ + val8 = 0; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* ParentTSF */ + val32 = pnetwork->network.PhyInfo.free_cnt; + if (prm->free_run_counter_valid) + val32 += prm->meas_start_time; + + pframe = rtw_set_fixed_ie(pframe, 4, (u8 *)&val32, &my_len); + + /* Generate Beacon detail */ + pframe = rm_gen_bcn_detail_elem(padapter, pframe, + prm, pnetwork, &my_len); + /* + * update my length + * content length does NOT include ID and LEN + */ + val8 = my_len - 2; + rtw_set_fixed_ie(plen, 1, &val8, &i); + + /* update length to caller */ + *fr_len += my_len; + + return pframe; +} + +#if 0 /* check MBO logo */ +static int rm_match_sub_elem(_adapter *padapter, + struct rm_obj *prm, struct wlan_network *pnetwork) +{ + WLAN_BSSID_EX *pbss = &pnetwork->network; + unsigned int my_len; + int j, k, len; + u8 *plen; + u8 *ptr; + u8 val8, eid; + + + my_len = 0; + /* Reporting Detail values + * 0: No fixed length fields or elements + * 1: All fixed length fields and any requested elements + * in the Request info element if present + * 2: All fixed length fields and elements + * 3-255: Reserved + */ + + /* report_detail != 1 */ + if (prm->q.opt.bcn.rep_detail != 1) + return _TRUE; + + /* report_detail = 1 */ + + for (j = 0; j < prm->q.opt.bcn.opt_id_num; j++) { + switch (prm->q.opt.bcn.opt_id[j]) { + case bcn_req_ssid: + /* SSID */ +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: bcn_req_ssid\n"); +#endif + if (pbss->Ssid.SsidLength == 0) + return _FALSE; + break; + case bcn_req_req: + if (prm->q.opt.bcn.req_start == NULL) + break; +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: bcn_req_req"); +#endif + for (k=0; kq.opt.bcn.req_len; k++) { + eid = prm->q.opt.bcn.req_start[k]; + + val8 = pbss->IELength - _FIXED_IE_LENGTH_; + ptr = rtw_get_ie(pbss->IEs + _FIXED_IE_LENGTH_, + eid, &len, val8); + +#if (RM_MORE_DBG_MSG) + switch (eid) { + case EID_SsId: + RTW_INFO("RM: EID_SSID\n"); + break; + case EID_QBSSLoad: + RTW_INFO("RM: EID_QBSSLoad\n"); + break; + case EID_HTCapability: + RTW_INFO("RM: EID_HTCapability\n"); + break; + case _MDIE_: + RTW_INFO("RM: EID_MobilityDomain\n"); + break; + case EID_Vendor: + RTW_INFO("RM: EID_Vendor\n"); + break; + default: + RTW_INFO("RM: EID %d todo\n",eid); + break; + } +#endif + if (!ptr) { + RTW_INFO("RM: EID %d not found\n",eid); + return _FALSE; + } + } /* for() */ + break; + case bcn_req_rep_detail: + RTW_INFO("RM: bcn_req_rep_detail\n"); + break; + case bcn_req_ap_ch_rep: + RTW_INFO("RM: bcn_req_ap_ch_rep\n"); + break; + default: + RTW_INFO("RM: OPT %d TODO\n",prm->q.opt.bcn.opt_id[j]); + break; + } + } + return _TRUE; +} +#endif + +static int retrieve_scan_result(struct rm_obj *prm) +{ + _irqL irqL; + _list *plist, *phead; + _queue *queue; + _adapter *padapter = prm->psta->padapter; + struct rtw_ieee80211_channel *pch_set; + struct wlan_network *pnetwork = NULL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + int i; + PWLAN_BSSID_EX pbss; + unsigned int matched_network; + int len, my_len; + u8 buf_idx, *pbuf = NULL, *tmp_buf = NULL; + + + tmp_buf = rtw_malloc(MAX_XMIT_EXTBUF_SZ); + if (tmp_buf == NULL) + return 0; + + my_len = 0; + buf_idx = 0; + matched_network = 0; + queue = &(pmlmepriv->scanned_queue); + + _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + phead = get_list_head(queue); + plist = get_next(phead); + + /* get requested measurement channel set */ + pch_set = prm->q.ch_set; + + /* search scan queue to find requested SSID */ + while (1) { + + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + pbss = &pnetwork->network; + +#if 0 + RTW_INFO("RM: ooo ch %u ssid %s bssid "MAC_FMT"\n", + pbss->Configuration.DSConfig, pbss->Ssid.Ssid, + MAC_ARG(pbss->MacAddress)); + /* + * report network if requested channel set contains + * the channel matchs selected network + */ + if (rtw_chset_search_ch(adapter_to_chset(padapter), + pbss->Configuration.DSConfig) < 0) /* not match */ + goto next; + + if (rtw_mlme_band_check(padapter, pbss->Configuration.DSConfig) + == _FALSE) + goto next; +#endif + if (rtw_validate_ssid(&(pbss->Ssid)) == _FALSE) + goto next; + + /* match bssid */ + if (is_wildcard_bssid(prm->q.bssid) == FALSE) + if (_rtw_memcmp(prm->q.bssid, + pbss->MacAddress, 6) == _FALSE) + //continue; + goto next; + /* + * default wildcard SSID. wildcard SSID: + * A SSID value (null) used to represent all SSIDs + */ + + /* match ssid */ + if ((prm->q.opt.bcn.ssid.SsidLength > 0) && + _rtw_memcmp(prm->q.opt.bcn.ssid.Ssid, + pbss->Ssid.Ssid, + prm->q.opt.bcn.ssid.SsidLength) == _FALSE) + goto next; + + /* go through measurement requested channels */ + for (i = 0; i < prm->q.ch_set_ch_amount; i++) { + if ((pch_set[i].hw_value) == + (pbss->Configuration.DSConfig)) /* match ch */ + break; + } + if (i >= prm->q.ch_set_ch_amount) /* channel mismatch */ + goto next; + + /* match condition */ + if (rm_bcn_req_cond_mach(prm, pnetwork) == _FALSE) { + RTW_INFO("RM: condition mismatch ch %u ssid %s bssid "MAC_FMT"\n", + pbss->Configuration.DSConfig, pbss->Ssid.Ssid, + MAC_ARG(pbss->MacAddress)); + RTW_INFO("RM: condition %u:%u\n", + prm->q.opt.bcn.rep_cond.cond, + prm->q.opt.bcn.rep_cond.threshold); + goto next; + //continue; + } +#if 0 /* check MBO logo */ + /* match subelement */ + if (rm_match_sub_elem(padapter, prm, pnetwork) == _FALSE) + goto next; +#endif + /* Found a matched SSID */ + matched_network++; + + RTW_INFO("RM: ch %u Found %s bssid "MAC_FMT"\n", + pbss->Configuration.DSConfig, pbss->Ssid.Ssid, + MAC_ARG(pbss->MacAddress)); + + len = 0; + _rtw_memset(tmp_buf, 0, MAX_XMIT_EXTBUF_SZ); + rm_gen_bcn_rep_ie(prm, tmp_buf, pnetwork, &len); +new_packet: + if (my_len == 0) { + pbuf = rtw_malloc(MAX_XMIT_EXTBUF_SZ); + if (pbuf == NULL) + goto fail; + prm->buf[buf_idx].pbuf = pbuf; + } + + if ((MAX_XMIT_EXTBUF_SZ - (my_len+len+24+4)) > 0) { + pbuf = rtw_set_fixed_ie(pbuf, + len, tmp_buf, &my_len); + prm->buf[buf_idx].len = my_len; + } else { + if (my_len == 0) /* not enough space */ + goto fail; + + my_len = 0; + buf_idx++; + goto new_packet; + } +next: + plist = get_next(plist); + } /* while() */ +fail: + _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL); + + if (tmp_buf) + rtw_mfree(tmp_buf, MAX_XMIT_EXTBUF_SZ); + + RTW_INFO("RM: Found %d matched %s\n", matched_network, + prm->q.opt.bcn.ssid.Ssid); + + if (prm->buf[buf_idx].pbuf) + return buf_idx+1; + + return 0; +} + +int issue_beacon_rep(struct rm_obj *prm) +{ + int i, my_len; + u8 *pframe; + _adapter *padapter = prm->psta->padapter; + struct pkt_attrib *pattr; + struct xmit_frame *pmgntframe; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + int pkt_num; + + + pkt_num = retrieve_scan_result(prm); + + if (pkt_num == 0) { + issue_null_reply(prm); + return _SUCCESS; + } + + for (i=0;iattrib; + pframe = build_wlan_hdr(padapter, + pmgntframe, prm->psta, WIFI_ACTION); + pframe = rtw_set_fixed_ie(pframe, + 3, &prm->p.category, &pattr->pktlen); + + my_len = 0; + pframe = rtw_set_fixed_ie(pframe, + prm->buf[i].len, prm->buf[i].pbuf, &my_len); + + pattr->pktlen += my_len; + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + } +fail: + for (i=0;ibuf[i].pbuf) { + rtw_mfree(prm->buf[i].pbuf, MAX_XMIT_EXTBUF_SZ); + prm->buf[i].pbuf = NULL; + prm->buf[i].len = 0; + } + } + return _SUCCESS; +} + +/* neighbor request */ +int issue_nb_req(struct rm_obj *prm) +{ + _adapter *padapter = prm->psta->padapter; + struct sta_info *psta = prm->psta; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_frame *pmgntframe = NULL; + struct pkt_attrib *pattr = NULL; + u8 val8; + u8 *pframe = NULL; + + + RTW_INFO("RM: %s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_ERR("RM: %s alloc xmit_frame fail\n",__func__); + return _FALSE; + } + pattr = &pmgntframe->attrib; + pframe = build_wlan_hdr(padapter, pmgntframe, psta, WIFI_ACTION); + pframe = rtw_set_fixed_ie(pframe, + 3, &prm->q.category, &pattr->pktlen); + + if (prm->q.pssid) { + + u8 sub_ie[64] = {0}; + u8 *pie = &sub_ie[2]; + + RTW_INFO("RM: Send NB Req to "MAC_FMT" for(SSID) %s searching\n", + MAC_ARG(pmlmepriv->cur_network.network.MacAddress), + pmlmepriv->cur_network.network.Ssid.Ssid); + + val8 = strlen(prm->q.pssid); + sub_ie[0] = 0; /*SSID*/ + sub_ie[1] = val8; + + _rtw_memcpy(pie, prm->q.pssid, val8); + + pframe = rtw_set_fixed_ie(pframe, val8 + 2, + sub_ie, &pattr->pktlen); + } else { + + if (!pmlmepriv->cur_network.network.Ssid.SsidLength) + RTW_INFO("RM: Send NB Req to "MAC_FMT"\n", + MAC_ARG(pmlmepriv->cur_network.network.MacAddress)); + else { + u8 sub_ie[64] = {0}; + u8 *pie = &sub_ie[2]; + + RTW_INFO("RM: Send NB Req to "MAC_FMT" for(SSID) %s searching\n", + MAC_ARG(pmlmepriv->cur_network.network.MacAddress), + pmlmepriv->cur_network.network.Ssid.Ssid); + + sub_ie[0] = 0; /*SSID*/ + sub_ie[1] = pmlmepriv->cur_network.network.Ssid.SsidLength; + + _rtw_memcpy(pie, pmlmepriv->cur_network.network.Ssid.Ssid, + pmlmepriv->cur_network.network.Ssid.SsidLength); + + pframe = rtw_set_fixed_ie(pframe, + pmlmepriv->cur_network.network.Ssid.SsidLength + 2, + sub_ie, &pattr->pktlen); + } + } + + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + + return _SUCCESS; +} + +/* issue link measurement request */ +int issue_link_meas_req(struct rm_obj *prm) +{ + _adapter *padapter = prm->psta->padapter; + struct sta_info *psta = prm->psta; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_frame *pmgntframe = NULL; + struct pkt_attrib *pattr = NULL; + u8 *pframe = NULL; + s8 pwr_used, path_a_pwr; + + + RTW_INFO("RM: %s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_ERR("RM: %s alloc xmit_frame fail\n",__func__); + return _FALSE; + } + pattr = &pmgntframe->attrib; + pframe = build_wlan_hdr(padapter, pmgntframe, psta, WIFI_ACTION); + + /* Category, Action code, Dialog token */ + pframe = rtw_set_fixed_ie(pframe, + 3, &prm->q.category, &pattr->pktlen); + + /* xmit power used */ + /* we don't know actual TX power due to RA may change TX rate; + * But if we fix TX rate then we can get specific tx power + */ + pattr->rate = MGN_6M; + rm_get_tx_power(padapter, RF_PATH_A, MGN_6M, &pwr_used); + pframe = rtw_set_fixed_ie(pframe, + 1, &pwr_used, &pattr->pktlen); + + /* Max xmit power */ + rm_get_path_a_max_tx_power(padapter, &path_a_pwr); + pframe = rtw_set_fixed_ie(pframe, + 1, &path_a_pwr, &pattr->pktlen); + + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + + return _SUCCESS; +} + +/* issue link measurement report */ +int issue_link_meas_rep(struct rm_obj *prm) +{ + u8 val8; + u8 *pframe; + unsigned int my_len; + _adapter *padapter = prm->psta->padapter; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattr; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct sta_info *psta = prm->psta; + int i; + u8 tpc[4]; + s8 pwr_used; + + + RTW_INFO("RM: %s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_ERR("RM: ERR %s alloc xmit_frame fail\n",__func__); + return _FALSE; + } + pattr = &pmgntframe->attrib; + pframe = build_wlan_hdr(padapter, pmgntframe, psta, WIFI_ACTION); + /* Category, action code, Dialog token */ + pframe = rtw_set_fixed_ie(pframe, 3, + &prm->p.category, &pattr->pktlen); + + my_len = 0; + + /* TPC report */ + rm_get_tx_power(padapter, RF_PATH_A, MGN_6M, &pwr_used); + tpc[0] = EID_TPC; + tpc[1] = 2; /* length */ + + /* TX power */ + tpc[2] = pwr_used; + + /* link margin */ + rm_get_rx_sensitivity(padapter, prm->q.rx_bw, prm->q.rx_rate, &pwr_used); + tpc[3] = prm->q.rx_pwr - pwr_used; /* RX sensitivity */ + pattr->rate = MGN_6M; /* use fix rate to get fixed RX sensitivity */ + +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: rx_pwr=%ddBm - rx_sensitivity=%ddBm = link_margin=%ddB\n", + prm->q.rx_pwr, pwr_used, tpc[3]); +#endif + pframe = rtw_set_fixed_ie(pframe, 4, tpc, &my_len); + + /* RECV antenna ID */ + val8 = 0; /* unknown antenna */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* XMIT antenna ID */ + /* Fix rate 6M(1T) always use main antenna to TX */ + val8 = 1; /* main antenna */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* RCPI */ + val8 = translate_dbm_to_rcpi(prm->q.rx_pwr); + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* RSNI */ + val8 = prm->q.rx_rsni; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* length */ + //val8 = (u8)my_len-2; + //rtw_set_fixed_ie(plen, 1, &val8, &i); /* use variable i to ignore it */ + + pattr->pktlen += my_len; + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + + return _SUCCESS; +} + +static u8 *rm_gen_bcn_req_s_elem(_adapter *padapter, + struct rm_obj *prm, u8 *pframe, unsigned int *fr_len) +{ + u8 val8, l; + int i; + unsigned int my_len = 0; + struct _RT_OPERATING_CLASS *op; + + /* meas mode */ + val8 = bcn_req_active; /* measurement mode T8-64 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* bssid */ + pframe = rtw_set_fixed_ie(pframe, 6, prm->q.bssid, &my_len); + + /* + * opt ssid (0) + */ + l = MIN(32, (int)prm->q.opt.bcn.ssid.SsidLength); + + l = (int)prm->q.opt.bcn.ssid.SsidLength; + + if (l > 32) + RTW_ERR("RM: %s SSID len over size %d! skip it!\n",__func__, l); + + if (l > 0 && l <= 32) { + /* Type */ + val8 = bcn_req_ssid; + pframe = rtw_set_fixed_ie(pframe, 1, + &val8, &my_len); + /* Len */ + pframe = rtw_set_fixed_ie(pframe, 1, + &l, &my_len); + /* Value */ + pframe = rtw_set_fixed_ie(pframe, l, + prm->q.opt.bcn.ssid.Ssid, &my_len); + } + + /* + * opt reporting detail (2) + */ + /* Type */ + val8 = bcn_req_rep_detail; + pframe = rtw_set_fixed_ie(pframe, 1, + &val8, &my_len); + /* Len */ + l = 1; + pframe = rtw_set_fixed_ie(pframe, 1, + &l, &my_len); + /* Value */ + pframe = rtw_set_fixed_ie(pframe, l, + &prm->q.opt.bcn.rep_detail, &my_len); + + /* + * opt request (10) + */ + + if (prm->q.opt.bcn.req_id_num > 0) { + /* Type */ + val8 = bcn_req_req; + pframe = rtw_set_fixed_ie(pframe, 1, + &val8, &my_len); + /* Len */ + l = prm->q.opt.bcn.req_id_num; + pframe = rtw_set_fixed_ie(pframe, 1, + &l, &my_len); + /* Value */ + pframe = rtw_set_fixed_ie(pframe, l, + prm->q.opt.bcn.req_id, &my_len); + } + + /* + * opt ap channel report (51) + */ + for (i = 0; i < prm->q.opt.bcn.ap_ch_rpt_num; i++) { + op = prm->q.opt.bcn.ap_ch_rpt[i]; + if (op == NULL) + break; + /* Type */ + val8 = bcn_req_ap_ch_rep; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + l = (u8)op->Len + 1; + /* length */ + pframe = rtw_set_fixed_ie(pframe, 1, &l, &my_len); + + /* op class */ + val8 = op->global_op_class; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + /* channel */ + pframe = rtw_set_fixed_ie(pframe, op->Len, op->Channel, &my_len); + } + + /* update length to caller */ + *fr_len += my_len; + + /* optional subelements */ + return pframe; +} + +static u8 *rm_gen_ch_load_req_s_elem(_adapter *padapter, + u8 *pframe, unsigned int *fr_len) +{ + u8 val8; + unsigned int my_len = 0; + + + val8 = 1; /* 1: channel load T8-60 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + val8 = 2; /* channel load length = 2 (extensible) */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + val8 = 0; /* channel load condition : 0 (issue when meas done) T8-61 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + val8 = 0; /* channel load reference value : 0 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* update length to caller */ + *fr_len += my_len; + + return pframe; +} + +static u8 *rm_gen_noise_histo_req_s_elem(_adapter *padapter, + u8 *pframe, unsigned int *fr_len) +{ + u8 val8; + unsigned int my_len = 0; + + + val8 = 1; /* 1: noise histogram T8-62 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + val8 = 2; /* noise histogram length = 2 (extensible) */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + val8 = 0; /* noise histogram condition : 0 (issue when meas done) T8-63 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + val8 = 0; /* noise histogram reference value : 0 */ + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + + /* update length to caller */ + *fr_len += my_len; + + return pframe; +} + +int issue_radio_meas_req(struct rm_obj *prm) +{ + u8 val8; + u8 *pframe; + u8 *plen; + u16 val16; + int my_len, i; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattr; + _adapter *padapter = prm->psta->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + + RTW_INFO("RM: %s - %s\n", __func__, rm_type_req_name(prm->q.m_type)); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_ERR("RM: %s alloc xmit_frame fail\n",__func__); + return _FALSE; + } + pattr = &pmgntframe->attrib; + pframe = build_wlan_hdr(padapter, pmgntframe, prm->psta, WIFI_ACTION); + + /* Category, Action code, Dialog token */ + pframe = rtw_set_fixed_ie(pframe, 3, &prm->q.category, &pattr->pktlen); + + /* repeat */ + val16 = cpu_to_le16(prm->q.rpt); + pframe = rtw_set_fixed_ie(pframe, 2, + (unsigned char *)&(val16), &pattr->pktlen); + + my_len = 0; + plen = pframe + 1; + /* Element ID, Length, Meas token, Meas Mode, Meas type, op class, ch */ + pframe = rtw_set_fixed_ie(pframe, 7, &prm->q.e_id, &my_len); + + /* random interval */ + val16 = cpu_to_le16(prm->q.rand_intvl); /* TU */ + pframe = rtw_set_fixed_ie(pframe, 2, (u8 *)&val16, &my_len); + + /* measurement duration */ + val16 = cpu_to_le16(prm->q.meas_dur); + pframe = rtw_set_fixed_ie(pframe, 2, (u8 *)&val16, &my_len); + + /* optional subelement */ + switch (prm->q.m_type) { + case bcn_req: + pframe = rm_gen_bcn_req_s_elem(padapter, prm, pframe, &my_len); + break; + case ch_load_req: + pframe = rm_gen_ch_load_req_s_elem(padapter, pframe, &my_len); + break; + case noise_histo_req: + pframe = rm_gen_noise_histo_req_s_elem(padapter, + pframe, &my_len); + break; + case basic_req: + default: + break; + } + + /* length */ + val8 = (u8)my_len - 2; + rtw_set_fixed_ie(plen, 1, &val8, &i); + + pattr->pktlen += my_len; + + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + + return _SUCCESS; +} + +int rm_radio_meas_report_cond(struct rm_obj *prm) +{ + u8 val8; + int i; + + + switch (prm->q.m_type) { + case ch_load_req: + + val8 = prm->p.ch_load; + switch (prm->q.opt.clm.rep_cond.cond) { + case ch_load_cond_immediately: + return _SUCCESS; + case ch_load_cond_anpi_equal_greater: + if (val8 >= prm->q.opt.clm.rep_cond.threshold) + return _SUCCESS; + break; + case ch_load_cond_anpi_equal_less: + if (val8 <= prm->q.opt.clm.rep_cond.threshold) + return _SUCCESS; + break; + default: + break; + } + break; + case noise_histo_req: + val8 = prm->p.anpi; + switch (prm->q.opt.nhm.rep_cond.cond) { + case noise_histo_cond_immediately: + return _SUCCESS; + case noise_histo_cond_anpi_equal_greater: + if (val8 >= prm->q.opt.nhm.rep_cond.threshold) + return _SUCCESS; + break; + case noise_histo_cond_anpi_equal_less: + if (val8 <= prm->q.opt.nhm.rep_cond.threshold) + return _SUCCESS; + break; + default: + break; + } + break; + default: + break; + } + return _FAIL; +} + +int retrieve_radio_meas_result(struct rm_obj *prm) +{ + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(prm->psta->padapter); + int i, ch = -1; + u8 val8; + + + ch = rtw_chset_search_ch(adapter_to_chset(prm->psta->padapter), + prm->q.ch_num); + + if ((ch == -1) || (ch >= MAX_CHANNEL_NUM)) { + RTW_ERR("RM: get ch(CH:%d) fail\n", prm->q.ch_num); + ch = 0; + } + + switch (prm->q.m_type) { + case ch_load_req: +#ifdef CONFIG_RTW_ACS + val8 = hal_data->acs.clm_ratio[ch]; +#else + val8 = 0; +#endif + prm->p.ch_load = val8; + break; + case noise_histo_req: +#ifdef CONFIG_RTW_ACS + /* ANPI */ + prm->p.anpi = hal_data->acs.nhm_ratio[ch]; + + /* IPI 0~10 */ + for (i=0;i<11;i++) + prm->p.ipi[i] = hal_data->acs.nhm[ch][i]; + +#else + val8 = 0; + prm->p.anpi = val8; + for (i=0;i<11;i++) + prm->p.ipi[i] = val8; +#endif + break; + default: + break; + } + return _SUCCESS; +} + +int issue_radio_meas_rep(struct rm_obj *prm) +{ + u8 val8; + u8 *pframe; + u8 *plen; + u16 val16; + u64 val64; + unsigned int my_len; + _adapter *padapter = prm->psta->padapter; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattr; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct sta_info *psta = prm->psta; + int i; + + + RTW_INFO("RM: %s\n", __func__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + RTW_ERR("RM: ERR %s alloc xmit_frame fail\n",__func__); + return _FALSE; + } + pattr = &pmgntframe->attrib; + pframe = build_wlan_hdr(padapter, pmgntframe, psta, WIFI_ACTION); + pframe = rtw_set_fixed_ie(pframe, 3, + &prm->p.category, &pattr->pktlen); + + my_len = 0; + plen = pframe + 1; + pframe = rtw_set_fixed_ie(pframe, 7, &prm->p.e_id, &my_len); + + /* Actual Meas start time - 8 bytes */ + val64 = cpu_to_le64(prm->meas_start_time); + pframe = rtw_set_fixed_ie(pframe, 8, (u8 *)&val64, &my_len); + + /* measurement duration */ + val16 = prm->meas_end_time - prm->meas_start_time; + val16 = cpu_to_le16(val16); + pframe = rtw_set_fixed_ie(pframe, 2, (u8 *)&val16, &my_len); + + /* optional subelement */ + switch (prm->q.m_type) { + case ch_load_req: + val8 = prm->p.ch_load; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + break; + case noise_histo_req: + /* + * AntennaID + * 0: unknown + * 255: multiple antenna (Diversity) + */ + val8 = 0; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + /* ANPI */ + val8 = prm->p.anpi; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + /* IPI 0~10 */ + for (i=0;i<11;i++) { + val8 = prm->p.ipi[i]; + pframe = rtw_set_fixed_ie(pframe, 1, &val8, &my_len); + } + break; + default: + break; + } + /* length */ + val8 = (u8)my_len-2; + rtw_set_fixed_ie(plen, 1, &val8, &i); /* use variable i to ignore it */ + + pattr->pktlen += my_len; + pattr->last_txcmdsz = pattr->pktlen; + dump_mgntframe(padapter, pmgntframe); + + return _SUCCESS; +} + +void rtw_ap_parse_sta_rm_en_cap(_adapter *padapter, + struct sta_info *psta, struct rtw_ieee802_11_elems *elem) +{ + if (elem->rm_en_cap) { + RTW_INFO("assoc.rm_en_cap="RM_CAP_FMT"\n", + RM_CAP_ARG(elem->rm_en_cap)); + _rtw_memcpy(psta->rm_en_cap, + (elem->rm_en_cap), elem->rm_en_cap_len); + } +} + +void RM_IE_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + int i; + + _rtw_memcpy(&padapter->rmpriv.rm_en_cap_assoc, pIE->data, pIE->Length); + RTW_INFO("assoc.rm_en_cap="RM_CAP_FMT"\n", RM_CAP_ARG(pIE->data)); +} + +/* Debug command */ + +#if (RM_SUPPORT_IWPRIV_DBG) +static int hex2num(char c) +{ + if (c >= '0' && c <= '9') + return c - '0'; + if (c >= 'a' && c <= 'f') + return c - 'a' + 10; + if (c >= 'A' && c <= 'F') + return c - 'A' + 10; + return -1; +} + +int hex2byte(const char *hex) +{ + int a, b; + a = hex2num(*hex++); + if (a < 0) + return -1; + b = hex2num(*hex++); + if (b < 0) + return -1; + return (a << 4) | b; +} + +static char * hwaddr_parse(char *txt, u8 *addr) +{ + size_t i; + + for (i = 0; i < ETH_ALEN; i++) { + int a; + + a = hex2byte(txt); + if (a < 0) + return NULL; + txt += 2; + addr[i] = a; + if (i < ETH_ALEN - 1 && *txt++ != ':') + return NULL; + } + return txt; +} + +void rm_dbg_list_sta(_adapter *padapter, char *s) +{ + int i; + _irqL irqL; + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + _list *plist, *phead; + + + sprintf(pstr(s), "\n"); + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, + struct sta_info, hash_list); + + plist = get_next(plist); + + sprintf(pstr(s), "=========================================\n"); + sprintf(pstr(s), "mac=" MAC_FMT "\n", + MAC_ARG(psta->cmn.mac_addr)); + sprintf(pstr(s), "state=0x%x, aid=%d, macid=%d\n", + psta->state, psta->cmn.aid, psta->cmn.mac_id); + sprintf(pstr(s), "rm_cap="RM_CAP_FMT"\n", + RM_CAP_ARG(psta->rm_en_cap)); + } + + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + sprintf(pstr(s), "=========================================\n"); +} + +void rm_dbg_help(_adapter *padapter, char *s) +{ + int i; + + + sprintf(pstr(s), "\n"); + sprintf(pstr(s), "rrm list_sta\n"); + sprintf(pstr(s), "rrm list_meas\n"); + + sprintf(pstr(s), "rrm add_meas ,m=,rpt=\n"); + sprintf(pstr(s), "rrm run_meas \n"); + sprintf(pstr(s), "rrm del_meas\n"); + + sprintf(pstr(s), "rrm run_meas rmid=xxxx,ev=xx\n"); + sprintf(pstr(s), "rrm activate\n"); + + for (i=0;istapriv; + _list *plist, *phead; + + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, + struct sta_info, hash_list); + + plist = get_next(plist); + + if (psta->cmn.aid == aid) + goto done; + + if (pbssid && _rtw_memcmp(psta->cmn.mac_addr, + pbssid, 6)) + goto done; + } + + } + psta = NULL; +done: + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + return psta; +} + +static int rm_dbg_modify_meas(_adapter *padapter, char *s) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_obj *prm; + struct sta_info *psta; + char *pmac, *ptr, *paid, *prpt, *pnbp, *pclm, *pnhm, *pbcn, *plnk; + unsigned val; + u8 bssid[ETH_ALEN]; + + + /* example : + * rrm add_meas ,m=, + * rrm run_meas + */ + paid = strstr(s, "aid="); + pmac = strstr(s, "mac="); + pbcn = strstr(s, "m=bcn"); + pclm = strstr(s, "m=clm"); + pnhm = strstr(s, "m=nhm"); + pnbp = strstr(s, "m=nb"); + plnk = strstr(s, "m=link"); + prpt = strstr(s, "rpt="); + + /* set all ',' to NULL (end of line) */ + ptr = s; + while (ptr) { + ptr = strchr(ptr, ','); + if (ptr) { + *(ptr) = 0x0; + ptr++; + } + } + prm = (struct rm_obj *)prmpriv->prm_sel; + prm->q.m_token = rm_gen_meas_token(padapter); + psta = prm->psta; + + if (paid) { /* find sta_info according to aid */ + paid += 4; /* skip aid= */ + sscanf(paid, "%u", &val); /* aid=x */ + psta = rm_get_sta(padapter, val, NULL); + + } else if (pmac) { /* find sta_info according to bssid */ + pmac += 4; /* skip mac= */ + if (hwaddr_parse(pmac, bssid) == NULL) { + sprintf(pstr(s), "Err: \nincorrect mac format\n"); + return _FAIL; + } + psta = rm_get_sta(padapter, 0xff, bssid); + } + + if (psta) { + prm->psta = psta; + prm->q.diag_token = rm_gen_dialog_token(padapter); + prm->rmid = rm_gen_rmid(padapter, prm, RM_MASTER); + } else + return _FAIL; + + prm->q.action_code = RM_ACT_RADIO_MEAS_REQ; + if (pbcn) { + prm->q.m_type = bcn_req; + prm->q.rand_intvl = le16_to_cpu(100); + prm->q.meas_dur = le16_to_cpu(100); + } else if (pnhm) { + prm->q.m_type = noise_histo_req; + } else if (pclm) { + prm->q.m_type = ch_load_req; + } else if (pnbp) { + prm->q.action_code = RM_ACT_NB_REP_REQ; + } else if (plnk) { + prm->q.action_code = RM_ACT_LINK_MEAS_REQ; + } else + return _FAIL; + + if (prpt) { + prpt += 4; /* skip rpt= */ + sscanf(prpt, "%u", &val); + prm->q.rpt = (u8)val; + } + + return _SUCCESS; +} + +static void rm_dbg_activate_meas(_adapter *padapter, char *s) +{ + struct rm_priv *prmpriv = &(padapter->rmpriv); + struct rm_obj *prm; + + + if (prmpriv->prm_sel == NULL) { + sprintf(pstr(s), "\nErr: No inActivate measurement\n"); + return; + } + prm = (struct rm_obj *)prmpriv->prm_sel; + + /* verify attributes */ + if (prm->psta == NULL) { + sprintf(pstr(s), "\nErr: inActivate meas has no psta\n"); + return; + } + + /* measure current channel */ + prm->q.ch_num = padapter->mlmeextpriv.cur_channel; + prm->q.op_class = rm_get_oper_class_via_ch(prm->q.ch_num); + + /* enquee rmobj */ + rm_enqueue_rmobj(padapter, prm, _FALSE); + + sprintf(pstr(s), "\nActivate rmid=%x, state=%s, meas_type=%s\n", + prm->rmid, rm_state_name(prm->state), + rm_type_req_name(prm->q.m_type)); + + sprintf(pstr(s), "aid=%d, mac=" MAC_FMT "\n", + prm->psta->cmn.aid, MAC_ARG(prm->psta->cmn.mac_addr)); + + /* clearn inActivate prm info */ + prmpriv->prm_sel = NULL; +} + +/* for ioctl */ +int rm_send_bcn_reqs(_adapter *padapter, u8 *sta_addr, u8 op_class, u8 ch, + u16 measure_duration, u8 measure_mode, u8 *bssid, u8 *ssid, + u8 reporting_detail, + u8 n_ap_ch_rpt, struct _RT_OPERATING_CLASS *rpt, + u8 n_elem_id, u8 *elem_id_list) + +{ + struct rm_obj *prm; + char *pact; + struct sta_info *psta; + struct _RT_OPERATING_CLASS *prpt; + void *ptr; + int i,j,sz; + u8 bcast[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + + if (n_ap_ch_rpt > BCN_REQ_OPT_AP_CH_RPT_MAX_NUM) { + RTW_ERR("RM: chset num %d > %d\n", + n_ap_ch_rpt, BCN_REQ_OPT_AP_CH_RPT_MAX_NUM); + return -1; + } + /* dest sta */ + psta = rtw_get_stainfo(&padapter->stapriv, sta_addr); + if (!psta) { + RTW_ERR("RM: psta not found\n"); + return -2; + } + prm = rm_alloc_rmobj(padapter); + if (prm == NULL) { + RTW_ERR("RM: unable to alloc rm obj for requeset\n"); + return -3; + } + + prm->psta = psta; + prm->q.meas_dur = measure_duration; + + /* Figure 8-104 Measurement Requested format */ + prm->q.category = RTW_WLAN_CATEGORY_RADIO_MEAS; + prm->q.action_code = RM_ACT_RADIO_MEAS_REQ; + prm->q.m_mode = measure_mode; + prm->q.m_type = bcn_req; + prm->q.diag_token = rm_gen_dialog_token(padapter); + prm->q.m_token = rm_gen_meas_token(padapter); + prm->rmid = rm_gen_rmid(padapter, prm, RM_MASTER); + + prm->q.e_id = _MEAS_REQ_IE_; /* 38 */ + prm->q.ch_num = ch; + prm->q.op_class = op_class; + prm->from_ioctl = true; + + if (bssid != NULL) + memcpy(prm->q.bssid, bssid, ETH_ALEN); + else + memcpy(prm->q.bssid, bcast, ETH_ALEN); + + if (ssid != NULL) { + i = MIN(32, strlen(ssid)); + prm->q.opt.bcn.ssid.SsidLength = i; + memcpy(prm->q.opt.bcn.ssid.Ssid, ssid, i); + } + + if (n_ap_ch_rpt > 0) { + prm->q.opt.bcn.ap_ch_rpt_num = n_ap_ch_rpt; + j = 0; + for (i = 0; i < n_ap_ch_rpt; i++) { + prpt = rpt++; + if (prpt == NULL) + break; + + sz = sizeof(struct _RT_OPERATING_CLASS) * prpt->Len; + ptr = rtw_malloc(sz); + _rtw_memcpy(ptr, prpt, sz); + prm->q.opt.bcn.ap_ch_rpt[i] = (struct _RT_OPERATING_CLASS *)ptr; + } + } + prm->q.opt.bcn.rep_detail = reporting_detail; + + if ((n_elem_id > 0) && (n_elem_id < BCN_REQ_REQ_OPT_MAX_NUM)) { + prm->q.opt.bcn.req_id_num = n_elem_id; + _rtw_memcpy(prm->q.opt.bcn.req_id, elem_id_list, n_elem_id); + } + /* enquee rmobj */ + rm_enqueue_rmobj(padapter, prm, _FALSE); + + RTW_INFO("\nAdd rmid=%x, meas_type=%s ok\n", + prm->rmid, rm_type_req_name(prm->q.m_type)); + + if (prm->psta) + RTW_INFO("mac="MAC_FMT"\n", MAC_ARG(prm->psta->cmn.mac_addr)); + return 0; +} + +void indicate_beacon_report(u8 *sta_addr, + u8 n_measure_rpt, u32 elem_len, u8 *elem) +{ + RTW_INFO("RM: recv bcn reprot from mac="MAC_FMT"\n", MAC_ARG(sta_addr)); +#ifdef CONFIG_PLATFORM_CMAP_INTFS + cmap_intfs_nl_beacon_report_event(sta_addr, n_measure_rpt, elem_len, elem); +#endif +} + +static void rm_dbg_add_meas(_adapter *padapter, char *s) +{ + struct rm_priv *prmpriv = &(padapter->rmpriv); + struct rm_obj *prm; + char *pact; + + + /* example : + * rrm add_meas ,m= + * rrm run_meas + */ + prm = (struct rm_obj *)prmpriv->prm_sel; + if (prm == NULL) + prm = rm_alloc_rmobj(padapter); + + if (prm == NULL) { + sprintf(pstr(s), "\nErr: alloc meas fail\n"); + return; + } + + prmpriv->prm_sel = prm; + + pact = strstr(s, "act"); + if (rm_dbg_modify_meas(padapter, s) == _FAIL) { + + sprintf(pstr(s), "\nErr: add meas fail\n"); + rm_free_rmobj(prm); + prmpriv->prm_sel = NULL; + return; + } + prm->q.category = RTW_WLAN_CATEGORY_RADIO_MEAS; + prm->q.e_id = _MEAS_REQ_IE_; /* 38 */ + + sprintf(pstr(s), "\nAdd rmid=%x, meas_type=%s ok\n", + prm->rmid, rm_type_req_name(prm->q.m_type)); + + if (prm->psta) + sprintf(pstr(s), "mac="MAC_FMT"\n", + MAC_ARG(prm->psta->cmn.mac_addr)); + + if (pact) + rm_dbg_activate_meas(padapter, pstr(s)); +} + +static void rm_dbg_del_meas(_adapter *padapter, char *s) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_obj *prm = (struct rm_obj *)prmpriv->prm_sel; + + + if (prm) { + sprintf(pstr(s), "\ndelete rmid=%x\n",prm->rmid); + + /* free inActivate meas - enqueue yet */ + prmpriv->prm_sel = NULL; + rtw_mfree(prmpriv->prm_sel, sizeof(struct rm_obj)); + } else + sprintf(pstr(s), "Err: no inActivate measurement\n"); +} + +static void rm_dbg_run_meas(_adapter *padapter, char *s) +{ + struct rm_obj *prm; + char *pevid, *prmid; + u32 rmid, evid; + + + prmid = strstr(s, "rmid="); /* hex */ + pevid = strstr(s, "evid="); /* dec */ + + if (prmid && pevid) { + prmid += 5; /* rmid= */ + sscanf(prmid, "%x", &rmid); + + pevid += 5; /* evid= */ + sscanf(pevid, "%u", &evid); + } else { + sprintf(pstr(s), "\nErr: incorrect attribute\n"); + return; + } + + prm = rm_get_rmobj(padapter, rmid); + + if (!prm) { + sprintf(pstr(s), "\nErr: measurement not found\n"); + return; + } + + if (evid >= RM_EV_max) { + sprintf(pstr(s), "\nErr: wrong event id\n"); + return; + } + + rm_post_event(padapter, prm->rmid, evid); + sprintf(pstr(s), "\npost %s to rmid=%x\n",rm_event_name(evid), rmid); +} + +static void rm_dbg_show_meas(struct rm_obj *prm, char *s) +{ + struct sta_info *psta; + + psta = prm->psta; + + if (prm->q.action_code == RM_ACT_RADIO_MEAS_REQ) { + + sprintf(pstr(s), "\nrmid=%x, meas_type=%s\n", + prm->rmid, rm_type_req_name(prm->q.m_type)); + + } else if (prm->q.action_code == RM_ACT_NB_REP_REQ) { + + sprintf(pstr(s), "\nrmid=%x, action=neighbor_req\n", + prm->rmid); + } else + sprintf(pstr(s), "\nrmid=%x, action=unknown\n", + prm->rmid); + + if (psta) + sprintf(pstr(s), "aid=%d, mac="MAC_FMT"\n", + psta->cmn.aid, MAC_ARG(psta->cmn.mac_addr)); + + sprintf(pstr(s), "clock=%d, state=%s, rpt=%u/%u\n", + (int)ATOMIC_READ(&prm->pclock->counter), + rm_state_name(prm->state), prm->p.rpt, prm->q.rpt); +} + +static void rm_dbg_list_meas(_adapter *padapter, char *s) +{ + int meas_amount; + _irqL irqL; + struct rm_obj *prm; + struct sta_info *psta; + struct rm_priv *prmpriv = &padapter->rmpriv; + _queue *queue = &prmpriv->rm_queue; + _list *plist, *phead; + + + sprintf(pstr(s), "\n"); + _enter_critical(&queue->lock, &irqL); + phead = get_list_head(queue); + plist = get_next(phead); + meas_amount = 0; + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + prm = LIST_CONTAINOR(plist, struct rm_obj, list); + meas_amount++; + plist = get_next(plist); + psta = prm->psta; + sprintf(pstr(s), "=========================================\n"); + + rm_dbg_show_meas(prm, s); + } + _exit_critical(&queue->lock, &irqL); + + sprintf(pstr(s), "=========================================\n"); + + if (meas_amount==0) { + sprintf(pstr(s), "No Activate measurement\n"); + sprintf(pstr(s), "=========================================\n"); + } + + if (prmpriv->prm_sel == NULL) + sprintf(pstr(s), "\nNo inActivate measurement\n"); + else { + sprintf(pstr(s), "\ninActivate measurement\n"); + rm_dbg_show_meas((struct rm_obj *)prmpriv->prm_sel, s); + } +} +#endif /* RM_SUPPORT_IWPRIV_DBG */ + +int verify_bcn_req(_adapter *padapter, struct sta_info *psta) +{ + char *bssid = NULL; + char ssid[] = "RealKungFu"; + u8 op_class = 0; + u8 ch = 255; + u16 measure_duration = 100; + u8 reporting_detaial = 0; + u8 n_ap_ch_rpt = 6; + u8 measure_mode = bcn_req_active; + u8 req[] = {1,2,3}; + u8 req_len = sizeof(req); + + + static RT_OPERATING_CLASS US[] = { + /* 0, OP_CLASS_NULL */ //{ 0, 0, {}}, + /* 1, OP_CLASS_1 */ {115, 4, {36, 40, 44, 48}}, + /* 2, OP_CLASS_2 */ {118, 4, {52, 56, 60, 64}}, + /* 3, OP_CLASS_3 */ {124, 4, {149, 153, 157, 161}}, + /* 4, OP_CLASS_4 */ {121, 11, {100, 104, 108, 112, 116, 120, 124, + 128, 132, 136, 140}}, + /* 5, OP_CLASS_5 */ {125, 5, {149, 153, 157, 161, 165}}, + /* 6, OP_CLASS_12 */ { 81, 11, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}} + }; + + rm_send_bcn_reqs(padapter, psta->cmn.mac_addr, op_class, ch, + measure_duration, measure_mode, bssid, ssid, + reporting_detaial, n_ap_ch_rpt, US, req_len, req); + return 0; +} + +void rm_dbg_cmd(_adapter *padapter, char *s) +{ + unsigned val; + char *paid; + struct sta_info *psta=NULL; + +#if (RM_SUPPORT_IWPRIV_DBG) + if (_rtw_memcmp(s, "help", 4)) { + rm_dbg_help(padapter, s); + + } else if (_rtw_memcmp(s, "send_bcn_req", 12)) { + + /* rtwpriv wls1 rrm send_bcn_req aid=1 */ + paid = strstr(s, "aid="); + if (paid) { /* find sta_info according to aid */ + paid += 4; /* skip aid= */ + sscanf(paid, "%u", &val); /* aid=x */ + psta = rm_get_sta(padapter, val, NULL); + + if (psta) + verify_bcn_req(padapter, psta); + } + + } else if (_rtw_memcmp(s, "list_sta", 8)) { + rm_dbg_list_sta(padapter, s); + + } else if (_rtw_memcmp(s, "list_meas", 9)) { + rm_dbg_list_meas(padapter, s); + + } else if (_rtw_memcmp(s, "add_meas", 8)) { + rm_dbg_add_meas(padapter, s); + + } else if (_rtw_memcmp(s, "del_meas", 8)) { + rm_dbg_del_meas(padapter, s); + + } else if (_rtw_memcmp(s, "activate", 8)) { + rm_dbg_activate_meas(padapter, s); + + } else if (_rtw_memcmp(s, "run_meas", 8)) { + rm_dbg_run_meas(padapter, s); + + } else if (_rtw_memcmp(s, "nb", 2)) { + + paid = strstr(s, "aid="); + + if (paid) { /* find sta_info according to aid */ + paid += 4; /* skip aid= */ + sscanf(paid, "%u", &val); /* aid=x */ + psta = rm_get_sta(padapter, val, NULL); + + if (psta) + rm_add_nb_req(padapter, psta); + } + } +#else + sprintf(pstr(s), "\n"); + sprintf(pstr(s), "rrm debug command was disabled\n"); +#endif +} +#endif /* CONFIG_RTW_80211K */ diff --git a/core/rtw_rm_fsm.c b/core/rtw_rm_fsm.c new file mode 100644 index 0000000..244b799 --- /dev/null +++ b/core/rtw_rm_fsm.c @@ -0,0 +1,1017 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include +#ifdef CONFIG_RTW_80211K +#include "rtw_rm_fsm.h" +#include "rtw_rm_util.h" + +struct fsm_state { + u8 *name; + int(*fsm_func)(struct rm_obj *prm, enum RM_EV_ID evid); +}; + +static void rm_state_initial(struct rm_obj *prm); +static void rm_state_goto(struct rm_obj *prm, enum RM_STATE rm_state); +static void rm_state_run(struct rm_obj *prm, enum RM_EV_ID evid); +static struct rm_event *rm_dequeue_ev(_queue *queue); +static struct rm_obj *rm_dequeue_rm(_queue *queue); + +void rm_timer_callback(void *data) +{ + int i; + _adapter *padapter = (_adapter *)data; + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_clock *pclock; + + + /* deal with clock */ + for (i=0;iclock[i]; + if (pclock->prm == NULL + ||(ATOMIC_READ(&(pclock->counter)) == 0)) + continue; + + ATOMIC_DEC(&(pclock->counter)); + + if (ATOMIC_READ(&(pclock->counter)) == 0) + rm_post_event(pclock->prm->psta->padapter, + pclock->prm->rmid, prmpriv->clock[i].evid); + } + _set_timer(&prmpriv->rm_timer, CLOCK_UNIT); +} + +int rtw_init_rm(_adapter *padapter) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + + + RTW_INFO("RM: %s\n",__func__); + _rtw_init_queue(&(prmpriv->rm_queue)); + _rtw_init_queue(&(prmpriv->ev_queue)); + + /* bit 0-7 */ + prmpriv->rm_en_cap_def[0] = 0 + | BIT(RM_LINK_MEAS_CAP_EN) + | BIT(RM_NB_REP_CAP_EN) + /*| BIT(RM_PARAL_MEAS_CAP_EN)*/ + | BIT(RM_REPEAT_MEAS_CAP_EN) + | BIT(RM_BCN_PASSIVE_MEAS_CAP_EN) + | BIT(RM_BCN_ACTIVE_MEAS_CAP_EN) + | BIT(RM_BCN_TABLE_MEAS_CAP_EN) + /*| BIT(RM_BCN_MEAS_REP_COND_CAP_EN)*/; + + /* bit 8-15 */ + prmpriv->rm_en_cap_def[1] = 0 + /*| BIT(RM_FRAME_MEAS_CAP_EN - 8)*/ +#ifdef CONFIG_RTW_ACS + | BIT(RM_CH_LOAD_CAP_EN - 8) + | BIT(RM_NOISE_HISTO_CAP_EN - 8) +#endif + /*| BIT(RM_STATIS_MEAS_CAP_EN - 8)*/ + /*| BIT(RM_LCI_MEAS_CAP_EN - 8)*/ + /*| BIT(RM_LCI_AMIMUTH_CAP_EN - 8)*/ + /*| BIT(RM_TRANS_STREAM_CAT_MEAS_CAP_EN - 8)*/ + /*| BIT(RM_TRIG_TRANS_STREAM_CAT_MEAS_CAP_EN - 8)*/; + + /* bit 16-23 */ + prmpriv->rm_en_cap_def[2] = 0 + /*| BIT(RM_AP_CH_REP_CAP_EN - 16)*/ + /*| BIT(RM_RM_MIB_CAP_EN - 16)*/ + /*| BIT(RM_OP_CH_MAX_MEAS_DUR0 - 16)*/ + /*| BIT(RM_OP_CH_MAX_MEAS_DUR1 - 16)*/ + /*| BIT(RM_OP_CH_MAX_MEAS_DUR2 - 16)*/ + /*| BIT(RM_NONOP_CH_MAX_MEAS_DUR0 - 16)*/ + /*| BIT(RM_NONOP_CH_MAX_MEAS_DUR1 - 16)*/ + /*| BIT(RM_NONOP_CH_MAX_MEAS_DUR2 - 16)*/; + + /* bit 24-31 */ + prmpriv->rm_en_cap_def[3] = 0 + /*| BIT(RM_MEAS_PILOT_CAP0 - 24)*/ + /*| BIT(RM_MEAS_PILOT_CAP1 - 24)*/ + /*| BIT(RM_MEAS_PILOT_CAP2 - 24)*/ + /*| BIT(RM_MEAS_PILOT_TRANS_INFO_CAP_EN - 24)*/ + /*| BIT(RM_NB_REP_TSF_OFFSET_CAP_EN - 24)*/ + | BIT(RM_RCPI_MEAS_CAP_EN - 24) + | BIT(RM_RSNI_MEAS_CAP_EN - 24) + /*| BIT(RM_BSS_AVG_ACCESS_DELAY_CAP_EN - 24)*/; + + /* bit 32-39 */ + prmpriv->rm_en_cap_def[4] = 0 + /*| BIT(RM_BSS_AVG_ACCESS_DELAY_CAP_EN - 32)*/ + /*| BIT(RM_AVALB_ADMIS_CAPACITY_CAP_EN - 32)*/ + /*| BIT(RM_ANT_CAP_EN - 32)*/; + + prmpriv->enable = _TRUE; + + /* clock timer */ + rtw_init_timer(&prmpriv->rm_timer, + padapter, rm_timer_callback, padapter); + _set_timer(&prmpriv->rm_timer, CLOCK_UNIT); + + prmpriv->meas_token = 1; + return _SUCCESS; +} + +int rtw_deinit_rm(_adapter *padapter) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_obj *prm; + struct rm_event *pev; + + + RTW_INFO("RM: %s\n",__func__); + prmpriv->enable = _FALSE; + _cancel_timer_ex(&prmpriv->rm_timer); + + /* free all events and measurements */ + while((pev = rm_dequeue_ev(&prmpriv->ev_queue)) != NULL) + rtw_mfree((void *)pev, sizeof(struct rm_event)); + + while((prm = rm_dequeue_rm(&prmpriv->rm_queue)) != NULL) + rm_state_run(prm, RM_EV_cancel); + + _rtw_deinit_queue(&(prmpriv->rm_queue)); + _rtw_deinit_queue(&(prmpriv->ev_queue)); + + return _SUCCESS; +} + +int rtw_free_rm_priv(_adapter *padapter) +{ + return rtw_deinit_rm(padapter); +} + +static int rm_enqueue_ev(_queue *queue, struct rm_event *obj, bool to_head) +{ + _irqL irqL; + + + if (obj == NULL) + return _FAIL; + + _enter_critical(&queue->lock, &irqL); + + if (to_head) + rtw_list_insert_head(&obj->list, &queue->queue); + else + rtw_list_insert_tail(&obj->list, &queue->queue); + + _exit_critical(&queue->lock, &irqL); + + return _SUCCESS; +} + +static void rm_set_clock(struct rm_obj *prm, u32 ms, enum RM_EV_ID evid) +{ + ATOMIC_SET(&(prm->pclock->counter), (ms/CLOCK_UNIT)); + prm->pclock->evid = evid; +} + +static struct rm_clock *rm_alloc_clock(_adapter *padapter, struct rm_obj *prm) +{ + int i; + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_clock *pclock = NULL; + + + for (i=0;iclock[i]; + + if (pclock->prm == NULL) { + pclock->prm = prm; + ATOMIC_SET(&(pclock->counter), 0); + pclock->evid = RM_EV_max; + break; + } + } + return pclock; +} + +static void rm_cancel_clock(struct rm_obj *prm) +{ + ATOMIC_SET(&(prm->pclock->counter), 0); + prm->pclock->evid = RM_EV_max; +} + +static void rm_free_clock(struct rm_clock *pclock) +{ + pclock->prm = NULL; + ATOMIC_SET(&(pclock->counter), 0); + pclock->evid = RM_EV_max; +} + +static int is_list_linked(const struct list_head *head) +{ + return head->prev != NULL; +} + +void rm_free_rmobj(struct rm_obj *prm) +{ + if (is_list_linked(&prm->list)) + rtw_list_delete(&prm->list); + + if (prm->q.pssid) + rtw_mfree(prm->q.pssid, strlen(prm->q.pssid)+1); + + if (prm->q.opt.bcn.req_start) + rtw_mfree(prm->q.opt.bcn.req_start, + prm->q.opt.bcn.req_len); + + if (prm->pclock) + rm_free_clock(prm->pclock); + + rtw_mfree((void *)prm, sizeof(struct rm_obj)); +} + +struct rm_obj *rm_alloc_rmobj(_adapter *padapter) +{ + struct rm_obj *prm; + + + prm = (struct rm_obj *)rtw_malloc(sizeof(struct rm_obj)); + if (prm == NULL) + return NULL; + + _rtw_memset(prm, 0, sizeof(struct rm_obj)); + + /* alloc timer */ + if ((prm->pclock = rm_alloc_clock(padapter, prm)) == NULL) { + rm_free_rmobj(prm); + return NULL; + } + return prm; +} + +int rm_enqueue_rmobj(_adapter *padapter, struct rm_obj *prm, bool to_head) +{ + _irqL irqL; + struct rm_priv *prmpriv = &padapter->rmpriv; + _queue *queue = &prmpriv->rm_queue; + + + if (prm == NULL) + return _FAIL; + + _enter_critical(&queue->lock, &irqL); + if (to_head) + rtw_list_insert_head(&prm->list, &queue->queue); + else + rtw_list_insert_tail(&prm->list, &queue->queue); + _exit_critical(&queue->lock, &irqL); + + rm_state_initial(prm); + + return _SUCCESS; +} + +static struct rm_obj *rm_dequeue_rm(_queue *queue) +{ + _irqL irqL; + struct rm_obj *prm; + + + _enter_critical(&queue->lock, &irqL); + if (rtw_is_list_empty(&(queue->queue))) + prm = NULL; + else { + prm = LIST_CONTAINOR(get_next(&(queue->queue)), + struct rm_obj, list); + /* rtw_list_delete(&prm->list); */ + } + _exit_critical(&queue->lock, &irqL); + + return prm; +} + +static struct rm_event *rm_dequeue_ev(_queue *queue) +{ + _irqL irqL; + struct rm_event *ev; + + + _enter_critical(&queue->lock, &irqL); + if (rtw_is_list_empty(&(queue->queue))) + ev = NULL; + else { + ev = LIST_CONTAINOR(get_next(&(queue->queue)), + struct rm_event, list); + rtw_list_delete(&ev->list); + } + _exit_critical(&queue->lock, &irqL); + + return ev; +} + +static struct rm_obj *_rm_get_rmobj(_queue *queue, u32 rmid) +{ + _irqL irqL; + _list *phead, *plist; + struct rm_obj *prm = NULL; + + + if (rmid == 0) + return NULL; + + _enter_critical(&queue->lock, &irqL); + + phead = get_list_head(queue); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + + prm = LIST_CONTAINOR(plist, struct rm_obj, list); + if (rmid == (prm->rmid)) { + _exit_critical(&queue->lock, &irqL); + return prm; + } + plist = get_next(plist); + } + _exit_critical(&queue->lock, &irqL); + + return NULL; +} + +struct sta_info *rm_get_psta(_adapter *padapter, u32 rmid) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_obj *prm; + + + prm = _rm_get_rmobj(&prmpriv->rm_queue, rmid); + + if (prm) + return prm->psta; + + return NULL; +} + +struct rm_obj *rm_get_rmobj(_adapter *padapter, u32 rmid) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + + return _rm_get_rmobj(&prmpriv->rm_queue, rmid); +} + +u8 rtw_rm_post_envent_cmd(_adapter *padapter, u32 rmid, u8 evid) +{ + struct cmd_obj *pcmd; + struct rm_event *pev; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (pcmd == NULL) { + res = _FAIL; + goto exit; + } + pev = (struct rm_event*)rtw_zmalloc(sizeof(struct rm_event)); + + if (pev == NULL) { + rtw_mfree((u8 *) pcmd, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + pev->rmid = rmid; + pev->evid = evid; + + init_h2fwcmd_w_parm_no_rsp(pcmd, pev, CMD_RM_POST_EVENT); + res = rtw_enqueue_cmd(pcmdpriv, pcmd); +exit: + return res; +} + +int rm_post_event(_adapter *padapter, u32 rmid, enum RM_EV_ID evid) +{ + if (padapter->rmpriv.enable == _FALSE) + return _FALSE; + + RTW_INFO("RM: post asyn %s to rmid=%x\n", rm_event_name(evid), rmid); + rtw_rm_post_envent_cmd(padapter, rmid, evid); + return _SUCCESS; +} + +int _rm_post_event(_adapter *padapter, u32 rmid, enum RM_EV_ID evid) +{ + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_event *pev; + + if (evid >= RM_EV_max || rmid == 0) + return _FALSE; + + pev = (struct rm_event *)rtw_malloc(sizeof(struct rm_event)); + if (pev == NULL) + return _FALSE; + + pev->rmid = rmid; + pev->evid = evid; + + RTW_INFO("RM: post sync %s to rmid=%x\n", rm_event_name(evid), rmid); + rm_enqueue_ev(&prmpriv->ev_queue, pev, FALSE); + + return _SUCCESS; +} + +static void rm_bcast_aid_handler(_adapter *padapter, struct rm_event *pev) +{ + _irqL irqL; + _list *phead, *plist; + _queue *queue = &padapter->rmpriv.rm_queue; + struct rm_obj *prm; + + + _enter_critical(&queue->lock, &irqL); + phead = get_list_head(queue); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + + prm = LIST_CONTAINOR(plist, struct rm_obj, list); + plist = get_next(plist); + if (RM_GET_AID(pev->rmid) == RM_GET_AID(prm->rmid)) { + _exit_critical(&queue->lock, &irqL); + rm_state_run(prm, pev->evid); + _enter_critical(&queue->lock, &irqL); + } + } + _exit_critical(&queue->lock, &irqL); + return; +} + +/* main handler of RM (Resource Management) */ +void rm_handler(_adapter *padapter, struct rm_event *pe) +{ + int i; + struct rm_priv *prmpriv = &padapter->rmpriv; + struct rm_obj *prm; + struct rm_event *pev; + + + /* dequeue event */ + while((pev = rm_dequeue_ev(&prmpriv->ev_queue)) != NULL) + { + if (RM_IS_ID_FOR_ALL(pev->rmid)) { + /* apply to all aid mateched measurement */ + rm_bcast_aid_handler(padapter, pev); + rtw_mfree((void *)pev, sizeof(struct rm_event)); + continue; + } + + /* retrieve rmobj */ + prm = _rm_get_rmobj(&prmpriv->rm_queue, pev->rmid); + if (prm == NULL) { + RTW_ERR("RM: rmid=%x event=%s doesn't find rm obj\n", + pev->rmid, rm_event_name(pev->evid)); + rtw_mfree((void *)pev, sizeof(struct rm_event)); + return; + } + /* run state machine */ + rm_state_run(prm, pev->evid); + rtw_mfree((void *)pev, sizeof(struct rm_event)); + } +} + +static int rm_issue_meas_req(struct rm_obj *prm) +{ + switch (prm->q.action_code) { + case RM_ACT_RADIO_MEAS_REQ: + switch (prm->q.m_type) { + case bcn_req: + case ch_load_req: + case noise_histo_req: + issue_radio_meas_req(prm); + break; + default: + break; + } /* meas_type */ + break; + case RM_ACT_NB_REP_REQ: + /* issue neighbor request */ + issue_nb_req(prm); + break; + case RM_ACT_LINK_MEAS_REQ: + issue_link_meas_req(prm); + break; + default: + return _FALSE; + } /* action_code */ + + return _SUCCESS; +} + +/* +* RM state machine +*/ + +static int rm_state_idle(struct rm_obj *prm, enum RM_EV_ID evid) +{ + _adapter *padapter = prm->psta->padapter; + u8 val8; + u32 val32; + + + prm->p.category = RTW_WLAN_CATEGORY_RADIO_MEAS; + + switch (evid) { + case RM_EV_state_in: + switch (prm->q.action_code) { + case RM_ACT_RADIO_MEAS_REQ: + /* copy attrib from meas_req to meas_rep */ + prm->p.action_code = RM_ACT_RADIO_MEAS_REP; + prm->p.diag_token = prm->q.diag_token; + prm->p.e_id = _MEAS_RSP_IE_; + prm->p.m_token = prm->q.m_token; + prm->p.m_type = prm->q.m_type; + prm->p.rpt = prm->q.rpt; + prm->p.ch_num = prm->q.ch_num; + prm->p.op_class = prm->q.op_class; + + if (prm->q.m_type == ch_load_req + || prm->q.m_type == noise_histo_req) { + /* + * phydm measure current ch periodically + * scan current ch is not necessary + */ + val8 = padapter->mlmeextpriv.cur_channel; + if (prm->q.ch_num == val8) + prm->poll_mode = 1; + } + RTW_INFO("RM: rmid=%x %s switch in repeat=%u\n", + prm->rmid, rm_type_req_name(prm->q.m_type), + prm->q.rpt); + break; + case RM_ACT_NB_REP_REQ: + prm->p.action_code = RM_ACT_NB_REP_RESP; + RTW_INFO("RM: rmid=%x Neighbor request switch in\n", + prm->rmid); + break; + case RM_ACT_LINK_MEAS_REQ: + prm->p.diag_token = prm->q.diag_token; + prm->p.action_code = RM_ACT_LINK_MEAS_REP; + RTW_INFO("RM: rmid=%x Link meas switch in\n", + prm->rmid); + break; + default: + prm->p.action_code = prm->q.action_code; + rm_set_rep_mode(prm, MEAS_REP_MOD_INCAP); + RTW_INFO("RM: rmid=%x recv unknown action %d\n", + prm->rmid,prm->p.action_code); + break; + } /* switch() */ + + if (prm->rmid & RM_MASTER) { + if (rm_issue_meas_req(prm) == _SUCCESS) + rm_state_goto(prm, RM_ST_WAIT_MEAS); + else + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } else { + rm_state_goto(prm, RM_ST_DO_MEAS); + return _SUCCESS; + } + + if (prm->p.m_mode) { + issue_null_reply(prm); + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + if (prm->q.rand_intvl) { + /* get low tsf to generate random interval */ + val32 = rtw_read32(padapter, REG_TSFTR); + val32 = val32 % prm->q.rand_intvl; + RTW_INFO("RM: rmid=%x rand_intval=%d, rand=%d\n", + prm->rmid, (int)prm->q.rand_intvl,val32); + rm_set_clock(prm, prm->q.rand_intvl, + RM_EV_delay_timer_expire); + return _SUCCESS; + } + break; + case RM_EV_delay_timer_expire: + rm_state_goto(prm, RM_ST_DO_MEAS); + break; + case RM_EV_cancel: + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_state_out: + rm_cancel_clock(prm); + break; + default: + break; + } + return _SUCCESS; +} + +/* we do the measuring */ +static int rm_state_do_meas(struct rm_obj *prm, enum RM_EV_ID evid) +{ + _adapter *padapter = prm->psta->padapter; + u8 val8; + u64 val64; + + + switch (evid) { + case RM_EV_state_in: + if (prm->q.action_code == RM_ACT_RADIO_MEAS_REQ) { + switch (prm->q.m_type) { + case bcn_req: + if (prm->q.m_mode == bcn_req_bcn_table) { + RTW_INFO("RM: rmid=%x Beacon table\n", + prm->rmid); + _rm_post_event(padapter, prm->rmid, + RM_EV_survey_done); + return _SUCCESS; + } + break; + case ch_load_req: + case noise_histo_req: + if (prm->poll_mode) + _rm_post_event(padapter, prm->rmid, + RM_EV_survey_done); + return _SUCCESS; + default: + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + + if (!ready_for_scan(prm)) { + prm->wait_busy = RM_BUSY_TRAFFIC_TIMES; + RTW_INFO("RM: wait busy traffic - %d\n", + prm->wait_busy); + rm_set_clock(prm, RM_WAIT_BUSY_TIMEOUT, + RM_EV_busy_timer_expire); + return _SUCCESS; + } + } else if (prm->q.action_code == RM_ACT_LINK_MEAS_REQ) { + ; /* do nothing */ + rm_state_goto(prm, RM_ST_SEND_REPORT); + return _SUCCESS; + } + _rm_post_event(padapter, prm->rmid, RM_EV_start_meas); + break; + case RM_EV_start_meas: + if (prm->q.action_code == RM_ACT_RADIO_MEAS_REQ) { + /* resotre measurement start time */ + prm->meas_start_time = rtw_hal_get_tsftr_by_port(padapter + , rtw_hal_get_port(padapter)); + + switch (prm->q.m_type) { + case bcn_req: + val8 = 1; /* Enable free run counter */ + prm->free_run_counter_valid = rtw_hal_set_hwreg( + padapter, HW_VAR_FREECNT, &val8); + + rm_sitesurvey(prm); + break; + case ch_load_req: + case noise_histo_req: + rm_sitesurvey(prm); + break; + default: + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + break; + } + } + /* handle measurement timeout */ + rm_set_clock(prm, RM_MEAS_TIMEOUT, RM_EV_meas_timer_expire); + break; + case RM_EV_survey_done: + if (prm->q.action_code == RM_ACT_RADIO_MEAS_REQ) { + switch (prm->q.m_type) { + case bcn_req: + rm_cancel_clock(prm); + rm_state_goto(prm, RM_ST_SEND_REPORT); + return _SUCCESS; + case ch_load_req: + case noise_histo_req: + retrieve_radio_meas_result(prm); + + if (rm_radio_meas_report_cond(prm) == _SUCCESS) + rm_state_goto(prm, RM_ST_SEND_REPORT); + else + rm_set_clock(prm, RM_COND_INTVL, + RM_EV_retry_timer_expire); + break; + default: + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + } + break; + case RM_EV_meas_timer_expire: + RTW_INFO("RM: rmid=%x measurement timeount\n",prm->rmid); + rm_set_rep_mode(prm, MEAS_REP_MOD_REFUSE); + issue_null_reply(prm); + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_busy_timer_expire: + if (!ready_for_scan(prm) && prm->wait_busy--) { + RTW_INFO("RM: wait busy - %d\n",prm->wait_busy); + rm_set_clock(prm, RM_WAIT_BUSY_TIMEOUT, + RM_EV_busy_timer_expire); + break; + } + else if (prm->wait_busy <= 0) { + RTW_INFO("RM: wait busy timeout\n"); + rm_set_rep_mode(prm, MEAS_REP_MOD_REFUSE); + issue_null_reply(prm); + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + _rm_post_event(padapter, prm->rmid, RM_EV_start_meas); + break; + case RM_EV_request_timer_expire: + rm_set_rep_mode(prm, MEAS_REP_MOD_REFUSE); + issue_null_reply(prm); + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_retry_timer_expire: + /* expired due to meas condition mismatch, meas again */ + _rm_post_event(padapter, prm->rmid, RM_EV_start_meas); + break; + case RM_EV_cancel: + rm_set_rep_mode(prm, MEAS_REP_MOD_REFUSE); + issue_null_reply(prm); + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_state_out: + rm_cancel_clock(prm); + /* resotre measurement end time */ + prm->meas_end_time = rtw_hal_get_tsftr_by_port(padapter + , rtw_hal_get_port(padapter)); + + val8 = 0; /* Disable free run counter */ + rtw_hal_set_hwreg(padapter, HW_VAR_FREECNT, &val8); + break; + default: + break; + } + + return _SUCCESS; +} + +static int rm_state_wait_meas(struct rm_obj *prm, enum RM_EV_ID evid) +{ + u8 val8; + u64 val64; + + + switch (evid) { + case RM_EV_state_in: + /* we create meas_req, waiting for peer report */ + rm_set_clock(prm, RM_REQ_TIMEOUT, + RM_EV_request_timer_expire); + break; + case RM_EV_recv_rep: + rm_state_goto(prm, RM_ST_RECV_REPORT); + break; + case RM_EV_request_timer_expire: + case RM_EV_cancel: + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_state_out: + rm_cancel_clock(prm); + break; + default: + break; + } + return _SUCCESS; +} + +static int rm_state_send_report(struct rm_obj *prm, enum RM_EV_ID evid) +{ + u8 val8; + + + switch (evid) { + case RM_EV_state_in: + /* we have to issue report */ + if (prm->q.action_code == RM_ACT_RADIO_MEAS_REQ) { + switch (prm->q.m_type) { + case bcn_req: + issue_beacon_rep(prm); + break; + case ch_load_req: + case noise_histo_req: + issue_radio_meas_rep(prm); + break; + default: + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + + } else if (prm->q.action_code == RM_ACT_LINK_MEAS_REQ) { + issue_link_meas_rep(prm); + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + + } else { + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + + /* check repeat */ + if (prm->p.rpt) { + RTW_INFO("RM: rmid=%x repeat=%u/%u\n", + prm->rmid, prm->p.rpt, + prm->q.rpt); + prm->p.rpt--; + /* + * we recv meas_req, + * delay for a wihile and than meas again + */ + if (prm->poll_mode) + rm_set_clock(prm, RM_REPT_POLL_INTVL, + RM_EV_repeat_delay_expire); + else + rm_set_clock(prm, RM_REPT_SCAN_INTVL, + RM_EV_repeat_delay_expire); + return _SUCCESS; + } + /* we are done */ + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_repeat_delay_expire: + rm_state_goto(prm, RM_ST_DO_MEAS); + break; + case RM_EV_cancel: + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_state_out: + rm_cancel_clock(prm); + break; + default: + break; + } + return _SUCCESS; +} + +static int rm_state_recv_report(struct rm_obj *prm, enum RM_EV_ID evid) +{ + u8 val8; + + + switch (evid) { + case RM_EV_state_in: + /* we issue meas_req, got peer's meas report */ + switch (prm->p.action_code) { + case RM_ACT_RADIO_MEAS_REP: + /* check refuse, incapable and repeat */ + val8 = prm->p.m_mode; + if (val8) { + RTW_INFO("RM: rmid=%x peer reject (%s repeat=%d)\n", + prm->rmid, + val8|MEAS_REP_MOD_INCAP?"INCAP": + val8|MEAS_REP_MOD_REFUSE?"REFUSE": + val8|MEAS_REP_MOD_LATE?"LATE":"", + prm->p.rpt); + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + break; + case RM_ACT_NB_REP_RESP: + /* report to upper layer if needing */ + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + default: + rm_state_goto(prm, RM_ST_END); + return _SUCCESS; + } + /* check repeat */ + if (prm->p.rpt) { + RTW_INFO("RM: rmid=%x repeat=%u/%u\n", + prm->rmid, prm->p.rpt, + prm->q.rpt); + prm->p.rpt--; + /* waitting more report */ + rm_state_goto(prm, RM_ST_WAIT_MEAS); + break; + } + /* we are done */ + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_cancel: + rm_state_goto(prm, RM_ST_END); + break; + case RM_EV_state_out: + rm_cancel_clock(prm); + break; + default: + break; + } + return _SUCCESS; +} + +static int rm_state_end(struct rm_obj *prm, enum RM_EV_ID evid) +{ + switch (evid) { + case RM_EV_state_in: + _rm_post_event(prm->psta->padapter, prm->rmid, RM_EV_state_out); + break; + + case RM_EV_cancel: + case RM_EV_state_out: + default: + rm_free_rmobj(prm); + break; + } + return _SUCCESS; +} + +struct fsm_state rm_fsm[] = { + {"RM_ST_IDLE", rm_state_idle}, + {"RM_ST_DO_MEAS", rm_state_do_meas}, + {"RM_ST_WAIT_MEAS", rm_state_wait_meas}, + {"RM_ST_SEND_REPORT", rm_state_send_report}, + {"RM_ST_RECV_REPORT", rm_state_recv_report}, + {"RM_ST_END", rm_state_end} +}; + +char *rm_state_name(enum RM_STATE state) +{ + return rm_fsm[state].name; +} + +char *rm_event_name(enum RM_EV_ID evid) +{ + switch(evid) { + case RM_EV_state_in: + return "RM_EV_state_in"; + case RM_EV_busy_timer_expire: + return "RM_EV_busy_timer_expire"; + case RM_EV_delay_timer_expire: + return "RM_EV_delay_timer_expire"; + case RM_EV_meas_timer_expire: + return "RM_EV_meas_timer_expire"; + case RM_EV_repeat_delay_expire: + return "RM_EV_repeat_delay_expire"; + case RM_EV_retry_timer_expire: + return "RM_EV_retry_timer_expire"; + case RM_EV_request_timer_expire: + return "RM_EV_request_timer_expire"; + case RM_EV_wait_report: + return "RM_EV_wait_report"; + case RM_EV_start_meas: + return "RM_EV_start_meas"; + case RM_EV_survey_done: + return "RM_EV_survey_done"; + case RM_EV_recv_rep: + return "RM_EV_recv_report"; + case RM_EV_cancel: + return "RM_EV_cancel"; + case RM_EV_state_out: + return "RM_EV_state_out"; + case RM_EV_max: + return "RM_EV_max"; + default: + return "RM_EV_unknown"; + } + return "UNKNOWN"; +} + +static void rm_state_initial(struct rm_obj *prm) +{ + prm->state = RM_ST_IDLE; + + RTW_INFO("\n"); + RTW_INFO("RM: rmid=%x %-18s -> %s\n",prm->rmid, + "new measurement", rm_fsm[prm->state].name); + + rm_post_event(prm->psta->padapter, prm->rmid, RM_EV_state_in); +} + +static void rm_state_run(struct rm_obj *prm, enum RM_EV_ID evid) +{ + RTW_INFO("RM: rmid=%x %-18s %s\n",prm->rmid, + rm_fsm[prm->state].name,rm_event_name(evid)); + + rm_fsm[prm->state].fsm_func(prm, evid); +} + +static void rm_state_goto(struct rm_obj *prm, enum RM_STATE rm_state) +{ + if (prm->state == rm_state) + return; + + rm_state_run(prm, RM_EV_state_out); + + RTW_INFO("\n"); + RTW_INFO("RM: rmid=%x %-18s -> %s\n",prm->rmid, + rm_fsm[prm->state].name, rm_fsm[rm_state].name); + + prm->state = rm_state; + rm_state_run(prm, RM_EV_state_in); +} +#endif /* CONFIG_RTW_80211K */ diff --git a/core/rtw_rm_util.c b/core/rtw_rm_util.c new file mode 100644 index 0000000..b0c2428 --- /dev/null +++ b/core/rtw_rm_util.c @@ -0,0 +1,501 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include +#ifdef CONFIG_RTW_80211K +#include "rtw_rm_fsm.h" +#include "rtw_rm_util.h" + +/* 802.11-2012 Table E-1 Operationg classes in United States */ +static RT_OPERATING_CLASS RTW_OP_CLASS_US[] = { + /* 0, OP_CLASS_NULL */ { 0, 0, {}}, + /* 1, OP_CLASS_1 */ {115, 4, {36, 40, 44, 48}}, + /* 2, OP_CLASS_2 */ {118, 4, {52, 56, 60, 64}}, + /* 3, OP_CLASS_3 */ {124, 4, {149, 153, 157, 161}}, + /* 4, OP_CLASS_4 */ {121, 11, {100, 104, 108, 112, 116, 120, 124, + 128, 132, 136, 140}}, + /* 5, OP_CLASS_5 */ {125, 5, {149, 153, 157, 161, 165}}, + /* 6, OP_CLASS_12 */ { 81, 11, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}} +}; + +u8 rm_get_ch_set( + struct rtw_ieee80211_channel *pch_set, u8 op_class, u8 ch_num) +{ + int i,j,sz; + u8 ch_amount = 0; + + + sz = sizeof(RTW_OP_CLASS_US)/sizeof(struct _RT_OPERATING_CLASS); + + if (ch_num != 0) { + pch_set[0].hw_value = ch_num; + ch_amount = 1; + RTW_INFO("RM: meas_ch->hw_value = %u\n", pch_set->hw_value); + goto done; + } + + for (i = 0; i < sz; i++) { + + if (RTW_OP_CLASS_US[i].global_op_class == op_class) { + + for (j = 0; j < RTW_OP_CLASS_US[i].Len; j++) { + pch_set[j].hw_value = + RTW_OP_CLASS_US[i].Channel[j]; + RTW_INFO("RM: meas_ch[%d].hw_value = %u\n", + j, pch_set[j].hw_value); + } + ch_amount = RTW_OP_CLASS_US[i].Len; + break; + } + } +done: + return ch_amount; +} + +u8 rm_get_ch_set_from_bcn_req_opt( + struct rtw_ieee80211_channel *pch_set, struct bcn_req_opt *opt) +{ + int i,j,k,sz; + struct _RT_OPERATING_CLASS *ap_ch_rpt; + u8 ch_amount = 0; + + k = 0; + for (i = 0; i < opt->ap_ch_rpt_num; i++) { + if (opt->ap_ch_rpt[i] == NULL) + break; + ap_ch_rpt = opt->ap_ch_rpt[i]; + for (j = 0; j < ap_ch_rpt->Len; j++) { + pch_set[k].hw_value = + ap_ch_rpt->Channel[j]; + RTW_INFO("RM: meas_ch[%d].hw_value = %u\n", + j, pch_set[k].hw_value); + k++; + } + } + return k; +} + +u8 rm_get_oper_class_via_ch(u8 ch) +{ + int i,j,sz; + + + sz = sizeof(RTW_OP_CLASS_US)/sizeof(struct _RT_OPERATING_CLASS); + + for (i = 0; i < sz; i++) { + for (j = 0; j < RTW_OP_CLASS_US[i].Len; j++) { + if ( ch == RTW_OP_CLASS_US[i].Channel[j]) { + RTW_INFO("RM: ch %u in oper_calss %u\n", + ch, RTW_OP_CLASS_US[i].global_op_class); + return RTW_OP_CLASS_US[i].global_op_class; + break; + } + } + } + return 0; +} + +int is_wildcard_bssid(u8 *bssid) +{ + int i; + u8 val8 = 0xff; + + + for (i=0;i<6;i++) + val8 &= bssid[i]; + + if (val8 == 0xff) + return _SUCCESS; + return _FALSE; +} + +u8 translate_dbm_to_rcpi(s8 SignalPower) +{ + /* RCPI = Int{(Power in dBm + 110)*2} for 0dBm > Power > -110dBm + * 0 : power <= -110.0 dBm + * 1 : power = -109.5 dBm + * 2 : power = -109.0 dBm + */ + return (SignalPower + 110)*2; +} + +u8 translate_percentage_to_rcpi(u32 SignalStrengthIndex) +{ + /* Translate to dBm (x=y-100) */ + return translate_dbm_to_rcpi(SignalStrengthIndex - 100); +} + +u8 rm_get_bcn_rcpi(struct rm_obj *prm, struct wlan_network *pnetwork) +{ + return translate_percentage_to_rcpi( + pnetwork->network.PhyInfo.SignalStrength); +} + +u8 rm_get_frame_rsni(struct rm_obj *prm, union recv_frame *pframe) +{ + int i; + u8 val8, snr, rx_num; + struct hal_spec_t *hal_spec = GET_HAL_SPEC(prm->psta->padapter); + + if (IS_CCK_RATE((hw_rate_to_m_rate(pframe->u.hdr.attrib.data_rate)))) + val8 = 255; + else { + snr = rx_num = 0; + for (i = 0; i < hal_spec->rf_reg_path_num; i++) { + if (GET_HAL_RX_PATH_BMP(prm->psta->padapter) & BIT(i)) { + snr += pframe->u.hdr.attrib.phy_info.rx_snr[i]; + rx_num++; + } + } + snr = snr / rx_num; + val8 = (u8)(snr + 10)*2; + } + return val8; +} + +u8 rm_get_bcn_rsni(struct rm_obj *prm, struct wlan_network *pnetwork) +{ + int i; + u8 val8, snr, rx_num; + struct hal_spec_t *hal_spec = GET_HAL_SPEC(prm->psta->padapter); + + if (pnetwork->network.PhyInfo.is_cck_rate) { + /* current HW doesn't have CCK RSNI */ + /* 255 indicates RSNI is unavailable */ + val8 = 255; + } else { + snr = rx_num = 0; + for (i = 0; i < hal_spec->rf_reg_path_num; i++) { + if (GET_HAL_RX_PATH_BMP(prm->psta->padapter) & BIT(i)) { + snr += pnetwork->network.PhyInfo.rx_snr[i]; + rx_num++; + } + } + snr = snr / rx_num; + val8 = (u8)(snr + 10)*2; + } + return val8; +} + +/* output: pwr (unit dBm) */ +int rm_get_tx_power(PADAPTER adapter, enum rf_path path, enum MGN_RATE rate, s8 *pwr) +{ + struct hal_spec_t *hal_spec = GET_HAL_SPEC(adapter); + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter); + int tx_num, band, bw, ch, n, rs; + u8 base; + s8 limt_offset = 127; /* max value of s8 */ + s8 rate_offset; + s8 powr_offset; + int rate_pos; + + + band = hal_data->current_band_type; + bw = hal_data->current_channel_bw; + ch = hal_data->current_channel; + + if (!HAL_SPEC_CHK_RF_PATH(hal_spec, band, path)) + return -1; + + if (HAL_IsLegalChannel(adapter, ch) == _FALSE) { + RTW_INFO("Illegal channel!!\n"); + return -2; + } + + *pwr = phy_get_tx_power_final_absolute_value(adapter, path, rate, bw, ch); + + return 0; +} + +int rm_get_rx_sensitivity(PADAPTER adapter, enum channel_width bw, enum MGN_RATE rate, s8 *pwr) +{ + s8 rx_sensitivity = -110; + + switch(rate) { + case MGN_1M: + rx_sensitivity= -101; + break; + case MGN_2M: + rx_sensitivity= -98; + break; + case MGN_5_5M: + rx_sensitivity= -92; + break; + case MGN_11M: + rx_sensitivity= -89; + break; + case MGN_6M: + case MGN_9M: + case MGN_12M: + rx_sensitivity = -92; + break; + case MGN_18M: + rx_sensitivity = -90; + break; + case MGN_24M: + rx_sensitivity = -88; + break; + case MGN_36M: + rx_sensitivity = -84; + break; + case MGN_48M: + rx_sensitivity = -79; + break; + case MGN_54M: + rx_sensitivity = -78; + break; + + case MGN_MCS0: + case MGN_MCS8: + case MGN_MCS16: + case MGN_MCS24: + case MGN_VHT1SS_MCS0: + case MGN_VHT2SS_MCS0: + case MGN_VHT3SS_MCS0: + case MGN_VHT4SS_MCS0: + /* BW20 BPSK 1/2 */ + rx_sensitivity = -82; + break; + + case MGN_MCS1: + case MGN_MCS9: + case MGN_MCS17: + case MGN_MCS25: + case MGN_VHT1SS_MCS1: + case MGN_VHT2SS_MCS1: + case MGN_VHT3SS_MCS1: + case MGN_VHT4SS_MCS1: + /* BW20 QPSK 1/2 */ + rx_sensitivity = -79; + break; + + case MGN_MCS2: + case MGN_MCS10: + case MGN_MCS18: + case MGN_MCS26: + case MGN_VHT1SS_MCS2: + case MGN_VHT2SS_MCS2: + case MGN_VHT3SS_MCS2: + case MGN_VHT4SS_MCS2: + /* BW20 QPSK 3/4 */ + rx_sensitivity = -77; + break; + + case MGN_MCS3: + case MGN_MCS11: + case MGN_MCS19: + case MGN_MCS27: + case MGN_VHT1SS_MCS3: + case MGN_VHT2SS_MCS3: + case MGN_VHT3SS_MCS3: + case MGN_VHT4SS_MCS3: + /* BW20 16-QAM 1/2 */ + rx_sensitivity = -74; + break; + + case MGN_MCS4: + case MGN_MCS12: + case MGN_MCS20: + case MGN_MCS28: + case MGN_VHT1SS_MCS4: + case MGN_VHT2SS_MCS4: + case MGN_VHT3SS_MCS4: + case MGN_VHT4SS_MCS4: + /* BW20 16-QAM 3/4 */ + rx_sensitivity = -70; + break; + + case MGN_MCS5: + case MGN_MCS13: + case MGN_MCS21: + case MGN_MCS29: + case MGN_VHT1SS_MCS5: + case MGN_VHT2SS_MCS5: + case MGN_VHT3SS_MCS5: + case MGN_VHT4SS_MCS5: + /* BW20 64-QAM 2/3 */ + rx_sensitivity = -66; + break; + + case MGN_MCS6: + case MGN_MCS14: + case MGN_MCS22: + case MGN_MCS30: + case MGN_VHT1SS_MCS6: + case MGN_VHT2SS_MCS6: + case MGN_VHT3SS_MCS6: + case MGN_VHT4SS_MCS6: + /* BW20 64-QAM 3/4 */ + rx_sensitivity = -65; + break; + + case MGN_MCS7: + case MGN_MCS15: + case MGN_MCS23: + case MGN_MCS31: + case MGN_VHT1SS_MCS7: + case MGN_VHT2SS_MCS7: + case MGN_VHT3SS_MCS7: + case MGN_VHT4SS_MCS7: + /* BW20 64-QAM 5/6 */ + rx_sensitivity = -64; + break; + + case MGN_VHT1SS_MCS8: + case MGN_VHT2SS_MCS8: + case MGN_VHT3SS_MCS8: + case MGN_VHT4SS_MCS8: + /* BW20 256-QAM 3/4 */ + rx_sensitivity = -59; + break; + + case MGN_VHT1SS_MCS9: + case MGN_VHT2SS_MCS9: + case MGN_VHT3SS_MCS9: + case MGN_VHT4SS_MCS9: + /* BW20 256-QAM 5/6 */ + rx_sensitivity = -57; + break; + + default: + return -1; + break; + + } + + switch(bw) { + case CHANNEL_WIDTH_20: + break; + case CHANNEL_WIDTH_40: + rx_sensitivity -= 3; + break; + case CHANNEL_WIDTH_80: + rx_sensitivity -= 6; + break; + case CHANNEL_WIDTH_160: + rx_sensitivity -= 9; + break; + case CHANNEL_WIDTH_5: + case CHANNEL_WIDTH_10: + case CHANNEL_WIDTH_80_80: + default: + return -1; + break; + } + *pwr = rx_sensitivity; + + return 0; +} + +/* output: path_a max tx power in dBm */ +int rm_get_path_a_max_tx_power(_adapter *adapter, s8 *path_a) +{ + struct hal_spec_t *hal_spec = GET_HAL_SPEC(adapter); + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter); + int path, tx_num, band, bw, ch, n, rs; + u8 rate_num; + s8 max_pwr[RF_PATH_MAX], pwr; + + + band = hal_data->current_band_type; + bw = hal_data->current_channel_bw; + ch = hal_data->current_channel; + + for (path = 0; path < RF_PATH_MAX; path++) { + if (!HAL_SPEC_CHK_RF_PATH(hal_spec, band, path)) + break; + + max_pwr[path] = -127; /* min value of s8 */ +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: [%s][%c]\n", band_str(band), rf_path_char(path)); +#endif + for (rs = 0; rs < RATE_SECTION_NUM; rs++) { + tx_num = rate_section_to_tx_num(rs); + + if (tx_num >= hal_spec->tx_nss_num) + continue; + + if (band == BAND_ON_5G && IS_CCK_RATE_SECTION(rs)) + continue; + + if (IS_VHT_RATE_SECTION(rs) && !IS_HARDWARE_TYPE_JAGUAR_ALL(adapter)) + continue; + + rate_num = rate_section_rate_num(rs); + + /* get power by rate in db */ + for (n = rate_num - 1; n >= 0; n--) { + pwr = phy_get_tx_power_final_absolute_value(adapter, path, rates_by_sections[rs].rates[n], bw, ch); + max_pwr[path] = MAX(max_pwr[path], pwr); +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: %9s = %2d\n", + MGN_RATE_STR(rates_by_sections[rs].rates[n]), pwr); +#endif + } + } + } +#if (RM_MORE_DBG_MSG) + RTW_INFO("RM: path_a max_pwr=%ddBm\n", max_pwr[0]); +#endif + *path_a = max_pwr[0]; + return 0; +} + +u8 rm_gen_dialog_token(_adapter *padapter) +{ + struct rm_priv *prmpriv = &(padapter->rmpriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + + do { + pmlmeinfo->dialogToken++; + } while (pmlmeinfo->dialogToken == 0); + + return pmlmeinfo->dialogToken; +} + +u8 rm_gen_meas_token(_adapter *padapter) +{ + struct rm_priv *prmpriv = &(padapter->rmpriv); + + do { + prmpriv->meas_token++; + } while (prmpriv->meas_token == 0); + + return prmpriv->meas_token; +} + +u32 rm_gen_rmid(_adapter *padapter, struct rm_obj *prm, u8 role) +{ + u32 rmid; + + if (prm->psta == NULL) + goto err; + + if (prm->q.diag_token == 0) + goto err; + + rmid = prm->psta->cmn.aid << 16 + | prm->q.diag_token << 8 + | role; + + return rmid; +err: + RTW_ERR("RM: unable to gen rmid\n"); + return 0; +} + +#endif /* CONFIG_RTW_80211K */ diff --git a/core/rtw_roch.c b/core/rtw_roch.c new file mode 100644 index 0000000..2674fe6 --- /dev/null +++ b/core/rtw_roch.c @@ -0,0 +1,591 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2020 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include + +#ifdef CONFIG_IOCTL_CFG80211 +u8 rtw_roch_stay_in_cur_chan(_adapter *padapter) +{ + int i; + _adapter *iface; + struct mlme_priv *pmlmepriv; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + u8 rst = _FALSE; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (iface) { + pmlmepriv = &iface->mlmepriv; + + if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS | WIFI_UNDER_KEY_HANDSHAKE) == _TRUE) { + RTW_INFO(ADPT_FMT"- WIFI_UNDER_LINKING |WIFI_UNDER_WPS | WIFI_UNDER_KEY_HANDSHAKE (mlme state:0x%x)\n", + ADPT_ARG(iface), get_fwstate(&iface->mlmepriv)); + rst = _TRUE; + break; + } + #ifdef CONFIG_AP_MODE + if (MLME_IS_AP(iface) || MLME_IS_MESH(iface)) { + if (rtw_ap_sta_states_check(iface) == _TRUE) { + rst = _TRUE; + break; + } + } + #endif + } + } + + return rst; +} + +static int rtw_ro_ch_handler(_adapter *adapter, u8 *buf) +{ + int ret = H2C_SUCCESS; + struct rtw_roch_parm *roch_parm = (struct rtw_roch_parm *)buf; + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(adapter); + struct roch_info *prochinfo = &adapter->rochinfo; +#ifdef CONFIG_CONCURRENT_MODE + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; +#endif + u8 ready_on_channel = _FALSE; + u8 remain_ch; + unsigned int duration; + + _enter_critical_mutex(&pwdev_priv->roch_mutex, NULL); + + if (rtw_cfg80211_get_is_roch(adapter) != _TRUE) + goto exit; + + remain_ch = (u8)ieee80211_frequency_to_channel(roch_parm->ch.center_freq); + duration = roch_parm->duration; + + RTW_INFO(FUNC_ADPT_FMT" ch:%u duration:%d, cookie:0x%llx\n" + , FUNC_ADPT_ARG(adapter), remain_ch, roch_parm->duration, roch_parm->cookie); + + if (roch_parm->wdev && roch_parm->cookie) { + if (prochinfo->ro_ch_wdev != roch_parm->wdev) { + RTW_WARN(FUNC_ADPT_FMT" ongoing wdev:%p, wdev:%p\n" + , FUNC_ADPT_ARG(adapter), prochinfo->ro_ch_wdev, roch_parm->wdev); + rtw_warn_on(1); + } + + if (prochinfo->remain_on_ch_cookie != roch_parm->cookie) { + RTW_WARN(FUNC_ADPT_FMT" ongoing cookie:0x%llx, cookie:0x%llx\n" + , FUNC_ADPT_ARG(adapter), prochinfo->remain_on_ch_cookie, roch_parm->cookie); + rtw_warn_on(1); + } + } + + if (rtw_roch_stay_in_cur_chan(adapter) == _TRUE) { + remain_ch = rtw_mi_get_union_chan(adapter); + RTW_INFO(FUNC_ADPT_FMT" stay in union ch:%d\n", FUNC_ADPT_ARG(adapter), remain_ch); + } + + #ifdef CONFIG_CONCURRENT_MODE + if (rtw_mi_check_status(adapter, MI_LINKED) && (0 != rtw_mi_get_union_chan(adapter))) { + if ((remain_ch != rtw_mi_get_union_chan(adapter)) && !check_fwstate(&adapter->mlmepriv, WIFI_ASOC_STATE)) { + if (remain_ch != pmlmeext->cur_channel + #ifdef RTW_ROCH_BACK_OP + || ATOMIC_READ(&pwdev_priv->switch_ch_to) == 1 + #endif + ) { + rtw_leave_opch(adapter); + + #ifdef RTW_ROCH_BACK_OP + RTW_INFO("%s, set switch ch timer, duration=%d\n", __func__, prochinfo->max_away_dur); + ATOMIC_SET(&pwdev_priv->switch_ch_to, 0); + /* remain_ch is not same as union channel. duration is max_away_dur to + * back to AP's channel. + */ + _set_timer(&prochinfo->ap_roch_ch_switch_timer, prochinfo->max_away_dur); + #endif + } + } + ready_on_channel = _TRUE; + } else + #endif /* CONFIG_CONCURRENT_MODE */ + { + if (remain_ch != rtw_get_oper_ch(adapter)) + ready_on_channel = _TRUE; + } + + if (ready_on_channel == _TRUE) { + #ifndef RTW_SINGLE_WIPHY + if (!check_fwstate(&adapter->mlmepriv, WIFI_ASOC_STATE)) + #endif + { + #ifdef CONFIG_CONCURRENT_MODE + if (rtw_get_oper_ch(adapter) != remain_ch) + #endif + { + /* if (!padapter->mlmepriv.LinkDetectInfo.bBusyTraffic) */ + set_channel_bwmode(adapter, remain_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + } + } + } + + #ifdef CONFIG_BT_COEXIST + rtw_btcoex_ScanNotify(adapter, _TRUE); + #endif + + RTW_INFO("%s, set ro ch timer, duration=%d\n", __func__, duration); + _set_timer(&prochinfo->remain_on_ch_timer, duration); + +exit: + _exit_critical_mutex(&pwdev_priv->roch_mutex, NULL); + + return ret; +} + +static int rtw_cancel_ro_ch_handler(_adapter *padapter, u8 *buf) +{ + int ret = H2C_SUCCESS; + struct rtw_roch_parm *roch_parm = (struct rtw_roch_parm *)buf; + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); + struct roch_info *prochinfo = &padapter->rochinfo; + struct wireless_dev *wdev; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif + u8 ch, bw, offset; + + _enter_critical_mutex(&pwdev_priv->roch_mutex, NULL); + + if (rtw_cfg80211_get_is_roch(padapter) != _TRUE) + goto exit; + + if (roch_parm->wdev && roch_parm->cookie) { + if (prochinfo->ro_ch_wdev != roch_parm->wdev) { + RTW_WARN(FUNC_ADPT_FMT" ongoing wdev:%p, wdev:%p\n" + , FUNC_ADPT_ARG(padapter), prochinfo->ro_ch_wdev, roch_parm->wdev); + rtw_warn_on(1); + } + + if (prochinfo->remain_on_ch_cookie != roch_parm->cookie) { + RTW_WARN(FUNC_ADPT_FMT" ongoing cookie:0x%llx, cookie:0x%llx\n" + , FUNC_ADPT_ARG(padapter), prochinfo->remain_on_ch_cookie, roch_parm->cookie); + rtw_warn_on(1); + } + } + +#if defined(RTW_ROCH_BACK_OP) && defined(CONFIG_CONCURRENT_MODE) + _cancel_timer_ex(&prochinfo->ap_roch_ch_switch_timer); + ATOMIC_SET(&pwdev_priv->switch_ch_to, 1); +#endif + + if (rtw_mi_get_ch_setting_union(padapter, &ch, &bw, &offset) != 0) { + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), ch, bw, offset); +#ifdef CONFIG_P2P + } else if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->listen_channel) { + ch = pwdinfo->listen_channel; + bw = CHANNEL_WIDTH_20; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to listen ch - ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), ch, bw, offset); +#endif + } else { + ch = prochinfo->restore_channel; + bw = CHANNEL_WIDTH_20; + offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + if (0) + RTW_INFO(FUNC_ADPT_FMT" back to restore ch - ch:%u, bw:%u, offset:%u\n", + FUNC_ADPT_ARG(padapter), ch, bw, offset); + } + + set_channel_bwmode(padapter, ch, offset, bw); + rtw_back_opch(padapter); +#ifdef CONFIG_P2P + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); +#ifdef CONFIG_DEBUG_CFG80211 + RTW_INFO("%s, role=%d, p2p_state=%d\n", __func__, rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo)); +#endif +#endif + + wdev = prochinfo->ro_ch_wdev; + + rtw_cfg80211_set_is_roch(padapter, _FALSE); + prochinfo->ro_ch_wdev = NULL; + rtw_cfg80211_set_last_ro_ch_time(padapter); + + rtw_cfg80211_remain_on_channel_expired(wdev + , prochinfo->remain_on_ch_cookie + , &prochinfo->remain_on_ch_channel + , prochinfo->remain_on_ch_type, GFP_KERNEL); + + RTW_INFO("cfg80211_remain_on_channel_expired cookie:0x%llx\n" + , prochinfo->remain_on_ch_cookie); + +#ifdef CONFIG_BT_COEXIST + rtw_btcoex_ScanNotify(padapter, _FALSE); +#endif + +exit: + _exit_critical_mutex(&pwdev_priv->roch_mutex, NULL); + + return ret; +} + +static void rtw_ro_ch_timer_process(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + + rtw_cancel_roch_cmd(adapter, 0, NULL, 0); +} +#endif /* CONFIG_IOCTL_CFG80211 */ + +#if (defined(CONFIG_P2P) && defined(CONFIG_CONCURRENT_MODE)) || defined(CONFIG_IOCTL_CFG80211) +s32 rtw_roch_wk_hdl(_adapter *padapter, int intCmdType, u8 *buf) +{ + int ret = H2C_SUCCESS; + + switch (intCmdType) { + +#ifdef CONFIG_IOCTL_CFG80211 + case ROCH_RO_CH_WK: + ret = rtw_ro_ch_handler(padapter, buf); + break; + case ROCH_CANCEL_RO_CH_WK: + ret = rtw_cancel_ro_ch_handler(padapter, buf); + break; +#endif + +#ifdef CONFIG_CONCURRENT_MODE + case ROCH_AP_ROCH_CH_SWITCH_PROCESS_WK: + rtw_concurrent_handler(padapter); + break; +#endif + + default: + rtw_warn_on(1); + break; + } + + return ret; +} + +static int get_roch_parm_size(struct rtw_roch_parm *roch_parm) +{ +#ifdef CONFIG_IOCTL_CFG80211 + return (roch_parm ? sizeof(*roch_parm) : 0); +#else + rtw_warn_on(roch_parm); + return 0; +#endif +} + +u8 rtw_roch_wk_cmd(_adapter *padapter, int intCmdType, struct rtw_roch_parm *roch_parm, u8 flags) +{ + struct cmd_obj *ph2c = NULL; + struct drvextra_cmd_parm *pdrvextra_cmd_parm = NULL; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct submit_ctx sctx; + u8 res = _SUCCESS; + + if (flags & RTW_CMDF_DIRECTLY) { + /* no need to enqueue, do the cmd hdl directly and free cmd parameter */ + if (H2C_SUCCESS != rtw_roch_wk_hdl(padapter, intCmdType, (u8 *)roch_parm)) + res = _FAIL; + goto free_parm; + } else { + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (!ph2c) { + res = _FAIL; + goto free_parm; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (!pdrvextra_cmd_parm) { + res = _FAIL; + goto free_parm; + } + + pdrvextra_cmd_parm->ec_id = ROCH_WK_CID; + pdrvextra_cmd_parm->type = intCmdType; + pdrvextra_cmd_parm->size = get_roch_parm_size(roch_parm); + pdrvextra_cmd_parm->pbuf = (u8 *)roch_parm; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + if (flags & RTW_CMDF_WAIT_ACK) { + ph2c->sctx = &sctx; + rtw_sctx_init(&sctx, 10 * 1000); + } + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + + if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) { + rtw_sctx_wait(&sctx, __func__); + _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status == RTW_SCTX_SUBMITTED) + ph2c->sctx = NULL; + _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL); + if (sctx.status != RTW_SCTX_DONE_SUCCESS) + res = _FAIL; + } + } + + return res; + +free_parm: + if (roch_parm) + rtw_mfree((u8 *)roch_parm, get_roch_parm_size(roch_parm)); + if (ph2c) + rtw_mfree((u8 *)ph2c, sizeof(*ph2c)); + + return res; +} + +#ifdef CONFIG_CONCURRENT_MODE +void rtw_ap_roch_ch_switch_timer_process(void *ctx) +{ + _adapter *adapter = (_adapter *)ctx; +#ifdef CONFIG_IOCTL_CFG80211 + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(adapter); +#endif + +#ifdef CONFIG_IOCTL_CFG80211 + ATOMIC_SET(&pwdev_priv->switch_ch_to, 1); +#endif + + rtw_roch_wk_cmd(adapter, ROCH_AP_ROCH_CH_SWITCH_PROCESS_WK, NULL, 0); +} + +static bool chk_need_stay_in_cur_chan(_adapter *padapter) +{ +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + /* When CONFIG_FULL_CH_IN_P2P_HANDSHAKE is defined and the + * interface is in the P2P_STATE_GONEGO_OK state, do not let the + * interface switch to the listen channel, because the interface will + * switch to the OP channel after the GO negotiation is successful. + */ + if (padapter->registrypriv.full_ch_in_p2p_handshake == 1 && rtw_p2p_chk_state(pwdinfo , P2P_STATE_GONEGO_OK)) { + RTW_INFO("%s, No linked interface now, but go nego ok, do not back to listen channel\n", __func__); + return _TRUE; + } +#endif + + return _FALSE; +} + +static bool chk_driver_interface(_adapter *padapter, u8 driver_interface) +{ +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if (pwdinfo->driver_interface == driver_interface) + return _TRUE; +#elif defined(CONFIG_IOCTL_CFG80211) + if (driver_interface == DRIVER_CFG80211) + return _TRUE; +#endif + + return _FALSE; +} + +static u8 get_remain_ch(_adapter *padapter) +{ + struct roch_info *prochinfo = &padapter->rochinfo; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; +#endif + u8 remain_ch; + +#ifdef CONFIG_P2P + remain_ch = pwdinfo->listen_channel; +#elif defined(CONFIG_IOCTL_CFG80211) + if (chk_driver_interface(padapter, DRIVER_CFG80211)) + remain_ch = ieee80211_frequency_to_channel(prochinfo->remain_on_ch_channel.center_freq); + else + rtw_warn_on(1); +#endif + + return remain_ch; +} + +void rtw_concurrent_handler(_adapter *padapter) +{ +#ifdef CONFIG_IOCTL_CFG80211 + struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter); +#endif + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + struct roch_info *prochinfo = &padapter->rochinfo; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8; +#endif + u8 remain_ch = get_remain_ch(padapter); + +#ifdef CONFIG_IOCTL_CFG80211 + if (chk_driver_interface(padapter, DRIVER_CFG80211) + && !rtw_cfg80211_get_is_roch(padapter)) + return; +#endif + + if (rtw_mi_check_status(padapter, MI_LINKED)) { + u8 union_ch = rtw_mi_get_union_chan(padapter); + u8 union_bw = rtw_mi_get_union_bw(padapter); + u8 union_offset = rtw_mi_get_union_offset(padapter); + unsigned int duration; + + #ifdef CONFIG_P2P + pwdinfo->operating_channel = union_ch; + #endif + + if (chk_driver_interface(padapter, DRIVER_CFG80211)) { + #ifdef CONFIG_IOCTL_CFG80211 + _enter_critical_mutex(&pwdev_priv->roch_mutex, NULL); + + if (rtw_get_oper_ch(padapter) != union_ch) { + /* Current channel is not AP's channel - switching to AP's channel */ + RTW_INFO("%s, switch ch back to union=%u,%u, %u\n" + , __func__, union_ch, union_bw, union_offset); + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + rtw_back_opch(padapter); + + /* Now, the driver stays on AP's channel. We should stay on AP's + * channel for min_home_dur (duration) and next switch channel is + * listen channel. + */ + duration = prochinfo->min_home_dur; + } else { + /* Current channel is AP's channel - switching to listen channel */ + RTW_INFO("%s, switch ch to roch=%u\n" + , __func__, remain_ch); + rtw_leave_opch(padapter); + set_channel_bwmode(padapter, + remain_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + + /* Now, the driver stays on listen channel. We should stay on listen + * channel for max_away_dur (duration) and next switch channel is AP's + * channel. + */ + duration = prochinfo->max_away_dur; + } + + /* set channel switch timer */ + ATOMIC_SET(&pwdev_priv->switch_ch_to, 0); + _set_timer(&prochinfo->ap_roch_ch_switch_timer, duration); + RTW_INFO("%s, set switch ch timer, duration=%d\n", __func__, duration); + + _exit_critical_mutex(&pwdev_priv->roch_mutex, NULL); + #endif + } + #ifdef CONFIG_P2P + else if (chk_driver_interface(padapter, DRIVER_WEXT)) { + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) { + /* Now, the driver stays on the AP's channel. */ + /* If the pwdinfo->ext_listen_period = 0, that means the P2P listen state is not available on listen channel. */ + if (pwdinfo->ext_listen_period > 0) { + RTW_INFO("[%s] P2P_STATE_IDLE, ext_listen_period = %d\n", __FUNCTION__, pwdinfo->ext_listen_period); + + if (union_ch != pwdinfo->listen_channel) { + rtw_leave_opch(padapter); + set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + } + + rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN); + + if (!rtw_mi_check_mlmeinfo_state(padapter, WIFI_FW_AP_STATE)) { + val8 = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + } + /* Todo: To check the value of pwdinfo->ext_listen_period is equal to 0 or not. */ + _set_timer(&prochinfo->ap_roch_ch_switch_timer, pwdinfo->ext_listen_period); + } + + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) || + rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL) || + (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == _FALSE) || + rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ)) { + /* Now, the driver is in the listen state of P2P mode. */ + RTW_INFO("[%s] P2P_STATE_IDLE, ext_listen_interval = %d\n", __FUNCTION__, pwdinfo->ext_listen_interval); + + /* Commented by Albert 2012/11/01 */ + /* If the AP's channel is the same as the listen channel, we should still be in the listen state */ + /* Other P2P device is still able to find this device out even this device is in the AP's channel. */ + /* So, configure this device to be able to receive the probe request frame and set it to listen state. */ + if (union_ch != pwdinfo->listen_channel) { + + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + if (!rtw_mi_check_status(padapter, MI_AP_MODE)) { + val8 = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + } + rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE); + rtw_back_opch(padapter); + } + + /* Todo: To check the value of pwdinfo->ext_listen_interval is equal to 0 or not. */ + _set_timer(&prochinfo->ap_roch_ch_switch_timer, pwdinfo->ext_listen_interval); + + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_OK)) { + /* The driver had finished the P2P handshake successfully. */ + val8 = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + set_channel_bwmode(padapter, union_ch, union_offset, union_bw); + rtw_back_opch(padapter); + + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) { + val8 = 1; + set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq_p2p(padapter, NULL); + _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == _TRUE) { + val8 = 1; + set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq_p2p(padapter, NULL); + _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); + } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ) && pwdinfo->invitereq_info.benable == _TRUE) { + /* + val8 = 1; + set_channel_bwmode(padapter, , HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq_p2p(padapter, NULL); + _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT ); + */ + } + } + #endif /* CONFIG_P2P */ + } else if (!chk_need_stay_in_cur_chan(padapter)) { + set_channel_bwmode(padapter, remain_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20); + } +} +#endif /* CONFIG_CONCURRENT_MODE */ + +void rtw_init_roch_info(_adapter *padapter) +{ + struct roch_info *prochinfo = &padapter->rochinfo; + + _rtw_memset(prochinfo, 0x00, sizeof(struct roch_info)); + +#ifdef CONFIG_CONCURRENT_MODE + rtw_init_timer(&prochinfo->ap_roch_ch_switch_timer, padapter, rtw_ap_roch_ch_switch_timer_process, padapter); +#ifdef CONFIG_IOCTL_CFG80211 + prochinfo->min_home_dur = 1500; /* min duration for traffic, home_time */ + prochinfo->max_away_dur = 250; /* max acceptable away duration, home_away_time */ +#endif +#endif + +#ifdef CONFIG_IOCTL_CFG80211 + rtw_init_timer(&prochinfo->remain_on_ch_timer, padapter, rtw_ro_ch_timer_process, padapter); +#endif +} +#endif /* (defined(CONFIG_P2P) && defined(CONFIG_CONCURRENT_MODE)) || defined(CONFIG_IOCTL_CFG80211) */ \ No newline at end of file diff --git a/core/rtw_rson.c b/core/rtw_rson.c new file mode 100644 index 0000000..dba8fb7 --- /dev/null +++ b/core/rtw_rson.c @@ -0,0 +1,592 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA + * + * + ******************************************************************************/ +#define _RTW_RSON_C_ + +#include + +#ifdef CONFIG_RTW_REPEATER_SON + +/******** Custommize Part ***********************/ + +unsigned char RTW_RSON_OUI[] = {0xFA, 0xFA, 0xFA}; +#define RSON_SCORE_DIFF_TH 8 + +/* + Calculate the corresponding score. +*/ +inline u8 rtw_cal_rson_score(struct rtw_rson_struct *cand_rson_data, NDIS_802_11_RSSI Rssi) +{ + if ((cand_rson_data->hopcnt == RTW_RSON_HC_NOTREADY) + || (cand_rson_data->connectible == RTW_RSON_DENYCONNECT)) + return RTW_RSON_SCORE_NOTCNNT; + + return RTW_RSON_SCORE_MAX - (cand_rson_data->hopcnt * 10) + (Rssi/10); +} + +/*************************************************/ + + +static u8 rtw_rson_block_bssid_idx = 0; +u8 rtw_rson_block_bssid[10][6] = { + /*{0x02, 0xE0, 0x4C, 0x07, 0xC3, 0xF6}*/ +}; + +/* fake root, regard a real AP as a SO root */ +static u8 rtw_rson_root_bssid_idx = 0; +u8 rtw_rson_root_bssid[10][6] = { + /*{0x1c, 0x5f, 0x2b, 0x5a, 0x60, 0x24}*/ +}; + +int is_match_bssid(u8 *mac, u8 bssid_array[][6], int num) +{ + int i; + + for (i = 0; i < num; i++) + if (_rtw_memcmp(mac, bssid_array[i], 6) == _TRUE) + return _TRUE; + return _FALSE; +} + +void init_rtw_rson_data(struct dvobj_priv *dvobj) +{ + /*Aries todo. if pdvobj->rson_data.ver == 1 */ + dvobj->rson_data.ver = RTW_RSON_VER; + dvobj->rson_data.id = CONFIG_RTW_REPEATER_SON_ID; +#ifdef CONFIG_RTW_REPEATER_SON_ROOT + dvobj->rson_data.hopcnt = RTW_RSON_HC_ROOT; + dvobj->rson_data.connectible = RTW_RSON_ALLOWCONNECT; +#else + dvobj->rson_data.hopcnt = RTW_RSON_HC_NOTREADY; + dvobj->rson_data.connectible = RTW_RSON_DENYCONNECT; +#endif + dvobj->rson_data.loading = 0; + _rtw_memset(dvobj->rson_data.res, 0xAA, sizeof(dvobj->rson_data.res)); +} + +void rtw_rson_get_property_str(_adapter *padapter, char *rson_data_str) +{ + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + + sprintf(rson_data_str, "version : \t%d\nid : \t\t%08x\nhop count : \t%d\nconnectible : \t%s\nloading : \t%d\nreserve : \t%16ph\n", + pdvobj->rson_data.ver, + pdvobj->rson_data.id, + pdvobj->rson_data.hopcnt, + pdvobj->rson_data.connectible ? "connectable":"unconnectable", + pdvobj->rson_data.loading, + pdvobj->rson_data.res); +} + +int str2hexbuf(char *str, u8 *hexbuf, int len) +{ + u8 *p; + int i, slen, idx = 0; + + p = (unsigned char *)str; + if ((*p != '0') || (*(p+1) != 'x')) + return _FALSE; + slen = strlen(str); + if (slen > (len*2) + 2) + return _FALSE; + p += 2; + for (i = 0 ; i < len; i++, idx = idx+2) { + hexbuf[i] = key_2char2num(p[idx], p[idx + 1]); + if (slen <= idx+2) + break; + } + return _TRUE; +} + +int rtw_rson_set_property(_adapter *padapter, char *field, char *value) +{ + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + int num = 0; + + if (_rtw_memcmp(field, (u8 *)"ver", 3) == _TRUE) + pdvobj->rson_data.ver = rtw_atoi(value); + else if (_rtw_memcmp(field, (u8 *)"id", 2) == _TRUE) + num = sscanf(value, "%08x", &(pdvobj->rson_data.id)); + else if (_rtw_memcmp(field, (u8 *)"hc", 2) == _TRUE) + num = sscanf(value, "%hhu", &(pdvobj->rson_data.hopcnt)); + else if (_rtw_memcmp(field, (u8 *)"cnt", 3) == _TRUE) + num = sscanf(value, "%hhu", &(pdvobj->rson_data.connectible)); + else if (_rtw_memcmp(field, (u8 *)"loading", 2) == _TRUE) + num = sscanf(value, "%hhu", &(pdvobj->rson_data.loading)); + else if (_rtw_memcmp(field, (u8 *)"res", 2) == _TRUE) { + str2hexbuf(value, pdvobj->rson_data.res, 16); + return 1; + } else + return _FALSE; + return num; +} + +/* + return : TRUE -- competitor is taking advantage than condidate + FALSE -- we should continue keeping candidate +*/ +int rtw_rson_choose(struct wlan_network **candidate, struct wlan_network *competitor) +{ + s16 comp_score = 0, cand_score = 0; + struct rtw_rson_struct rson_cand, rson_comp; + + if (is_match_bssid(competitor->network.MacAddress, rtw_rson_block_bssid, rtw_rson_block_bssid_idx) == _TRUE) + return _FALSE; + + if ((competitor == NULL) + || (rtw_get_rson_struct(&(competitor->network), &rson_comp) != _TRUE) + || (rson_comp.id != CONFIG_RTW_REPEATER_SON_ID)) + return _FALSE; + + comp_score = rtw_cal_rson_score(&rson_comp, competitor->network.Rssi); + if (comp_score == RTW_RSON_SCORE_NOTCNNT) + return _FALSE; + + if (*candidate == NULL) + return _TRUE; + if (rtw_get_rson_struct(&((*candidate)->network), &rson_cand) != _TRUE) + return _FALSE; + + cand_score = rtw_cal_rson_score(&rson_cand, (*candidate)->network.Rssi); + RTW_INFO("%s: competitor_score=%d, candidate_score=%d\n", __func__, comp_score, cand_score); + if (comp_score - cand_score > RSON_SCORE_DIFF_TH) + return _TRUE; + + return _FALSE; +} + +inline u8 rtw_rson_varify_ie(u8 *p) +{ + u8 *ptr = NULL; + u8 ver; + u32 id; + u8 hopcnt; + u8 allcnnt; + + ptr = p + 2 + sizeof(RTW_RSON_OUI); + ver = *ptr; + + /* for (ver == 1) */ + if (ver != 1) + return _FALSE; + + return _TRUE; +} + +/* + Parsing RTK self-organization vendor IE +*/ +int rtw_get_rson_struct(WLAN_BSSID_EX *bssid, struct rtw_rson_struct *rson_data) +{ + sint limit = 0; + u32 len; + u8 *p; + + if ((rson_data == NULL) || (bssid == NULL)) + return -EINVAL; + + /* Default */ + rson_data->id = 0; + rson_data->ver = 0; + rson_data->hopcnt = 0; + rson_data->connectible = 0; + rson_data->loading = 0; + /* fake root */ + if (is_match_bssid(bssid->MacAddress, rtw_rson_root_bssid, rtw_rson_root_bssid_idx) == _TRUE) { + rson_data->id = CONFIG_RTW_REPEATER_SON_ID; + rson_data->ver = RTW_RSON_VER; + rson_data->hopcnt = RTW_RSON_HC_ROOT; + rson_data->connectible = RTW_RSON_ALLOWCONNECT; + rson_data->loading = 0; + return _TRUE; + } + limit = bssid->IELength - _BEACON_IE_OFFSET_; + + for (p = bssid->IEs + _BEACON_IE_OFFSET_; ; p += (len + 2)) { + p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &len, limit); + limit -= len; + if ((p == NULL) || (len == 0)) + break; + if (p && (_rtw_memcmp(p + 2, RTW_RSON_OUI, sizeof(RTW_RSON_OUI)) == _TRUE) + && rtw_rson_varify_ie(p)) { + p = p + 2 + sizeof(RTW_RSON_OUI); + rson_data->ver = *p; + /* for (ver == 1) */ + p = p + 1; + rson_data->id = le32_to_cpup((__le32 *)p); + p = p + 4; + rson_data->hopcnt = *p; + p = p + 1; + rson_data->connectible = *p; + p = p + 1; + rson_data->loading = *p; + + return _TRUE; + } + } + return -EBADMSG; +} + +u32 rtw_rson_append_ie(_adapter *padapter, unsigned char *pframe, u32 *len) +{ + u8 *ptr, *ori, ie_len = 0; + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); +/* static int iii = 0;*/ + + if ((!pdvobj) || (!pframe)) + return 0; + ptr = ori = pframe; + *ptr++ = _VENDOR_SPECIFIC_IE_; + *ptr++ = ie_len = sizeof(RTW_RSON_OUI)+sizeof(pdvobj->rson_data); + _rtw_memcpy(ptr, RTW_RSON_OUI, sizeof(RTW_RSON_OUI)); + ptr = ptr + sizeof(RTW_RSON_OUI); + *ptr++ = pdvobj->rson_data.ver; + *(s32 *)ptr = cpu_to_le32(pdvobj->rson_data.id); + ptr = ptr + sizeof(pdvobj->rson_data.id); + *ptr++ = pdvobj->rson_data.hopcnt; + *ptr++ = pdvobj->rson_data.connectible; + *ptr++ = pdvobj->rson_data.loading; + _rtw_memcpy(ptr, pdvobj->rson_data.res, sizeof(pdvobj->rson_data.res)); + pframe = ptr; +/* + iii = iii % 20; + if (iii++ == 0) + RTW_INFO("%s : RTW RSON IE : %20ph\n", __func__, ori); +*/ + *len += (ie_len+2); + return ie_len; + +} + +void rtw_rson_do_disconnect(_adapter *padapter) +{ + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); +#ifndef CONFIG_RTW_REPEATER_SON_ROOT + pdvobj->rson_data.ver = RTW_RSON_VER; + pdvobj->rson_data.id = CONFIG_RTW_REPEATER_SON_ID; + pdvobj->rson_data.hopcnt = RTW_RSON_HC_NOTREADY; + pdvobj->rson_data.connectible = RTW_RSON_DENYCONNECT; + pdvobj->rson_data.loading = 0; + #ifdef CONFIG_AP_MODE + rtw_mi_tx_beacon_hdl(padapter); + #endif +#endif +} + +void rtw_rson_join_done(_adapter *padapter) +{ + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + WLAN_BSSID_EX *cur_network = NULL; + struct rtw_rson_struct rson_data; + + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + if (!padapter->mlmepriv.cur_network_scanned) + return; + cur_network = &(padapter->mlmepriv.cur_network_scanned->network); + if (rtw_get_rson_struct(cur_network, &rson_data) != _TRUE) { + RTW_ERR("%s: try to join a improper network(%s)\n", __func__, cur_network->Ssid.Ssid); + return; + } + +#ifndef CONFIG_RTW_REPEATER_SON_ROOT + /* update rson_data */ + pdvobj->rson_data.ver = RTW_RSON_VER; + pdvobj->rson_data.id = rson_data.id; + pdvobj->rson_data.hopcnt = rson_data.hopcnt + 1; + pdvobj->rson_data.connectible = RTW_RSON_ALLOWCONNECT; + pdvobj->rson_data.loading = 0; + #ifdef CONFIG_AP_MODE + rtw_mi_tx_beacon_hdl(padapter); + #endif +#endif +} + +int rtw_rson_isupdate_roamcan(struct mlme_priv *mlme + , struct wlan_network **candidate, struct wlan_network *competitor) +{ + struct rtw_rson_struct rson_cand, rson_comp, rson_curr; + s16 comp_score, cand_score, curr_score; + + if ((competitor == NULL) + || (rtw_get_rson_struct(&(competitor->network), &rson_comp) != _TRUE) + || (rson_comp.id != CONFIG_RTW_REPEATER_SON_ID)) + return _FALSE; + + if (is_match_bssid(competitor->network.MacAddress, rtw_rson_block_bssid, rtw_rson_block_bssid_idx) == _TRUE) + return _FALSE; + + if ((!mlme->cur_network_scanned) + || (mlme->cur_network_scanned == competitor) + || (rtw_get_rson_struct(&(mlme->cur_network_scanned->network), &rson_curr)) != _TRUE) + return _FALSE; + + if (rtw_get_passing_time_ms((u32)competitor->last_scanned) >= mlme->roam_scanr_exp_ms) + return _FALSE; + + comp_score = rtw_cal_rson_score(&rson_comp, competitor->network.Rssi); + curr_score = rtw_cal_rson_score(&rson_curr, mlme->cur_network_scanned->network.Rssi); + if (comp_score - curr_score < RSON_SCORE_DIFF_TH) + return _FALSE; + + if (*candidate == NULL) + return _TRUE; + + if (rtw_get_rson_struct(&((*candidate)->network), &rson_cand) != _TRUE) { + RTW_ERR("%s : Unable to get rson_struct from candidate(%s -- " MAC_FMT")\n", + __func__, (*candidate)->network.Ssid.Ssid, MAC_ARG((*candidate)->network.MacAddress)); + return _FALSE; + } + cand_score = rtw_cal_rson_score(&rson_cand, (*candidate)->network.Rssi); + RTW_DBG("comp_score=%d , cand_score=%d , curr_score=%d\n", comp_score, cand_score, curr_score); + if (cand_score < comp_score) + return _TRUE; + +#if 0 /* Handle 11R protocol */ +#ifdef CONFIG_RTW_80211R + if (rtw_chk_ft_flags(adapter, RTW_FT_SUPPORTED)) { + ptmp = rtw_get_ie(&competitor->network.IEs[12], _MDIE_, &mdie_len, competitor->network.IELength-12); + if (ptmp) { + if (!_rtw_memcmp(&pftpriv->mdid, ptmp+2, 2)) + goto exit; + + /*The candidate don't support over-the-DS*/ + if (rtw_chk_ft_flags(adapter, RTW_FT_STA_OVER_DS_SUPPORTED)) { + if ((rtw_chk_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED) && !(*(ptmp+4) & 0x01)) || + (!rtw_chk_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED) && (*(ptmp+4) & 0x01))) { + RTW_INFO("FT: ignore the candidate(" MAC_FMT ") for over-the-DS\n", MAC_ARG(competitor->network.MacAddress)); + rtw_clr_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED); + goto exit; + } + } + } else + goto exit; + } +#endif +#endif + return _FALSE; +} + +void rtw_rson_show_survey_info(struct seq_file *m, _list *plist, _list *phead) +{ + struct wlan_network *pnetwork = NULL; + struct rtw_rson_struct rson_data; + s16 rson_score; + u16 index = 0; + + RTW_PRINT_SEL(m, "%5s %-17s %3s %5s %14s %10s %-3s %5s %32s\n", "index", "bssid", "ch", "id", "hop_cnt", "loading", "RSSI", "score", "ssid"); + while (1) { + if (rtw_end_of_queue_search(phead, plist) == _TRUE) + break; + + pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list); + if (!pnetwork) + break; + + _rtw_memset(&rson_data, 0, sizeof(rson_data)); + rson_score = 0; + if (rtw_get_rson_struct(&(pnetwork->network), &rson_data) == _TRUE) + rson_score = rtw_cal_rson_score(&rson_data, pnetwork->network.Rssi); + RTW_PRINT_SEL(m, "%5d "MAC_FMT" %3d 0x%08x %6d %10d %6d %6d %32s\n", + ++index, + MAC_ARG(pnetwork->network.MacAddress), + pnetwork->network.Configuration.DSConfig, + rson_data.id, + rson_data.hopcnt, + rson_data.loading, + (int)pnetwork->network.Rssi, + rson_score, + pnetwork->network.Ssid.Ssid); + plist = get_next(plist); + } + +} + +/* + Description : As a AP role, We need to check the qualify of associating STA. + We also need to check if we are ready to be associated. + + return : TRUE -- AP REJECT this STA + FALSE -- AP ACCEPT this STA +*/ +u8 rtw_rson_ap_check_sta(_adapter *padapter, u8 *pframe, uint pkt_len, unsigned short ie_offset) +{ + struct wlan_network *pnetwork = NULL; + struct rtw_rson_struct rson_target; + struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter); + int len = 0; + u8 ret = _FALSE; + u8 *p; + +#ifndef CONFIG_RTW_REPEATER_SON_ROOT + _rtw_memset(&rson_target, 0, sizeof(rson_target)); + for (p = pframe + WLAN_HDR_A3_LEN + ie_offset; ; p += (len + 2)) { + p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &len, pkt_len - WLAN_HDR_A3_LEN - ie_offset); + + if ((p == NULL) || (len == 0)) + break; + + if (p && (_rtw_memcmp(p + 2, RTW_RSON_OUI, sizeof(RTW_RSON_OUI)) == _TRUE) + && rtw_rson_varify_ie(p)) { + p = p + 2 + sizeof(RTW_RSON_OUI); + rson_target.ver = *p; + /* for (ver == 1) */ + p = p + 1; + rson_target.id = le32_to_cpup((__le32 *)p); + p = p + 4; + rson_target.hopcnt = *p; + p = p + 1; + rson_target.connectible = *p; + p = p + 1; + rson_target.loading = *p; + break; + } + } + + if (rson_target.id == 0) /* Normal STA, not a RSON STA */ + ret = _FALSE; + else if (rson_target.id != pdvobj->rson_data.id) { + ret = _TRUE; + RTW_INFO("%s : Reject AssoReq because RSON ID not match, STA=%08x, our=%08x\n", + __func__, rson_target.id, pdvobj->rson_data.id); + } else if ((pdvobj->rson_data.hopcnt == RTW_RSON_HC_NOTREADY) + || (pdvobj->rson_data.connectible == RTW_RSON_DENYCONNECT)) { + ret = _TRUE; + RTW_INFO("%s : Reject AssoReq becuase our hopcnt=%d or connectbile=%d\n", + __func__, pdvobj->rson_data.hopcnt, pdvobj->rson_data.connectible); + } +#endif + return ret; +} + +u8 rtw_rson_scan_wk_cmd(_adapter *padapter, int op) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 *extra_cmd_buf; + u8 res = _SUCCESS; + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); + if (pdrvextra_cmd_parm == NULL) { + rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj)); + res = _FAIL; + goto exit; + } + pdrvextra_cmd_parm->ec_id = RSON_SCAN_WK_CID; + pdrvextra_cmd_parm->type = op; + pdrvextra_cmd_parm->size = 0; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, CMD_SET_DRV_EXTRA); + + res = rtw_enqueue_cmd(pcmdpriv, ph2c); + +exit: + return res; + +} + +void rtw_rson_scan_cmd_hdl(_adapter *padapter, int op) +{ + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 val8; + + if (mlmeext_chk_scan_state(pmlmeext, SCAN_DISABLE) != _TRUE) + return; + if (op == RSON_SCAN_PROCESS) { + padapter->rtw_rson_scanstage = RSON_SCAN_PROCESS; + val8 = 0x1e; + rtw_hal_set_odm_var(padapter, HAL_ODM_INITIAL_GAIN, &val8, _FALSE); + val8 = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue_probereq(padapter, NULL, NULL); + /* stop rson_scan after 100ms */ + _set_timer(&(pmlmeext->rson_scan_timer), 100); + } else if (op == RSON_SCAN_DISABLE) { + padapter->rtw_rson_scanstage = RSON_SCAN_DISABLE; + val8 = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + val8 = 0xff; + rtw_hal_set_odm_var(padapter, HAL_ODM_INITIAL_GAIN, &val8, _FALSE); + /* report_surveydone_event(padapter);*/ + if (pmlmepriv->to_join == _TRUE) { + if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) != _TRUE) { + int s_ret; + + set_fwstate(pmlmepriv, WIFI_UNDER_LINKING); + pmlmepriv->to_join = _FALSE; + s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv); + if (s_ret == _SUCCESS) + _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT); + else if (s_ret == 2) { + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + rtw_indicate_connect(padapter); + } else { + RTW_INFO("try_to_join, but select scanning queue fail, to_roam:%d\n", rtw_to_roam(padapter)); + if (rtw_to_roam(padapter) != 0) { + if (rtw_dec_to_roam(padapter) == 0) { + rtw_set_to_roam(padapter, 0); + rtw_free_assoc_resources(padapter, _TRUE); + rtw_indicate_disconnect(padapter, 0, _FALSE); + } else + pmlmepriv->to_join = _TRUE; + } else + rtw_indicate_disconnect(padapter, 0, _FALSE); + _clr_fwstate_(pmlmepriv, WIFI_UNDER_LINKING); + } + } + } else { + if (rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE)) { + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) + && check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) { + if (rtw_select_roaming_candidate(pmlmepriv) == _SUCCESS) { +#ifdef CONFIG_RTW_80211R + if (rtw_chk_ft_flags(padapter, RTW_FT_OVER_DS_SUPPORTED)) { + start_clnt_ft_action(adapter, (u8 *)pmlmepriv->roam_network->network.MacAddress); + } else { + /*wait a little time to retrieve packets buffered in the current ap while scan*/ + _set_timer(&pmlmeext->ft_roam_timer, 30); + } +#else + receive_disconnect(padapter, pmlmepriv->cur_network.network.MacAddress + , WLAN_REASON_ACTIVE_ROAM, _FALSE); +#endif + } + } + } + issue_action_BSSCoexistPacket(padapter); + issue_action_BSSCoexistPacket(padapter); + issue_action_BSSCoexistPacket(padapter); + } + } else { + RTW_ERR("%s : improper parameter -- op = %d\n", __func__, op); + } +} + +#endif /* CONFIG_RTW_REPEATER_SON */ diff --git a/core/rtw_sdio.c b/core/rtw_sdio.c new file mode 100644 index 0000000..cdadbaf --- /dev/null +++ b/core/rtw_sdio.c @@ -0,0 +1,157 @@ +/****************************************************************************** + * + * Copyright(c) 2015 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_SDIO_C_ + +#include /* struct dvobj_priv and etc. */ +#include /* RTW_SDIO_ADDR_CMD52_GEN */ + +/* + * Description: + * Use SDIO cmd52 or cmd53 to read/write data + * + * Parameters: + * d pointer of device object(struct dvobj_priv) + * addr SDIO address, 17 bits + * buf buffer for I/O + * len length + * write 0:read, 1:write + * cmd52 0:cmd52, 1:cmd53 + * + * Return: + * _SUCCESS I/O ok. + * _FAIL I/O fail. + */ +static u8 sdio_io(struct dvobj_priv *d, u32 addr, void *buf, size_t len, u8 write, u8 cmd52) +{ +#ifdef DBG_SDIO +#if (DBG_SDIO >= 3) + struct sdio_data *sdio; +#endif /* DBG_SDIO >= 3 */ +#endif /* DBG_SDIO */ + u32 addr_drv; /* address with driver defined bit */ + int err; + u8 retry = 0; + u8 stop_retry = _FALSE; /* flag for stopping retry or not */ + + +#ifdef DBG_SDIO +#if (DBG_SDIO >= 3) + sdio = &d->intf_data; +#endif /* DBG_SDIO >= 3 */ +#endif /* DBG_SDIO */ + + if (rtw_is_surprise_removed(dvobj_get_primary_adapter(d))) { + RTW_ERR("%s: bSurpriseRemoved, skip %s 0x%05x, %zu bytes\n", + __FUNCTION__, write?"write":"read", addr, len); + return _FAIL; + } + + addr_drv = addr; + if (cmd52) + addr_drv = RTW_SDIO_ADDR_CMD52_GEN(addr_drv); + + do { + if (write) + err = d->intf_ops->write(d, addr_drv, buf, len, 0); + else + err = d->intf_ops->read(d, addr_drv, buf, len, 0); + if (!err) { + if (retry) { + RTW_INFO("%s: Retry %s OK! addr=0x%05x %zu bytes, retry=%u,%u\n", + __FUNCTION__, write?"write":"read", + addr, len, retry, ATOMIC_READ(&d->continual_io_error)); + RTW_INFO_DUMP("Data: ", buf, len); + } + rtw_reset_continual_io_error(d); + break; + } + RTW_ERR("%s: %s FAIL! error(%d) addr=0x%05x %zu bytes, retry=%u,%u\n", + __FUNCTION__, write?"write":"read", err, addr, len, + retry, ATOMIC_READ(&d->continual_io_error)); + +#ifdef DBG_SDIO +#if (DBG_SDIO >= 3) + if (sdio->dbg_enable) { + if (sdio->err_test && sdio->err_test_triggered) + sdio->err_test = 0; + + if (sdio->err_stop) { + RTW_ERR("%s: I/O error! Set surprise remove flag ON!\n", + __FUNCTION__); + rtw_set_surprise_removed(dvobj_get_primary_adapter(d)); + return _FAIL; + } + } +#endif /* DBG_SDIO >= 3 */ +#endif /* DBG_SDIO */ + + retry++; + stop_retry = rtw_inc_and_chk_continual_io_error(d); + if ((err == -1) || (stop_retry == _TRUE) || (retry > SD_IO_TRY_CNT)) { + /* critical error, unrecoverable */ + RTW_ERR("%s: Fatal error! Set surprise remove flag ON! (retry=%u,%u)\n", + __FUNCTION__, retry, ATOMIC_READ(&d->continual_io_error)); + rtw_set_surprise_removed(dvobj_get_primary_adapter(d)); + return _FAIL; + } + + /* WLAN IOREG or SDIO Local */ + if ((addr & 0x10000) || !(addr & 0xE000)) { + RTW_WARN("%s: Retry %s addr=0x%05x %zu bytes, retry=%u,%u\n", + __FUNCTION__, write?"write":"read", addr, len, + retry, ATOMIC_READ(&d->continual_io_error)); + continue; + } + return _FAIL; + } while (1); + + return _SUCCESS; +} + +u8 rtw_sdio_read_cmd52(struct dvobj_priv *d, u32 addr, void *buf, size_t len) +{ + return sdio_io(d, addr, buf, len, 0, 1); +} + +u8 rtw_sdio_read_cmd53(struct dvobj_priv *d, u32 addr, void *buf, size_t len) +{ + return sdio_io(d, addr, buf, len, 0, 0); +} + +u8 rtw_sdio_write_cmd52(struct dvobj_priv *d, u32 addr, void *buf, size_t len) +{ + return sdio_io(d, addr, buf, len, 1, 1); +} + +u8 rtw_sdio_write_cmd53(struct dvobj_priv *d, u32 addr, void *buf, size_t len) +{ + return sdio_io(d, addr, buf, len, 1, 0); +} + +u8 rtw_sdio_f0_read(struct dvobj_priv *d, u32 addr, void *buf, size_t len) +{ + int err; + u8 ret; + + + ret = _SUCCESS; + addr = RTW_SDIO_ADDR_F0_GEN(addr); + + err = d->intf_ops->read(d, addr, buf, len, 0); + if (err) + ret = _FAIL; + + return ret; +} diff --git a/core/rtw_security.c b/core/rtw_security.c new file mode 100644 index 0000000..639e1e5 --- /dev/null +++ b/core/rtw_security.c @@ -0,0 +1,2872 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_SECURITY_C_ + +#include +#include + +static const char *_security_type_str[] = { + "N/A", + "WEP40", + "TKIP", + "TKIP_WM", + "AES", + "WEP104", + "SMS4", + "GCMP", +}; + +static const char *_security_type_bip_str[] = { + "BIP_CMAC_128", + "BIP_GMAC_128", + "BIP_GMAC_256", + "BIP_CMAC_256", +}; + +const char *security_type_str(u8 value) +{ +#ifdef CONFIG_IEEE80211W + if ((_BIP_MAX_ > value) && (value >= _BIP_CMAC_128_)) + return _security_type_bip_str[value & ~_SEC_TYPE_BIT_]; +#endif + + if (_CCMP_256_ == value) + return "CCMP_256"; + if (_GCMP_256_ == value) + return "GCMP_256"; + + if (_SEC_TYPE_MAX_ > value) + return _security_type_str[value]; + + return NULL; +} + +#ifdef CONFIG_IEEE80211W +u32 security_type_bip_to_gmcs(enum security_type type) +{ + switch (type) { + case _BIP_CMAC_128_: + return WPA_CIPHER_BIP_CMAC_128; + case _BIP_GMAC_128_: + return WPA_CIPHER_BIP_GMAC_128; + case _BIP_GMAC_256_: + return WPA_CIPHER_BIP_GMAC_256; + case _BIP_CMAC_256_: + return WPA_CIPHER_BIP_CMAC_256; + default: + return 0; + } +} +#endif + +#ifdef DBG_SW_SEC_CNT +#define WEP_SW_ENC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->wep_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->wep_sw_enc_cnt_mc++; \ + else \ + sec->wep_sw_enc_cnt_uc++; \ + } while (0) + +#define WEP_SW_DEC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->wep_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->wep_sw_dec_cnt_mc++; \ + else \ + sec->wep_sw_dec_cnt_uc++; \ + } while (0) + +#define TKIP_SW_ENC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->tkip_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->tkip_sw_enc_cnt_mc++; \ + else \ + sec->tkip_sw_enc_cnt_uc++; \ + } while (0) + +#define TKIP_SW_DEC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->tkip_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->tkip_sw_dec_cnt_mc++; \ + else \ + sec->tkip_sw_dec_cnt_uc++; \ + } while (0) + +#define AES_SW_ENC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->aes_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->aes_sw_enc_cnt_mc++; \ + else \ + sec->aes_sw_enc_cnt_uc++; \ + } while (0) + +#define AES_SW_DEC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->aes_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->aes_sw_dec_cnt_mc++; \ + else \ + sec->aes_sw_dec_cnt_uc++; \ + } while (0) + +#define GCMP_SW_ENC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->gcmp_sw_enc_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->gcmp_sw_enc_cnt_mc++; \ + else \ + sec->gcmp_sw_enc_cnt_uc++; \ + } while (0) + +#define GCMP_SW_DEC_CNT_INC(sec, ra) do {\ + if (is_broadcast_mac_addr(ra)) \ + sec->gcmp_sw_dec_cnt_bc++; \ + else if (is_multicast_mac_addr(ra)) \ + sec->gcmp_sw_dec_cnt_mc++; \ + else \ + sec->gcmp_sw_dec_cnt_uc++; \ + } while (0) +#else +#define WEP_SW_ENC_CNT_INC(sec, ra) +#define WEP_SW_DEC_CNT_INC(sec, ra) +#define TKIP_SW_ENC_CNT_INC(sec, ra) +#define TKIP_SW_DEC_CNT_INC(sec, ra) +#define AES_SW_ENC_CNT_INC(sec, ra) +#define AES_SW_DEC_CNT_INC(sec, ra) +#define GCMP_SW_ENC_CNT_INC(sec, ra) +#define GCMP_SW_DEC_CNT_INC(sec, ra) +#endif /* DBG_SW_SEC_CNT */ + +/* *****WEP related***** */ + +#define CRC32_POLY 0x04c11db7 + +struct arc4context { + u32 x; + u32 y; + u8 state[256]; +}; + + +static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len) +{ + u32 t, u; + u32 keyindex; + u32 stateindex; + u8 *state; + u32 counter; + state = parc4ctx->state; + parc4ctx->x = 0; + parc4ctx->y = 0; + for (counter = 0; counter < 256; counter++) + state[counter] = (u8)counter; + keyindex = 0; + stateindex = 0; + for (counter = 0; counter < 256; counter++) { + t = state[counter]; + stateindex = (stateindex + key[keyindex] + t) & 0xff; + u = state[stateindex]; + state[stateindex] = (u8)t; + state[counter] = (u8)u; + if (++keyindex >= key_len) + keyindex = 0; + } +} +static u32 arcfour_byte(struct arc4context *parc4ctx) +{ + u32 x; + u32 y; + u32 sx, sy; + u8 *state; + state = parc4ctx->state; + x = (parc4ctx->x + 1) & 0xff; + sx = state[x]; + y = (sx + parc4ctx->y) & 0xff; + sy = state[y]; + parc4ctx->x = x; + parc4ctx->y = y; + state[y] = (u8)sx; + state[x] = (u8)sy; + return state[(sx + sy) & 0xff]; +} + + +static void arcfour_encrypt(struct arc4context *parc4ctx, + u8 *dest, + u8 *src, + u32 len) +{ + u32 i; + for (i = 0; i < len; i++) + dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx); +} + +static sint bcrc32initialized = 0; +static u32 crc32_table[256]; + + +static u8 crc32_reverseBit(u8 data) +{ + return (u8)((data << 7) & 0x80) | ((data << 5) & 0x40) | ((data << 3) & 0x20) | ((data << 1) & 0x10) | ((data >> 1) & 0x08) | ((data >> 3) & 0x04) | ((data >> 5) & 0x02) | (( + data >> 7) & 0x01) ; +} + +static void crc32_init(void) +{ + if (bcrc32initialized == 1) + goto exit; + else { + sint i, j; + u32 c; + u8 *p = (u8 *)&c, *p1; + u8 k; + + c = 0x12340000; + + for (i = 0; i < 256; ++i) { + k = crc32_reverseBit((u8)i); + for (c = ((u32)k) << 24, j = 8; j > 0; --j) + c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1); + p1 = (u8 *)&crc32_table[i]; + + p1[0] = crc32_reverseBit(p[3]); + p1[1] = crc32_reverseBit(p[2]); + p1[2] = crc32_reverseBit(p[1]); + p1[3] = crc32_reverseBit(p[0]); + } + bcrc32initialized = 1; + } +exit: + return; +} + +static u32 getcrc32(u8 *buf, sint len) +{ + u8 *p; + u32 crc; + if (bcrc32initialized == 0) + crc32_init(); + + crc = 0xffffffff; /* preload shift register, per CRC-32 spec */ + + for (p = buf; len > 0; ++p, --len) + crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8); + return ~crc; /* transmit complement, per CRC-32 spec */ +} + + +/* + Need to consider the fragment situation +*/ +void rtw_wep_encrypt(_adapter *padapter, u8 *pxmitframe) +{ + /* exclude ICV */ + + unsigned char crc[4]; + struct arc4context mycontext; + + sint curfragnum, length; + u32 keylength; + + u8 *pframe, *payload, *iv; /* ,*wepkey */ + u8 wepkey[16]; + u8 hw_hdr_offset = 0; + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + + (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* start to encrypt each fragment */ + if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) { + keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex]; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + iv = pframe + pattrib->hdrlen; + _rtw_memcpy(&wepkey[0], iv, 3); + _rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); + payload = pframe + pattrib->iv_len + pattrib->hdrlen; + + if ((curfragnum + 1) == pattrib->nr_frags) { + /* the last fragment */ + + length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + + *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length)); + + arcfour_init(&mycontext, wepkey, 3 + keylength); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload + length, crc, 4); + + } else { + length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len ; + *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length)); + arcfour_init(&mycontext, wepkey, 3 + keylength); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload + length, crc, 4); + + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + + } + + } + + WEP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } + + +} + +void rtw_wep_decrypt(_adapter *padapter, u8 *precvframe) +{ + /* exclude ICV */ + u8 crc[4]; + struct arc4context mycontext; + sint length; + u32 keylength; + u8 *pframe, *payload, *iv, wepkey[16]; + u8 keyindex; + struct rx_pkt_attrib *prxattrib = &(((union recv_frame *)precvframe)->u.hdr.attrib); + struct security_priv *psecuritypriv = &padapter->securitypriv; + + + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + + /* start to decrypt recvframe */ + if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) { + iv = pframe + prxattrib->hdrlen; + /* keyindex=(iv[3]&0x3); */ + keyindex = prxattrib->key_index; + keylength = psecuritypriv->dot11DefKeylen[keyindex]; + _rtw_memcpy(&wepkey[0], iv, 3); + /* _rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0],keylength); */ + _rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength); + length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len; + + payload = pframe + prxattrib->iv_len + prxattrib->hdrlen; + + /* decrypt payload include icv */ + arcfour_init(&mycontext, wepkey, 3 + keylength); + arcfour_encrypt(&mycontext, payload, payload, length); + + /* calculate icv and compare the icv */ + *((u32 *)crc) = le32_to_cpu(getcrc32(payload, length - 4)); + + + WEP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } + + + return; + +} + +/* 3 =====TKIP related===== */ + +static u32 secmicgetuint32(u8 *p) +/* Convert from Byte[] to Us4Byte32 in a portable way */ +{ + s32 i; + u32 res = 0; + for (i = 0; i < 4; i++) + res |= ((u32)(*p++)) << (8 * i); + return res; +} + +static void secmicputuint32(u8 *p, u32 val) +/* Convert from Us4Byte32 to Byte[] in a portable way */ +{ + long i; + for (i = 0; i < 4; i++) { + *p++ = (u8)(val & 0xff); + val >>= 8; + } +} + +static void secmicclear(struct mic_data *pmicdata) +{ + /* Reset the state to the empty message. */ + pmicdata->L = pmicdata->K0; + pmicdata->R = pmicdata->K1; + pmicdata->nBytesInM = 0; + pmicdata->M = 0; +} + +void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key) +{ + /* Set the key */ + pmicdata->K0 = secmicgetuint32(key); + pmicdata->K1 = secmicgetuint32(key + 4); + /* and reset the message */ + secmicclear(pmicdata); +} + +void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b) +{ + /* Append the byte to our word-sized buffer */ + pmicdata->M |= ((unsigned long)b) << (8 * pmicdata->nBytesInM); + pmicdata->nBytesInM++; + /* Process the word if it is full. */ + if (pmicdata->nBytesInM >= 4) { + pmicdata->L ^= pmicdata->M; + pmicdata->R ^= ROL32(pmicdata->L, 17); + pmicdata->L += pmicdata->R; + pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8); + pmicdata->L += pmicdata->R; + pmicdata->R ^= ROL32(pmicdata->L, 3); + pmicdata->L += pmicdata->R; + pmicdata->R ^= ROR32(pmicdata->L, 2); + pmicdata->L += pmicdata->R; + /* Clear the buffer */ + pmicdata->M = 0; + pmicdata->nBytesInM = 0; + } +} + +void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes) +{ + /* This is simple */ + while (nbytes > 0) { + rtw_secmicappendbyte(pmicdata, *src++); + nbytes--; + } +} + +void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst) +{ + /* Append the minimum padding */ + rtw_secmicappendbyte(pmicdata, 0x5a); + rtw_secmicappendbyte(pmicdata, 0); + rtw_secmicappendbyte(pmicdata, 0); + rtw_secmicappendbyte(pmicdata, 0); + rtw_secmicappendbyte(pmicdata, 0); + /* and then zeroes until the length is a multiple of 4 */ + while (pmicdata->nBytesInM != 0) + rtw_secmicappendbyte(pmicdata, 0); + /* The appendByte function has already computed the result. */ + secmicputuint32(dst, pmicdata->L); + secmicputuint32(dst + 4, pmicdata->R); + /* Reset to the empty message. */ + secmicclear(pmicdata); +} + + +void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri) +{ + + struct mic_data micdata; + u8 priority[4] = {0x0, 0x0, 0x0, 0x0}; + rtw_secmicsetkey(&micdata, key); + priority[0] = pri; + + /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */ + if (header[1] & 1) { /* ToDS==1 */ + rtw_secmicappend(&micdata, &header[16], 6); /* DA */ + if (header[1] & 2) /* From Ds==1 */ + rtw_secmicappend(&micdata, &header[24], 6); + else + rtw_secmicappend(&micdata, &header[10], 6); + } else { /* ToDS==0 */ + rtw_secmicappend(&micdata, &header[4], 6); /* DA */ + if (header[1] & 2) /* From Ds==1 */ + rtw_secmicappend(&micdata, &header[16], 6); + else + rtw_secmicappend(&micdata, &header[10], 6); + + } + rtw_secmicappend(&micdata, &priority[0], 4); + + + rtw_secmicappend(&micdata, data, data_len); + + rtw_secgetmic(&micdata, mic_code); +} + + + + +/* macros for extraction/creation of unsigned char/unsigned short values */ +#define RotR1(v16) ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15)) +#define Lo8(v16) ((u8)((v16) & 0x00FF)) +#define Hi8(v16) ((u8)(((v16) >> 8) & 0x00FF)) +#define Lo16(v32) ((u16)((v32) & 0xFFFF)) +#define Hi16(v32) ((u16)(((v32) >> 16) & 0xFFFF)) +#define Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8)) + +/* select the Nth 16-bit word of the temporal key unsigned char array TK[] */ +#define TK16(N) Mk16(tk[2*(N)+1], tk[2*(N)]) + +/* S-box lookup: 16 bits --> 16 bits */ +#define _S_(v16) (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)]) + +/* fixed algorithm "parameters" */ +#define PHASE1_LOOP_CNT 8 /* this needs to be "big enough" */ +#define TA_SIZE 6 /* 48-bit transmitter address */ +#define TK_SIZE 16 /* 128-bit temporal key */ +#define P1K_SIZE 10 /* 80-bit Phase1 key */ +#define RC4_KEY_SIZE 16 /* 128-bit RC4KEY (104 bits unknown) */ + + +/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */ +static const unsigned short Sbox1[2][256] = /* Sbox for hash (can be in ROM) */ +{ { + 0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154, + 0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A, + 0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B, + 0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B, + 0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F, + 0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F, + 0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5, + 0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F, + 0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB, + 0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397, + 0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED, + 0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A, + 0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194, + 0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3, + 0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104, + 0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D, + 0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39, + 0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695, + 0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83, + 0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76, + 0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4, + 0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B, + 0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0, + 0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018, + 0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751, + 0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85, + 0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12, + 0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9, + 0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7, + 0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A, + 0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8, + 0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A, + }, + + + { /* second half of table is unsigned char-reversed version of first! */ + 0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491, + 0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC, + 0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB, + 0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B, + 0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83, + 0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A, + 0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F, + 0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA, + 0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B, + 0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713, + 0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6, + 0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85, + 0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411, + 0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B, + 0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1, + 0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF, + 0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E, + 0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6, + 0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B, + 0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD, + 0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8, + 0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2, + 0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049, + 0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810, + 0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197, + 0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F, + 0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C, + 0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927, + 0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733, + 0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5, + 0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0, + 0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C, + } +}; + +/* +********************************************************************** +* Routine: Phase 1 -- generate P1K, given TA, TK, IV32 +* +* Inputs: +* tk[] = temporal key [128 bits] +* ta[] = transmitter's MAC address [ 48 bits] +* iv32 = upper 32 bits of IV [ 32 bits] +* Output: +* p1k[] = Phase 1 key [ 80 bits] +* +* Note: +* This function only needs to be called every 2**16 packets, +* although in theory it could be called every packet. +* +********************************************************************** +*/ +static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32) +{ + sint i; + /* Initialize the 80 bits of P1K[] from IV32 and TA[0..5] */ + p1k[0] = Lo16(iv32); + p1k[1] = Hi16(iv32); + p1k[2] = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */ + p1k[3] = Mk16(ta[3], ta[2]); + p1k[4] = Mk16(ta[5], ta[4]); + + /* Now compute an unbalanced Feistel cipher with 80-bit block */ + /* size on the 80-bit block P1K[], using the 128-bit key TK[] */ + for (i = 0; i < PHASE1_LOOP_CNT ; i++) { + /* Each add operation here is mod 2**16 */ + p1k[0] += _S_(p1k[4] ^ TK16((i & 1) + 0)); + p1k[1] += _S_(p1k[0] ^ TK16((i & 1) + 2)); + p1k[2] += _S_(p1k[1] ^ TK16((i & 1) + 4)); + p1k[3] += _S_(p1k[2] ^ TK16((i & 1) + 6)); + p1k[4] += _S_(p1k[3] ^ TK16((i & 1) + 0)); + p1k[4] += (unsigned short)i; /* avoid "slide attacks" */ + } +} + + +/* +********************************************************************** +* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16 +* +* Inputs: +* tk[] = Temporal key [128 bits] +* p1k[] = Phase 1 output key [ 80 bits] +* iv16 = low 16 bits of IV counter [ 16 bits] +* Output: +* rc4key[] = the key used to encrypt the packet [128 bits] +* +* Note: +* The value {TA,IV32,IV16} for Phase1/Phase2 must be unique +* across all packets using the same key TK value. Then, for a +* given value of TK[], this TKIP48 construction guarantees that +* the final RC4KEY value is unique across all packets. +* +* Suggested implementation optimization: if PPK[] is "overlaid" +* appropriately on RC4KEY[], there is no need for the final +* for loop below that copies the PPK[] result into RC4KEY[]. +* +********************************************************************** +*/ +static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16) +{ + sint i; + u16 PPK[6]; /* temporary key for mixing */ + /* Note: all adds in the PPK[] equations below are mod 2**16 */ + for (i = 0; i < 5; i++) + PPK[i] = p1k[i]; /* first, copy P1K to PPK */ + PPK[5] = p1k[4] + iv16; /* next, add in IV16 */ + + /* Bijective non-linear mixing of the 96 bits of PPK[0..5] */ + PPK[0] += _S_(PPK[5] ^ TK16(0)); /* Mix key in each "round" */ + PPK[1] += _S_(PPK[0] ^ TK16(1)); + PPK[2] += _S_(PPK[1] ^ TK16(2)); + PPK[3] += _S_(PPK[2] ^ TK16(3)); + PPK[4] += _S_(PPK[3] ^ TK16(4)); + PPK[5] += _S_(PPK[4] ^ TK16(5)); /* Total # S-box lookups == 6 */ + + /* Final sweep: bijective, "linear". Rotates kill LSB correlations */ + PPK[0] += RotR1(PPK[5] ^ TK16(6)); + PPK[1] += RotR1(PPK[0] ^ TK16(7)); /* Use all of TK[] in Phase2 */ + PPK[2] += RotR1(PPK[1]); + PPK[3] += RotR1(PPK[2]); + PPK[4] += RotR1(PPK[3]); + PPK[5] += RotR1(PPK[4]); + /* Note: At this point, for a given key TK[0..15], the 96-bit output */ + /* value PPK[0..5] is guaranteed to be unique, as a function */ + /* of the 96-bit "input" value {TA,IV32,IV16}. That is, P1K */ + /* is now a keyed permutation of {TA,IV32,IV16}. */ + + /* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key */ + rc4key[0] = Hi8(iv16); /* RC4KEY[0..2] is the WEP IV */ + rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys */ + rc4key[2] = Lo8(iv16); + rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1); + + + /* Copy 96 bits of PPK[0..5] to RC4KEY[4..15] (little-endian) */ + for (i = 0; i < 6; i++) { + rc4key[4 + 2 * i] = Lo8(PPK[i]); + rc4key[5 + 2 * i] = Hi8(PPK[i]); + } +} + + +/* The hlen isn't include the IV */ +u32 rtw_tkip_encrypt(_adapter *padapter, u8 *pxmitframe) +{ + /* exclude ICV */ + u16 pnl; + u32 pnh; + u8 rc4key[16]; + u8 ttkey[16]; + u8 crc[4]; + u8 hw_hdr_offset = 0; + struct arc4context mycontext; + sint curfragnum, length; + u32 prwskeylen; + + u8 *pframe, *payload, *iv, *prwskey; + union pn48 dot11txpn; + /* struct sta_info *stainfo; */ + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + u32 res = _SUCCESS; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + + (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + /* 4 start to encrypt each fragment */ + if (pattrib->encrypt == _TKIP_) { + + /* + if(pattrib->psta) + { + stainfo = pattrib->psta; + } + else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] ); + } + */ + /* if (stainfo!=NULL) */ + { + /* + if(!(stainfo->state &WIFI_ASOC_STATE)) + { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, stainfo->state); + return _FAIL; + } + */ + + if (IS_MCAST(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else { + /* prwskey=&stainfo->dot118021x_UncstKey.skey[0]; */ + prwskey = pattrib->dot118021x_UncstKey.skey; + } + + prwskeylen = 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + iv = pframe + pattrib->hdrlen; + payload = pframe + pattrib->iv_len + pattrib->hdrlen; + + GET_TKIP_PN(iv, dot11txpn); + + pnl = (u16)(dot11txpn.val); + pnh = (u32)(dot11txpn.val >> 16); + + phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh); + + phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl); + + if ((curfragnum + 1) == pattrib->nr_frags) { /* 4 the last fragment */ + length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length)); /* modified by Amy*/ + + arcfour_init(&mycontext, rc4key, 16); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload + length, crc, 4); + + } else { + length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len ; + *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length)); /* modified by Amy*/ + arcfour_init(&mycontext, rc4key, 16); + arcfour_encrypt(&mycontext, payload, payload, length); + arcfour_encrypt(&mycontext, payload + length, crc, 4); + + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + + } + } + + TKIP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } + /* + else{ + RTW_INFO("%s, psta==NUL\n", __func__); + res=_FAIL; + } + */ + + } + return res; + +} + + +/* The hlen isn't include the IV */ +u32 rtw_tkip_decrypt(_adapter *padapter, u8 *precvframe) +{ + /* exclude ICV */ + u16 pnl; + u32 pnh; + u8 rc4key[16]; + u8 ttkey[16]; + u8 crc[4]; + struct arc4context mycontext; + sint length; + u32 prwskeylen; + + u8 *pframe, *payload, *iv, *prwskey; + union pn48 dot11txpn; + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + /* struct recv_priv *precvpriv=&padapter->recvpriv; */ + u32 res = _SUCCESS; + + + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + + /* 4 start to decrypt recvframe */ + if (prxattrib->encrypt == _TKIP_) { + + stainfo = rtw_get_stainfo(&padapter->stapriv , &prxattrib->ta[0]); + if (stainfo != NULL) { + + if (IS_MCAST(prxattrib->ra)) { + static systime start = 0; + static u32 no_gkey_bc_cnt = 0; + static u32 no_gkey_mc_cnt = 0; + + if (psecuritypriv->binstallGrpkey == _FALSE) { + res = _FAIL; + + if (start == 0) + start = rtw_get_current_time(); + + if (is_broadcast_mac_addr(prxattrib->ra)) + no_gkey_bc_cnt++; + else + no_gkey_mc_cnt++; + + if (rtw_get_passing_time_ms(start) > 1000) { + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = rtw_get_current_time(); + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + } + goto exit; + } + + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = 0; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + + /* RTW_INFO("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n"); */ + /* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */ + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + prwskeylen = 16; + } else { + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + prwskeylen = 16; + } + + iv = pframe + prxattrib->hdrlen; + payload = pframe + prxattrib->iv_len + prxattrib->hdrlen; + length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len; + + GET_TKIP_PN(iv, dot11txpn); + + pnl = (u16)(dot11txpn.val); + pnh = (u32)(dot11txpn.val >> 16); + + phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh); + phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl); + + /* 4 decrypt payload include icv */ + + arcfour_init(&mycontext, rc4key, 16); + arcfour_encrypt(&mycontext, payload, payload, length); + + *((u32 *)crc) = le32_to_cpu(getcrc32(payload, length - 4)); + + if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] || crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) { + res = _FAIL; + } + + TKIP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } else { + res = _FAIL; + } + + } +exit: + return res; + +} + + +/* 3 =====AES related===== */ +#if (NEW_CRYPTO == 0) + +#define MAX_MSG_SIZE 2048 +/*****************************/ +/******** SBOX Table *********/ +/*****************************/ + +static u8 sbox_table[256] = { + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, + 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, + 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, + 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, + 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, + 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, + 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, + 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, + 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, + 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, + 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, + 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, + 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, + 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, + 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, + 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, + 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16 +}; + +/*****************************/ +/**** Function Prototypes ****/ +/*****************************/ + +static void bitwise_xor(u8 *ina, u8 *inb, u8 *out); +static void construct_mic_iv( + u8 *mic_header1, + sint qc_exists, + sint a4_exists, + u8 *mpdu, + uint payload_length, + u8 *pn_vector, + uint frtype);/* add for CONFIG_IEEE80211W, none 11w also can use */ +static void construct_mic_header1( + u8 *mic_header1, + sint header_length, + u8 *mpdu, + uint frtype);/* add for CONFIG_IEEE80211W, none 11w also can use */ +static void construct_mic_header2( + u8 *mic_header2, + u8 *mpdu, + sint a4_exists, + sint qc_exists); +static void construct_ctr_preload( + u8 *ctr_preload, + sint a4_exists, + sint qc_exists, + u8 *mpdu, + u8 *pn_vector, + sint c, + uint frtype);/* add for CONFIG_IEEE80211W, none 11w also can use */ +static void xor_128(u8 *a, u8 *b, u8 *out); +static void xor_32(u8 *a, u8 *b, u8 *out); +static u8 sbox(u8 a); +static void next_key(u8 *key, sint round); +static void byte_sub(u8 *in, u8 *out); +static void shift_row(u8 *in, u8 *out); +static void mix_column(u8 *in, u8 *out); +static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext); + + +/****************************************/ +/* aes128k128d() */ +/* Performs a 128 bit AES encrypt with */ +/* 128 bit data. */ +/****************************************/ +static void xor_128(u8 *a, u8 *b, u8 *out) +{ + sint i; + for (i = 0; i < 16; i++) + out[i] = a[i] ^ b[i]; +} + + +static void xor_32(u8 *a, u8 *b, u8 *out) +{ + sint i; + for (i = 0; i < 4; i++) + out[i] = a[i] ^ b[i]; +} + + +static u8 sbox(u8 a) +{ + return sbox_table[(sint)a]; +} + + +static void next_key(u8 *key, sint round) +{ + u8 rcon; + u8 sbox_key[4]; + u8 rcon_table[12] = { + 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, + 0x1b, 0x36, 0x36, 0x36 + }; + sbox_key[0] = sbox(key[13]); + sbox_key[1] = sbox(key[14]); + sbox_key[2] = sbox(key[15]); + sbox_key[3] = sbox(key[12]); + + rcon = rcon_table[round]; + + xor_32(&key[0], sbox_key, &key[0]); + key[0] = key[0] ^ rcon; + + xor_32(&key[4], &key[0], &key[4]); + xor_32(&key[8], &key[4], &key[8]); + xor_32(&key[12], &key[8], &key[12]); +} + + +static void byte_sub(u8 *in, u8 *out) +{ + sint i; + for (i = 0; i < 16; i++) + out[i] = sbox(in[i]); +} + + +static void shift_row(u8 *in, u8 *out) +{ + out[0] = in[0]; + out[1] = in[5]; + out[2] = in[10]; + out[3] = in[15]; + out[4] = in[4]; + out[5] = in[9]; + out[6] = in[14]; + out[7] = in[3]; + out[8] = in[8]; + out[9] = in[13]; + out[10] = in[2]; + out[11] = in[7]; + out[12] = in[12]; + out[13] = in[1]; + out[14] = in[6]; + out[15] = in[11]; +} + + +static void mix_column(u8 *in, u8 *out) +{ + sint i; + u8 add1b[4]; + u8 add1bf7[4]; + u8 rotl[4]; + u8 swap_halfs[4]; + u8 andf7[4]; + u8 rotr[4]; + u8 temp[4]; + u8 tempb[4]; + for (i = 0 ; i < 4; i++) { + if ((in[i] & 0x80) == 0x80) + add1b[i] = 0x1b; + else + add1b[i] = 0x00; + } + + swap_halfs[0] = in[2]; /* Swap halfs */ + swap_halfs[1] = in[3]; + swap_halfs[2] = in[0]; + swap_halfs[3] = in[1]; + + rotl[0] = in[3]; /* Rotate left 8 bits */ + rotl[1] = in[0]; + rotl[2] = in[1]; + rotl[3] = in[2]; + + andf7[0] = in[0] & 0x7f; + andf7[1] = in[1] & 0x7f; + andf7[2] = in[2] & 0x7f; + andf7[3] = in[3] & 0x7f; + + for (i = 3; i > 0; i--) { /* logical shift left 1 bit */ + andf7[i] = andf7[i] << 1; + if ((andf7[i - 1] & 0x80) == 0x80) + andf7[i] = (andf7[i] | 0x01); + } + andf7[0] = andf7[0] << 1; + andf7[0] = andf7[0] & 0xfe; + + xor_32(add1b, andf7, add1bf7); + + xor_32(in, add1bf7, rotr); + + temp[0] = rotr[0]; /* Rotate right 8 bits */ + rotr[0] = rotr[1]; + rotr[1] = rotr[2]; + rotr[2] = rotr[3]; + rotr[3] = temp[0]; + + xor_32(add1bf7, rotr, temp); + xor_32(swap_halfs, rotl, tempb); + xor_32(temp, tempb, out); +} + + +static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext) +{ + sint round; + sint i; + u8 intermediatea[16]; + u8 intermediateb[16]; + u8 round_key[16]; + for (i = 0; i < 16; i++) + round_key[i] = key[i]; + + for (round = 0; round < 11; round++) { + if (round == 0) { + xor_128(round_key, data, ciphertext); + next_key(round_key, round); + } else if (round == 10) { + byte_sub(ciphertext, intermediatea); + shift_row(intermediatea, intermediateb); + xor_128(intermediateb, round_key, ciphertext); + } else { /* 1 - 9 */ + byte_sub(ciphertext, intermediatea); + shift_row(intermediatea, intermediateb); + mix_column(&intermediateb[0], &intermediatea[0]); + mix_column(&intermediateb[4], &intermediatea[4]); + mix_column(&intermediateb[8], &intermediatea[8]); + mix_column(&intermediateb[12], &intermediatea[12]); + xor_128(intermediatea, round_key, ciphertext); + next_key(round_key, round); + } + } +} + + +/************************************************/ +/* construct_mic_iv() */ +/* Builds the MIC IV from header fields and PN */ +/* Baron think the function is construct CCM */ +/* nonce */ +/************************************************/ +static void construct_mic_iv( + u8 *mic_iv, + sint qc_exists, + sint a4_exists, + u8 *mpdu, + uint payload_length, + u8 *pn_vector, + uint frtype/* add for CONFIG_IEEE80211W, none 11w also can use */ +) +{ + sint i; + mic_iv[0] = 0x59; + if (qc_exists && a4_exists) + mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */ + if (qc_exists && !a4_exists) + mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */ + if (!qc_exists) + mic_iv[1] = 0x00; +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) + /* 802.11w management frame should set management bit(4) */ + if (frtype == WIFI_MGT_TYPE) + mic_iv[1] |= BIT(4); +#endif + for (i = 2; i < 8; i++) + mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */ +#ifdef CONSISTENT_PN_ORDER + for (i = 8; i < 14; i++) + mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */ +#else + for (i = 8; i < 14; i++) + mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */ +#endif + mic_iv[14] = (unsigned char)(payload_length / 256); + mic_iv[15] = (unsigned char)(payload_length % 256); +} + + +/************************************************/ +/* construct_mic_header1() */ +/* Builds the first MIC header block from */ +/* header fields. */ +/* Build AAD SC,A1,A2 */ +/************************************************/ +static void construct_mic_header1( + u8 *mic_header1, + sint header_length, + u8 *mpdu, + uint frtype/* add for CONFIG_IEEE80211W, none 11w also can use */ +) +{ + mic_header1[0] = (u8)((header_length - 2) / 256); + mic_header1[1] = (u8)((header_length - 2) % 256); +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) + /* 802.11w management frame don't AND subtype bits 4,5,6 of frame control field */ + if (frtype == WIFI_MGT_TYPE) + mic_header1[2] = mpdu[0]; + else +#endif + mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */ + + mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */ + mic_header1[4] = mpdu[4]; /* A1 */ + mic_header1[5] = mpdu[5]; + mic_header1[6] = mpdu[6]; + mic_header1[7] = mpdu[7]; + mic_header1[8] = mpdu[8]; + mic_header1[9] = mpdu[9]; + mic_header1[10] = mpdu[10]; /* A2 */ + mic_header1[11] = mpdu[11]; + mic_header1[12] = mpdu[12]; + mic_header1[13] = mpdu[13]; + mic_header1[14] = mpdu[14]; + mic_header1[15] = mpdu[15]; +} + + +/************************************************/ +/* construct_mic_header2() */ +/* Builds the last MIC header block from */ +/* header fields. */ +/************************************************/ +static void construct_mic_header2( + u8 *mic_header2, + u8 *mpdu, + sint a4_exists, + sint qc_exists +) +{ + sint i; + for (i = 0; i < 16; i++) + mic_header2[i] = 0x00; + + mic_header2[0] = mpdu[16]; /* A3 */ + mic_header2[1] = mpdu[17]; + mic_header2[2] = mpdu[18]; + mic_header2[3] = mpdu[19]; + mic_header2[4] = mpdu[20]; + mic_header2[5] = mpdu[21]; + + /* mic_header2[6] = mpdu[22] & 0xf0; SC */ + mic_header2[6] = 0x00; + mic_header2[7] = 0x00; /* mpdu[23]; */ + + + if (!qc_exists && a4_exists) { + for (i = 0; i < 6; i++) + mic_header2[8 + i] = mpdu[24 + i]; /* A4 */ + + } + + if (qc_exists && !a4_exists) { + mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */ + mic_header2[9] = mpdu[25] & 0x00; + } + + if (qc_exists && a4_exists) { + for (i = 0; i < 6; i++) + mic_header2[8 + i] = mpdu[24 + i]; /* A4 */ + + mic_header2[14] = mpdu[30] & 0x0f; + mic_header2[15] = mpdu[31] & 0x00; + } + +} + + +/************************************************/ +/* construct_mic_header2() */ +/* Builds the last MIC header block from */ +/* header fields. */ +/* Baron think the function is construct CCM */ +/* nonce */ +/************************************************/ +static void construct_ctr_preload( + u8 *ctr_preload, + sint a4_exists, + sint qc_exists, + u8 *mpdu, + u8 *pn_vector, + sint c, + uint frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ +) +{ + sint i = 0; + for (i = 0; i < 16; i++) + ctr_preload[i] = 0x00; + i = 0; + + ctr_preload[0] = 0x01; /* flag */ + if (qc_exists && a4_exists) + ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */ + if (qc_exists && !a4_exists) + ctr_preload[1] = mpdu[24] & 0x0f; +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) + /* 802.11w management frame should set management bit(4) */ + if (frtype == WIFI_MGT_TYPE) + ctr_preload[1] |= BIT(4); +#endif + for (i = 2; i < 8; i++) + ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */ +#ifdef CONSISTENT_PN_ORDER + for (i = 8; i < 14; i++) + ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */ +#else + for (i = 8; i < 14; i++) + ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */ +#endif + ctr_preload[14] = (unsigned char)(c / 256); /* Ctr */ + ctr_preload[15] = (unsigned char)(c % 256); +} + + +/************************************/ +/* bitwise_xor() */ +/* A 128 bit, bitwise exclusive or */ +/************************************/ +static void bitwise_xor(u8 *ina, u8 *inb, u8 *out) +{ + sint i; + for (i = 0; i < 16; i++) + out[i] = ina[i] ^ inb[i]; +} + + +static sint aes_cipher(u8 *key, uint hdrlen, + u8 *pframe, uint plen) +{ + /* static unsigned char message[MAX_MSG_SIZE]; */ + uint qc_exists, a4_exists, i, j, payload_remainder, + num_blocks, payload_index; + + u8 pn_vector[6]; + u8 mic_iv[16]; + u8 mic_header1[16]; + u8 mic_header2[16]; + u8 ctr_preload[16]; + + /* Intermediate Buffers */ + u8 chain_buffer[16]; + u8 aes_out[16]; + u8 padded_buffer[16]; + u8 mic[8]; + /* uint offset = 0; */ + uint frtype = GetFrameType(pframe); + uint frsubtype = get_frame_sub_type(pframe); + + frsubtype = frsubtype >> 4; + + + _rtw_memset((void *)mic_iv, 0, 16); + _rtw_memset((void *)mic_header1, 0, 16); + _rtw_memset((void *)mic_header2, 0, 16); + _rtw_memset((void *)ctr_preload, 0, 16); + _rtw_memset((void *)chain_buffer, 0, 16); + _rtw_memset((void *)aes_out, 0, 16); + _rtw_memset((void *)padded_buffer, 0, 16); + + if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen == WLAN_HDR_A3_QOS_LEN)) + a4_exists = 0; + else + a4_exists = 1; + + if ( + ((frtype | frsubtype) == WIFI_DATA_CFACK) || + ((frtype | frsubtype) == WIFI_DATA_CFPOLL) || + ((frtype | frsubtype) == WIFI_DATA_CFACKPOLL)) { + qc_exists = 1; + if (hdrlen != WLAN_HDR_A3_QOS_LEN && hdrlen != WLAN_HDR_A4_QOS_LEN) + hdrlen += 2; + } + /* add for CONFIG_IEEE80211W, none 11w also can use */ + else if ((frtype == WIFI_DATA) && + ((frsubtype == 0x08) || + (frsubtype == 0x09) || + (frsubtype == 0x0a) || + (frsubtype == 0x0b))) { + if (hdrlen != WLAN_HDR_A3_QOS_LEN && hdrlen != WLAN_HDR_A4_QOS_LEN) + hdrlen += 2; + qc_exists = 1; + } else + qc_exists = 0; + + pn_vector[0] = pframe[hdrlen]; + pn_vector[1] = pframe[hdrlen + 1]; + pn_vector[2] = pframe[hdrlen + 4]; + pn_vector[3] = pframe[hdrlen + 5]; + pn_vector[4] = pframe[hdrlen + 6]; + pn_vector[5] = pframe[hdrlen + 7]; + + construct_mic_iv( + mic_iv, + qc_exists, + a4_exists, + pframe, /* message, */ + plen, + pn_vector, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + construct_mic_header1( + mic_header1, + hdrlen, + pframe, /* message */ + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + construct_mic_header2( + mic_header2, + pframe, /* message, */ + a4_exists, + qc_exists + ); + + + payload_remainder = plen % 16; + num_blocks = plen / 16; + + /* Find start of payload */ + payload_index = (hdrlen + 8); + + /* Calculate MIC */ + aes128k128d(key, mic_iv, aes_out); + bitwise_xor(aes_out, mic_header1, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + bitwise_xor(aes_out, mic_header2, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + + for (i = 0; i < num_blocks; i++) { + bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */ + + payload_index += 16; + aes128k128d(key, chain_buffer, aes_out); + } + + /* Add on the final payload block if it needs padding */ + if (payload_remainder > 0) { + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) { + padded_buffer[j] = pframe[payload_index++];/* padded_buffer[j] = message[payload_index++]; */ + } + bitwise_xor(aes_out, padded_buffer, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + + } + + for (j = 0 ; j < 8; j++) + mic[j] = aes_out[j]; + + /* Insert MIC into payload */ + for (j = 0; j < 8; j++) + pframe[payload_index + j] = mic[j]; /* message[payload_index+j] = mic[j]; */ + + payload_index = hdrlen + 8; + for (i = 0; i < num_blocks; i++) { + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + pframe, /* message, */ + pn_vector, + i + 1, + frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */ + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */ + for (j = 0; j < 16; j++) + pframe[payload_index++] = chain_buffer[j];/* for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j]; */ + } + + if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/ + /* encrypt it and copy the unpadded part back */ + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + pframe, /* message, */ + pn_vector, + num_blocks + 1, + frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) { + padded_buffer[j] = pframe[payload_index + j]; /* padded_buffer[j] = message[payload_index+j]; */ + } + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, padded_buffer, chain_buffer); + for (j = 0; j < payload_remainder; j++) + pframe[payload_index++] = chain_buffer[j];/* for (j=0; jattrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + sint curfragnum, plen; + u32 prwskeylen; + u8 *pframe; + u8 *prwskey; + u8 hw_hdr_offset = 0; + + u32 res = _SUCCESS; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + + (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* start to encrypt each fragment */ + if ((pattrib->encrypt == _AES_) || + (pattrib->encrypt == _CCMP_256_)) { + + if (IS_MCAST(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else { + prwskey = pattrib->dot118021x_UncstKey.skey; + } + +#ifdef CONFIG_TDLS + { + /* Swencryption */ + struct sta_info *ptdls_sta; + ptdls_sta = rtw_get_stainfo(&padapter->stapriv, &pattrib->dst[0]); + if ((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) { + RTW_INFO("[%s] for tdls link\n", __FUNCTION__); + prwskey = &ptdls_sta->tpk.tk[0]; + } + } +#endif /* CONFIG_TDLS */ + + prwskeylen = (pattrib->encrypt == _CCMP_256_) ? 32 : 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + + if ((curfragnum + 1) == pattrib->nr_frags) { /* the last fragment */ + plen = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + + _rtw_ccmp_encrypt(padapter, prwskey, prwskeylen, pattrib->hdrlen, pframe, plen); + } else { + plen = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + + _rtw_ccmp_encrypt(padapter, prwskey, prwskeylen, pattrib->hdrlen, pframe, plen); + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + + } + } + + AES_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + + } + + + + return res; +} +#else +u32 rtw_aes_encrypt(_adapter *padapter, u8 *pxmitframe) +{ + /* exclude ICV */ + + + /*static*/ + /* unsigned char message[MAX_MSG_SIZE]; */ + + /* Intermediate Buffers */ + sint curfragnum, length; + u32 prwskeylen; + u8 *pframe, *prwskey; /* , *payload,*iv */ + u8 hw_hdr_offset = 0; + /* struct sta_info *stainfo=NULL; */ + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + /* uint offset = 0; */ + u32 res = _SUCCESS; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + + (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* 4 start to encrypt each fragment */ + if ((pattrib->encrypt == _AES_)) { + /* + if(pattrib->psta) + { + stainfo = pattrib->psta; + } + else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] ); + } + */ + /* if (stainfo!=NULL) */ + { + /* + if(!(stainfo->state &WIFI_ASOC_STATE)) + { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, stainfo->state); + return _FAIL; + } + */ + + if (IS_MCAST(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else { + /* prwskey=&stainfo->dot118021x_UncstKey.skey[0]; */ + prwskey = pattrib->dot118021x_UncstKey.skey; + } + +#ifdef CONFIG_TDLS + { + /* Swencryption */ + struct sta_info *ptdls_sta; + ptdls_sta = rtw_get_stainfo(&padapter->stapriv , &pattrib->dst[0]); + if ((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) { + RTW_INFO("[%s] for tdls link\n", __FUNCTION__); + prwskey = &ptdls_sta->tpk.tk[0]; + } + } +#endif /* CONFIG_TDLS */ + + prwskeylen = 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + + if ((curfragnum + 1) == pattrib->nr_frags) { /* 4 the last fragment */ + length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + + aes_cipher(prwskey, pattrib->hdrlen, pframe, length); + } else { + length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len ; + + aes_cipher(prwskey, pattrib->hdrlen, pframe, length); + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + + } + } + + AES_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } + /* + else{ + RTW_INFO("%s, psta==NUL\n", __func__); + res=_FAIL; + } + */ + } + + + + return res; +} +#endif + +#if (NEW_CRYPTO == 0) +static sint aes_decipher(u8 *key, uint hdrlen, + u8 *pframe, uint plen) +{ + static u8 message[MAX_MSG_SIZE]; + uint qc_exists, a4_exists, i, j, payload_remainder, + num_blocks, payload_index; + sint res = _SUCCESS; + u8 pn_vector[6]; + u8 mic_iv[16]; + u8 mic_header1[16]; + u8 mic_header2[16]; + u8 ctr_preload[16]; + + /* Intermediate Buffers */ + u8 chain_buffer[16]; + u8 aes_out[16]; + u8 padded_buffer[16]; + u8 mic[8]; + + + /* uint offset = 0; */ + uint frtype = GetFrameType(pframe); + uint frsubtype = get_frame_sub_type(pframe); + frsubtype = frsubtype >> 4; + + + _rtw_memset((void *)mic_iv, 0, 16); + _rtw_memset((void *)mic_header1, 0, 16); + _rtw_memset((void *)mic_header2, 0, 16); + _rtw_memset((void *)ctr_preload, 0, 16); + _rtw_memset((void *)chain_buffer, 0, 16); + _rtw_memset((void *)aes_out, 0, 16); + _rtw_memset((void *)padded_buffer, 0, 16); + + /* start to decrypt the payload */ + + num_blocks = (plen - 8) / 16; /* (plen including LLC, payload_length and mic ) */ + + payload_remainder = (plen - 8) % 16; + + pn_vector[0] = pframe[hdrlen]; + pn_vector[1] = pframe[hdrlen + 1]; + pn_vector[2] = pframe[hdrlen + 4]; + pn_vector[3] = pframe[hdrlen + 5]; + pn_vector[4] = pframe[hdrlen + 6]; + pn_vector[5] = pframe[hdrlen + 7]; + + if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen == WLAN_HDR_A3_QOS_LEN)) + a4_exists = 0; + else + a4_exists = 1; + + if ( + ((frtype | frsubtype) == WIFI_DATA_CFACK) || + ((frtype | frsubtype) == WIFI_DATA_CFPOLL) || + ((frtype | frsubtype) == WIFI_DATA_CFACKPOLL)) { + qc_exists = 1; + if (hdrlen != WLAN_HDR_A3_QOS_LEN && hdrlen != WLAN_HDR_A4_QOS_LEN) + hdrlen += 2; + } /* only for data packet . add for CONFIG_IEEE80211W, none 11w also can use */ + else if ((frtype == WIFI_DATA) && + ((frsubtype == 0x08) || + (frsubtype == 0x09) || + (frsubtype == 0x0a) || + (frsubtype == 0x0b))) { + if (hdrlen != WLAN_HDR_A3_QOS_LEN && hdrlen != WLAN_HDR_A4_QOS_LEN) + hdrlen += 2; + qc_exists = 1; + } else + qc_exists = 0; + + + /* now, decrypt pframe with hdrlen offset and plen long */ + + payload_index = hdrlen + 8; /* 8 is for extiv */ + + for (i = 0; i < num_blocks; i++) { + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + pframe, + pn_vector, + i + 1, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, &pframe[payload_index], chain_buffer); + + for (j = 0; j < 16; j++) + pframe[payload_index++] = chain_buffer[j]; + } + + if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/ + /* encrypt it and copy the unpadded part back */ + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + pframe, + pn_vector, + num_blocks + 1, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) + padded_buffer[j] = pframe[payload_index + j]; + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, padded_buffer, chain_buffer); + for (j = 0; j < payload_remainder; j++) + pframe[payload_index++] = chain_buffer[j]; + } + + /* start to calculate the mic */ + if ((hdrlen + plen + 8) <= MAX_MSG_SIZE) + _rtw_memcpy((void *)message, pframe, (hdrlen + plen + 8)); /* 8 is for ext iv len */ + + + pn_vector[0] = pframe[hdrlen]; + pn_vector[1] = pframe[hdrlen + 1]; + pn_vector[2] = pframe[hdrlen + 4]; + pn_vector[3] = pframe[hdrlen + 5]; + pn_vector[4] = pframe[hdrlen + 6]; + pn_vector[5] = pframe[hdrlen + 7]; + + + + construct_mic_iv( + mic_iv, + qc_exists, + a4_exists, + message, + plen - 8, + pn_vector, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + + construct_mic_header1( + mic_header1, + hdrlen, + message, + frtype /* add for CONFIG_IEEE80211W, none 11w also can use */ + ); + construct_mic_header2( + mic_header2, + message, + a4_exists, + qc_exists + ); + + + payload_remainder = (plen - 8) % 16; + num_blocks = (plen - 8) / 16; + + /* Find start of payload */ + payload_index = (hdrlen + 8); + + /* Calculate MIC */ + aes128k128d(key, mic_iv, aes_out); + bitwise_xor(aes_out, mic_header1, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + bitwise_xor(aes_out, mic_header2, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + + for (i = 0; i < num_blocks; i++) { + bitwise_xor(aes_out, &message[payload_index], chain_buffer); + + payload_index += 16; + aes128k128d(key, chain_buffer, aes_out); + } + + /* Add on the final payload block if it needs padding */ + if (payload_remainder > 0) { + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) + padded_buffer[j] = message[payload_index++]; + bitwise_xor(aes_out, padded_buffer, chain_buffer); + aes128k128d(key, chain_buffer, aes_out); + + } + + for (j = 0 ; j < 8; j++) + mic[j] = aes_out[j]; + + /* Insert MIC into payload */ + for (j = 0; j < 8; j++) + message[payload_index + j] = mic[j]; + + payload_index = hdrlen + 8; + for (i = 0; i < num_blocks; i++) { + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + message, + pn_vector, + i + 1, + frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */ + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, &message[payload_index], chain_buffer); + for (j = 0; j < 16; j++) + message[payload_index++] = chain_buffer[j]; + } + + if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/ + /* encrypt it and copy the unpadded part back */ + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + message, + pn_vector, + num_blocks + 1, + frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < payload_remainder; j++) + padded_buffer[j] = message[payload_index + j]; + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, padded_buffer, chain_buffer); + for (j = 0; j < payload_remainder; j++) + message[payload_index++] = chain_buffer[j]; + } + + /* Encrypt the MIC */ + construct_ctr_preload( + ctr_preload, + a4_exists, + qc_exists, + message, + pn_vector, + 0, + frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */ + + for (j = 0; j < 16; j++) + padded_buffer[j] = 0x00; + for (j = 0; j < 8; j++) + padded_buffer[j] = message[j + hdrlen + 8 + plen - 8]; + + aes128k128d(key, ctr_preload, aes_out); + bitwise_xor(aes_out, padded_buffer, chain_buffer); + for (j = 0; j < 8; j++) + message[payload_index++] = chain_buffer[j]; + + /* compare the mic */ + for (i = 0; i < 8; i++) { + if (pframe[hdrlen + 8 + plen - 8 + i] != message[hdrlen + 8 + plen - 8 + i]) { + RTW_INFO("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n", + i, pframe[hdrlen + 8 + plen - 8 + i], message[hdrlen + 8 + plen - 8 + i]); + res = _FAIL; + } + } + return res; +} +#endif /* (NEW_CRYPTO == 0) */ + +#if NEW_CRYPTO +u32 rtw_aes_decrypt(_adapter *padapter, u8 *precvframe) +{ + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + u8 *pframe; + u8 *prwskey; + u32 res = _SUCCESS; + + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + /* start to encrypt each fragment */ + if ((prxattrib->encrypt == _AES_) || + (prxattrib->encrypt == _CCMP_256_)) { + + stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]); + if (stainfo != NULL) { + + if (IS_MCAST(prxattrib->ra)) { + static systime start = 0; + static u32 no_gkey_bc_cnt = 0; + static u32 no_gkey_mc_cnt = 0; + + if ((!MLME_IS_MESH(padapter) && psecuritypriv->binstallGrpkey == _FALSE) + #ifdef CONFIG_RTW_MESH + || !(stainfo->gtk_bmp | BIT(prxattrib->key_index)) + #endif + ) { + res = _FAIL; + + if (start == 0) + start = rtw_get_current_time(); + + if (is_broadcast_mac_addr(prxattrib->ra)) + no_gkey_bc_cnt++; + else + no_gkey_mc_cnt++; + + if (rtw_get_passing_time_ms(start) > 1000) { + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = rtw_get_current_time(); + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + } + + goto exit; + } + + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = 0; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + /* TODO: multiple GK? */ + prwskey = &stainfo->gtk.skey[0]; + } else + #endif + { + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) { + RTW_DBG("not match packet_index=%d, install_index=%d\n" + , prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid); + res = _FAIL; + goto exit; + } + } + } else + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + + res = _rtw_ccmp_decrypt(padapter, prwskey, + prxattrib->encrypt == _CCMP_256_ ? 32 : 16, + prxattrib->hdrlen, pframe, + ((union recv_frame *)precvframe)->u.hdr.len); + + AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } else { + res = _FAIL; + } + + } +exit: + return res; +} +#else +u32 rtw_aes_decrypt(_adapter *padapter, u8 *precvframe) +{ + /* exclude ICV */ + + + /*static*/ + /* unsigned char message[MAX_MSG_SIZE]; */ + + + /* Intermediate Buffers */ + + + sint length; + u8 *pframe, *prwskey; /* , *payload,*iv */ + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + /* struct recv_priv *precvpriv=&padapter->recvpriv; */ + u32 res = _SUCCESS; + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + /* 4 start to encrypt each fragment */ + if ((prxattrib->encrypt == _AES_)) { + + stainfo = rtw_get_stainfo(&padapter->stapriv , &prxattrib->ta[0]); + if (stainfo != NULL) { + + if (IS_MCAST(prxattrib->ra)) { + static systime start = 0; + static u32 no_gkey_bc_cnt = 0; + static u32 no_gkey_mc_cnt = 0; + + /* RTW_INFO("rx bc/mc packets, to perform sw rtw_aes_decrypt\n"); */ + /* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */ + if ((!MLME_IS_MESH(padapter) && psecuritypriv->binstallGrpkey == _FALSE) + #ifdef CONFIG_RTW_MESH + || !(stainfo->gtk_bmp | BIT(prxattrib->key_index)) + #endif + ) { + res = _FAIL; + + if (start == 0) + start = rtw_get_current_time(); + + if (is_broadcast_mac_addr(prxattrib->ra)) + no_gkey_bc_cnt++; + else + no_gkey_mc_cnt++; + + if (rtw_get_passing_time_ms(start) > 1000) { + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = rtw_get_current_time(); + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + } + + goto exit; + } + + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = 0; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + /* TODO: multiple GK? */ + prwskey = &stainfo->gtk.skey[0]; + } else + #endif + { + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) { + RTW_DBG("not match packet_index=%d, install_index=%d\n" + , prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid); + res = _FAIL; + goto exit; + } + } + } else + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + + length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len; +#if 0 + /* add for CONFIG_IEEE80211W, debug */ + if (0) + printk("@@@@@@@@@@@@@@@@@@ length=%d, prxattrib->hdrlen=%d, prxattrib->pkt_len=%d\n" + , length, prxattrib->hdrlen, prxattrib->pkt_len); + if (0) { + int no; + /* test print PSK */ + printk("PSK key below:\n"); + for (no = 0; no < 16; no++) + printk(" %02x ", prwskey[no]); + printk("\n"); + } + if (0) { + int no; + /* test print PSK */ + printk("frame:\n"); + for (no = 0; no < prxattrib->pkt_len; no++) + printk(" %02x ", pframe[no]); + printk("\n"); + } +#endif + + res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length); + + AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } else { + res = _FAIL; + } + + } +exit: + return res; +} +#endif + +#ifdef CONFIG_RTW_MESH_AEK +/* for AES-SIV, wrapper to ase_siv_encrypt and aes_siv_decrypt */ +int rtw_aes_siv_encrypt(const u8 *key, size_t key_len, const u8 *pw, + size_t pwlen, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *out) +{ + return _aes_siv_encrypt(key, key_len, pw, pwlen, + num_elem, addr, len, out); +} + +int rtw_aes_siv_decrypt(const u8 *key, size_t key_len, const u8 *iv_crypt, size_t iv_c_len, + size_t num_elem, const u8 *addr[], const size_t *len, u8 *out) +{ + return _aes_siv_decrypt(key, key_len, iv_crypt, + iv_c_len, num_elem, addr, len, out); +} +#endif /* CONFIG_RTW_MESH_AEK */ + +#ifdef CONFIG_TDLS +void wpa_tdls_generate_tpk(_adapter *padapter, void *sta) +{ + struct sta_info *psta = (struct sta_info *)sta; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + _tdls_generate_tpk(psta, adapter_mac_addr(padapter), get_bssid(pmlmepriv)); +} + +/** + * wpa_tdls_ftie_mic - Calculate TDLS FTIE MIC + * @kck: TPK-KCK + * @lnkid: Pointer to the beginning of Link Identifier IE + * @rsnie: Pointer to the beginning of RSN IE used for handshake + * @timeoutie: Pointer to the beginning of Timeout IE used for handshake + * @ftie: Pointer to the beginning of FT IE + * @mic: Pointer for writing MIC + * + * Calculate MIC for TDLS frame. + */ +int wpa_tdls_ftie_mic(u8 *kck, u8 trans_seq, + u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie, + u8 *mic) +{ + u8 *buf, *pos; + struct wpa_tdls_ftie *_ftie; + struct wpa_tdls_lnkid *_lnkid; + int ret; + int len = 2 * ETH_ALEN + 1 + 2 + lnkid[1] + 2 + rsnie[1] + + 2 + timeoutie[1] + 2 + ftie[1]; + buf = rtw_zmalloc(len); + if (!buf) { + RTW_INFO("TDLS: No memory for MIC calculation\n"); + return -1; + } + + pos = buf; + _lnkid = (struct wpa_tdls_lnkid *) lnkid; + /* 1) TDLS initiator STA MAC address */ + _rtw_memcpy(pos, _lnkid->init_sta, ETH_ALEN); + pos += ETH_ALEN; + /* 2) TDLS responder STA MAC address */ + _rtw_memcpy(pos, _lnkid->resp_sta, ETH_ALEN); + pos += ETH_ALEN; + /* 3) Transaction Sequence number */ + *pos++ = trans_seq; + /* 4) Link Identifier IE */ + _rtw_memcpy(pos, lnkid, 2 + lnkid[1]); + pos += 2 + lnkid[1]; + /* 5) RSN IE */ + _rtw_memcpy(pos, rsnie, 2 + rsnie[1]); + pos += 2 + rsnie[1]; + /* 6) Timeout Interval IE */ + _rtw_memcpy(pos, timeoutie, 2 + timeoutie[1]); + pos += 2 + timeoutie[1]; + /* 7) FTIE, with the MIC field of the FTIE set to 0 */ + _rtw_memcpy(pos, ftie, 2 + ftie[1]); + _ftie = (struct wpa_tdls_ftie *) pos; + _rtw_memset(_ftie->mic, 0, TDLS_MIC_LEN); + pos += 2 + ftie[1]; + + /* ret = omac1_aes_128(kck, buf, pos - buf, mic); */ + ret = _bip_ccmp_protect(kck, 16, buf, pos - buf, mic); + rtw_mfree(buf, len); + return ret; + +} + +/** + * wpa_tdls_teardown_ftie_mic - Calculate TDLS TEARDOWN FTIE MIC + * @kck: TPK-KCK + * @lnkid: Pointer to the beginning of Link Identifier IE + * @reason: Reason code of TDLS Teardown + * @dialog_token: Dialog token that was used in the MIC calculation for TPK Handshake Message 3 + * @trans_seq: Transaction Sequence number (1 octet) which shall be set to the value 4 + * @ftie: Pointer to the beginning of FT IE + * @mic: Pointer for writing MIC + * + * Calculate MIC for TDLS TEARDOWN frame according to Section 10.22.5 in IEEE 802.11 - 2012. + */ +int wpa_tdls_teardown_ftie_mic(u8 *kck, u8 *lnkid, u16 reason, + u8 dialog_token, u8 trans_seq, u8 *ftie, u8 *mic) +{ + u8 *buf, *pos; + struct wpa_tdls_ftie *_ftie; + int ret; + int len = 2 + lnkid[1] + 2 + 1 + 1 + 2 + ftie[1]; + + buf = rtw_zmalloc(len); + if (!buf) { + RTW_INFO("TDLS: No memory for MIC calculation\n"); + return -1; + } + + pos = buf; + /* 1) Link Identifier IE */ + _rtw_memcpy(pos, lnkid, 2 + lnkid[1]); + pos += 2 + lnkid[1]; + /* 2) Reason Code */ + _rtw_memcpy(pos, (u8 *)&reason, 2); + pos += 2; + /* 3) Dialog Token */ + *pos++ = dialog_token; + /* 4) Transaction Sequence number */ + *pos++ = trans_seq; + /* 5) FTIE, with the MIC field of the FTIE set to 0 */ + _rtw_memcpy(pos, ftie, 2 + ftie[1]); + _ftie = (struct wpa_tdls_ftie *) pos; + _rtw_memset(_ftie->mic, 0, TDLS_MIC_LEN); + pos += 2 + ftie[1]; + + /* ret = omac1_aes_128(kck, buf, pos - buf, mic); */ + ret = _bip_ccmp_protect(kck, 16, buf, pos - buf, mic); + rtw_mfree(buf, len); + return ret; + +} + +int tdls_verify_mic(u8 *kck, u8 trans_seq, + u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie) +{ + u8 *buf, *pos; + int len; + u8 mic[16]; + int ret; + u8 *rx_ftie, *tmp_ftie; + + if (lnkid == NULL || rsnie == NULL || + timeoutie == NULL || ftie == NULL) + return _FAIL; + + len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + *(rsnie + 1) + 2 + *(timeoutie + 1) + 2 + *(ftie + 1); + + buf = rtw_zmalloc(len); + if (buf == NULL) + return _FAIL; + + pos = buf; + /* 1) TDLS initiator STA MAC address */ + _rtw_memcpy(pos, lnkid + ETH_ALEN + 2, ETH_ALEN); + pos += ETH_ALEN; + /* 2) TDLS responder STA MAC address */ + _rtw_memcpy(pos, lnkid + 2 * ETH_ALEN + 2, ETH_ALEN); + pos += ETH_ALEN; + /* 3) Transaction Sequence number */ + *pos++ = trans_seq; + /* 4) Link Identifier IE */ + _rtw_memcpy(pos, lnkid, 2 + 18); + pos += 2 + 18; + /* 5) RSN IE */ + _rtw_memcpy(pos, rsnie, 2 + *(rsnie + 1)); + pos += 2 + *(rsnie + 1); + /* 6) Timeout Interval IE */ + _rtw_memcpy(pos, timeoutie, 2 + *(timeoutie + 1)); + pos += 2 + *(timeoutie + 1); + /* 7) FTIE, with the MIC field of the FTIE set to 0 */ + _rtw_memcpy(pos, ftie, 2 + *(ftie + 1)); + pos += 2; + tmp_ftie = (u8 *)(pos + 2); + _rtw_memset(tmp_ftie, 0, 16); + pos += *(ftie + 1); + + /* ret = omac1_aes_128(kck, buf, pos - buf, mic); */ + ret = _bip_ccmp_protect(kck, 16, buf, pos - buf, mic); + rtw_mfree(buf, len); + if (ret == _FAIL) + return _FAIL; + rx_ftie = ftie + 4; + + if (_rtw_memcmp2(mic, rx_ftie, 16) == 0) { + /* Valid MIC */ + return _SUCCESS; + } + + /* Invalid MIC */ + RTW_INFO("[%s] Invalid MIC\n", __FUNCTION__); + return _FAIL; + +} +#endif /* CONFIG_TDLS */ + +/* Restore HW wep key setting according to key_mask */ +void rtw_sec_restore_wep_key(_adapter *adapter) +{ + struct security_priv *securitypriv = &(adapter->securitypriv); + sint keyid; + + if ((_WEP40_ == securitypriv->dot11PrivacyAlgrthm) || (_WEP104_ == securitypriv->dot11PrivacyAlgrthm)) { + for (keyid = 0; keyid < 4; keyid++) { + if (securitypriv->key_mask & BIT(keyid)) { + if (keyid == securitypriv->dot11PrivacyKeyIndex) + rtw_set_key(adapter, securitypriv, keyid, 1, _FALSE); + else + rtw_set_key(adapter, securitypriv, keyid, 0, _FALSE); + } + } + } +} + +u8 rtw_handle_tkip_countermeasure(_adapter *adapter, const char *caller) +{ + struct security_priv *securitypriv = &(adapter->securitypriv); + u8 status = _SUCCESS; + + if (securitypriv->btkip_countermeasure == _TRUE) { + u32 passing_ms = rtw_get_passing_time_ms(securitypriv->btkip_countermeasure_time); + if (passing_ms > 60 * 1000) { + RTW_PRINT("%s("ADPT_FMT") countermeasure time:%ds > 60s\n", + caller, ADPT_ARG(adapter), passing_ms / 1000); + securitypriv->btkip_countermeasure = _FALSE; + securitypriv->btkip_countermeasure_time = 0; + } else { + RTW_PRINT("%s("ADPT_FMT") countermeasure time:%ds < 60s\n", + caller, ADPT_ARG(adapter), passing_ms / 1000); + status = _FAIL; + } + } + + return status; +} + +#ifdef CONFIG_WOWLAN +u16 rtw_cal_crc16(u8 data, u16 crc) +{ + u8 shift_in, data_bit; + u8 crc_bit4, crc_bit11, crc_bit15; + u16 crc_result; + int index; + + for (index = 0; index < 8; index++) { + crc_bit15 = ((crc & BIT15) ? 1 : 0); + data_bit = (data & (BIT0 << index) ? 1 : 0); + shift_in = crc_bit15 ^ data_bit; + /*printf("crc_bit15=%d, DataBit=%d, shift_in=%d\n", + * crc_bit15, data_bit, shift_in);*/ + + crc_result = crc << 1; + + if (shift_in == 0) + crc_result &= (~BIT0); + else + crc_result |= BIT0; + /*printf("CRC =%x\n",CRC_Result);*/ + + crc_bit11 = ((crc & BIT11) ? 1 : 0) ^ shift_in; + + if (crc_bit11 == 0) + crc_result &= (~BIT12); + else + crc_result |= BIT12; + + /*printf("bit12 CRC =%x\n",CRC_Result);*/ + + crc_bit4 = ((crc & BIT4) ? 1 : 0) ^ shift_in; + + if (crc_bit4 == 0) + crc_result &= (~BIT5); + else + crc_result |= BIT5; + + /* printf("bit5 CRC =%x\n",CRC_Result); */ + /* repeat using the last result*/ + crc = crc_result; + } + return crc; +} + +/* + * function name :rtw_calc_crc + * + * input: char* pattern , pattern size + * + */ +u16 rtw_calc_crc(u8 *pdata, int length) +{ + u16 crc = 0xffff; + int i; + + for (i = 0; i < length; i++) + crc = rtw_cal_crc16(pdata[i], crc); + /* get 1' complement */ + crc = ~crc; + + return crc; +} +#endif /*CONFIG_WOWLAN*/ + +u32 rtw_calc_crc32(u8 *data, size_t len) +{ + size_t i; + u32 crc = 0xFFFFFFFF; + + if (bcrc32initialized == 0) + crc32_init(); + + for (i = 0; i < len; i++) + crc = crc32_table[(crc ^ data[i]) & 0xff] ^ (crc >> 8); + + /* return 1' complement */ + return ~crc; +} + + +/** + * rtw_gcmp_encrypt - + * @padapter: + * @pxmitframe: + * + */ +u32 rtw_gcmp_encrypt(_adapter *padapter, u8 *pxmitframe) +{ + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + /* Intermediate Buffers */ + sint curfragnum, plen; + u32 prwskeylen; + u8 *pframe = NULL; + u8 *prwskey = NULL; + u8 hw_hdr_offset = 0; + u32 res = _SUCCESS; + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + + (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset; + + /* start to encrypt each fragment */ + if ((pattrib->encrypt == _GCMP_) || + (pattrib->encrypt == _GCMP_256_)) { + + if (IS_MCAST(pattrib->ra)) + prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; + else + prwskey = pattrib->dot118021x_UncstKey.skey; + + prwskeylen = (pattrib->encrypt == _GCMP_256_) ? 32 : 16; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + if ((curfragnum + 1) == pattrib->nr_frags) { + /* the last fragment */ + plen = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + + _rtw_gcmp_encrypt(padapter, prwskey, prwskeylen, pattrib->hdrlen, pframe, plen); + } else { + plen = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len; + + _rtw_gcmp_encrypt(padapter, prwskey, prwskeylen, pattrib->hdrlen, pframe, plen); + pframe += pxmitpriv->frag_len; + pframe = (u8 *)RND4((SIZE_PTR)(pframe)); + } + } + + GCMP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra); + } + + return res; +} + +u32 rtw_gcmp_decrypt(_adapter *padapter, u8 *precvframe) +{ + u32 prwskeylen; + u8 * pframe,*prwskey; + struct sta_info *stainfo; + struct rx_pkt_attrib *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + u32 res = _SUCCESS; + pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data; + + if ((prxattrib->encrypt == _GCMP_) || + (prxattrib->encrypt == _GCMP_256_)) { + stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]); + if (stainfo != NULL) { + if (IS_MCAST(prxattrib->ra)) { + static systime start = 0; + static u32 no_gkey_bc_cnt = 0; + static u32 no_gkey_mc_cnt = 0; + + if ((!MLME_IS_MESH(padapter) && psecuritypriv->binstallGrpkey == _FALSE) + #ifdef CONFIG_RTW_MESH + || !(stainfo->gtk_bmp | BIT(prxattrib->key_index)) + #endif + ) { + res = _FAIL; + + if (start == 0) + start = rtw_get_current_time(); + + if (is_broadcast_mac_addr(prxattrib->ra)) + no_gkey_bc_cnt++; + else + no_gkey_mc_cnt++; + + if (rtw_get_passing_time_ms(start) > 1000) { + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = rtw_get_current_time(); + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + } + + goto exit; + } + + if (no_gkey_bc_cnt || no_gkey_mc_cnt) { + RTW_PRINT(FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n", + FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt); + } + start = 0; + no_gkey_bc_cnt = 0; + no_gkey_mc_cnt = 0; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + /* TODO: multiple GK? */ + prwskey = &stainfo->gtk.skey[0]; + } else + #endif + { + prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey; + if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) { + RTW_DBG("not match packet_index=%d, install_index=%d\n" + , prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid); + res = _FAIL; + goto exit; + } + } + } else + prwskey = &stainfo->dot118021x_UncstKey.skey[0]; + + res = _rtw_gcmp_decrypt(padapter, prwskey, + prxattrib->encrypt == _GCMP_256_ ? 32 : 16, + prxattrib->hdrlen, pframe, + ((union recv_frame *)precvframe)->u.hdr.len); + + GCMP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra); + } else { + res = _FAIL; + } + + } +exit: + return res; +} + + +#ifdef CONFIG_IEEE80211W +u8 rtw_calculate_bip_mic(enum security_type gmcs, u8 *whdr_pos, s32 len, + const u8 *key, const u8 *data, size_t data_len, u8 *mic) +{ + u8 res = _SUCCESS; + + if (gmcs == _BIP_CMAC_128_) { + if (_bip_ccmp_protect(key, 16, data, data_len, mic) == _FALSE) { + res = _FAIL; + RTW_ERR("%s : _bip_ccmp_protect(128) fail!", __func__); + } + } else if (gmcs == _BIP_CMAC_256_) { + if (_bip_ccmp_protect(key, 32, data, data_len, mic) == _FALSE) { + res = _FAIL; + RTW_ERR("%s : _bip_ccmp_protect(256) fail!", __func__); + } + } else if (gmcs == _BIP_GMAC_128_) { + if (_bip_gcmp_protect(whdr_pos, len, key, 16, + data, data_len, mic) == _FALSE) { + res = _FAIL; + RTW_ERR("%s : _bip_gcmp_protect(128) fail!", __func__); + } + } else if (gmcs == _BIP_GMAC_256_) { + if (_bip_gcmp_protect(whdr_pos, len, key, 32, + data, data_len, mic) == _FALSE) { + res = _FAIL; + RTW_ERR("%s : _bip_gcmp_protect(256) fail!", __func__); + } + } else { + res = _FAIL; + RTW_ERR("%s : unsupport dot11wCipher !\n", __func__); + } + + return res; +} + + +u32 rtw_bip_verify(enum security_type gmcs, u16 pkt_len, + u8 *whdr_pos, sint flen, const u8 *key, u16 keyid, u64 *ipn) +{ + u8 * BIP_AAD,*mme; + u32 res = _FAIL; + uint len, ori_len; + u16 pkt_keyid = 0; + u64 pkt_ipn = 0; + struct rtw_ieee80211_hdr *pwlanhdr; + u8 mic[16]; + u8 mic_len, mme_offset; + + mic_len = (gmcs == _BIP_CMAC_128_) ? 8 : 16; + + if (flen < WLAN_HDR_A3_LEN || flen - WLAN_HDR_A3_LEN < mic_len) + return RTW_RX_HANDLED; + + mme_offset = (mic_len == 8) ? 18 : 26; + mme = whdr_pos + flen - mme_offset; + if (*mme != _MME_IE_) + return RTW_RX_HANDLED; + + /* copy key index */ + _rtw_memcpy(&pkt_keyid, mme + 2, 2); + pkt_keyid = le16_to_cpu(pkt_keyid); + if (pkt_keyid != keyid) { + RTW_INFO("BIP key index error!\n"); + return _FAIL; + } + + /* save packet number */ + _rtw_memcpy(&pkt_ipn, mme + 4, 6); + pkt_ipn = le64_to_cpu(pkt_ipn); + /* BIP packet number should bigger than previous BIP packet */ + if (pkt_ipn <= *ipn) { /* wrap around? */ + RTW_INFO("replay BIP packet\n"); + return _FAIL; + } + + ori_len = flen - WLAN_HDR_A3_LEN + BIP_AAD_SIZE; + BIP_AAD = rtw_zmalloc(ori_len); + if (BIP_AAD == NULL) { + RTW_INFO("BIP AAD allocate fail\n"); + return _FAIL; + } + + /* mapping to wlan header */ + pwlanhdr = (struct rtw_ieee80211_hdr *)whdr_pos; + + /* save the frame body + MME (w/o mic) */ + _rtw_memcpy(BIP_AAD + BIP_AAD_SIZE, + whdr_pos + WLAN_HDR_A3_LEN, + flen - WLAN_HDR_A3_LEN - mic_len); + + /* conscruct AAD, copy frame control field */ + _rtw_memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2); + ClearRetry(BIP_AAD); + ClearPwrMgt(BIP_AAD); + ClearMData(BIP_AAD); + /* conscruct AAD, copy address 1 to address 3 */ + _rtw_memcpy(BIP_AAD + 2, GetAddr1Ptr((u8 *)pwlanhdr), 18); + + if (rtw_calculate_bip_mic(gmcs, whdr_pos, + pkt_len, key, BIP_AAD, ori_len, mic) == _FAIL) + goto BIP_exit; + + /* MIC field should be last 8 bytes of packet (packet without FCS) */ + if (_rtw_memcmp(mic, whdr_pos + flen - mic_len, mic_len)) { + *ipn = pkt_ipn; + res = _SUCCESS; + } else + RTW_INFO("BIP MIC error!\n"); + +#if 0 + /* management packet content */ + { + int pp; + RTW_INFO("pkt: "); + RTW_INFO_DUMP("", whdr_pos, flen); + RTW_INFO("\n"); + /* BIP AAD + management frame body + MME(MIC is zero) */ + RTW_INFO("AAD+PKT: "); + RTW_INFO_DUMP("", BIP_AAD, ori_len); + RTW_INFO("\n"); + /* show the MIC result */ + RTW_INFO("mic: "); + RTW_INFO_DUMP("", mic, mic_len); + RTW_INFO("\n"); + } +#endif + +BIP_exit: + + rtw_mfree(BIP_AAD, ori_len); + return res; +} + +#endif /* CONFIG_IEEE80211W */ + diff --git a/core/rtw_sreset.c b/core/rtw_sreset.c new file mode 100644 index 0000000..21cc18b --- /dev/null +++ b/core/rtw_sreset.c @@ -0,0 +1,320 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include +#include + +void sreset_init_value(_adapter *padapter) +{ +#if defined(DBG_CONFIG_ERROR_DETECT) + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + + _rtw_mutex_init(&psrtpriv->silentreset_mutex); + psrtpriv->silent_reset_inprogress = _FALSE; + psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS; + psrtpriv->last_tx_time = 0; + psrtpriv->last_tx_complete_time = 0; +#endif +} +void sreset_reset_value(_adapter *padapter) +{ +#if defined(DBG_CONFIG_ERROR_DETECT) + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + + psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS; + psrtpriv->last_tx_time = 0; + psrtpriv->last_tx_complete_time = 0; +#endif +} + +u8 sreset_get_wifi_status(_adapter *padapter) +{ +#if defined(DBG_CONFIG_ERROR_DETECT) + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + u8 status = WIFI_STATUS_SUCCESS; + u32 val32 = 0; + + if (psrtpriv->silent_reset_inprogress == _TRUE) + return status; + val32 = rtw_read32(padapter, REG_TXDMA_STATUS); + if (val32 == 0xeaeaeaea) + psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST; + else if (val32 != 0) { + RTW_INFO("txdmastatu(%x)\n", val32); + psrtpriv->Wifi_Error_Status = WIFI_MAC_TXDMA_ERROR; + } + + if (WIFI_STATUS_SUCCESS != psrtpriv->Wifi_Error_Status) { + RTW_INFO("==>%s error_status(0x%x)\n", __FUNCTION__, psrtpriv->Wifi_Error_Status); + status = (psrtpriv->Wifi_Error_Status & (~(USB_READ_PORT_FAIL | USB_WRITE_PORT_FAIL))); + } + RTW_INFO("==> %s wifi_status(0x%x)\n", __FUNCTION__, status); + + /* status restore */ + psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS; + + return status; +#else + return WIFI_STATUS_SUCCESS; +#endif +} + +void sreset_set_wifi_error_status(_adapter *padapter, u32 status) +{ +#if defined(DBG_CONFIG_ERROR_DETECT) + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + pHalData->srestpriv.Wifi_Error_Status = status; +#endif +} + +void sreset_set_trigger_point(_adapter *padapter, s32 tgp) +{ +#if defined(DBG_CONFIG_ERROR_DETECT) + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + pHalData->srestpriv.dbg_trigger_point = tgp; +#endif +} + +bool sreset_inprogress(_adapter *padapter) +{ +#if defined(DBG_CONFIG_ERROR_RESET) + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + return pHalData->srestpriv.silent_reset_inprogress; +#else + return _FALSE; +#endif +} + +void sreset_restore_security_station(_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + struct mlme_ext_info *pmlmeinfo = &padapter->mlmeextpriv.mlmext_info; + + { + u8 val8; + + if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) { + val8 = 0xcc; +#ifdef CONFIG_WAPI_SUPPORT + } else if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI) { + /* Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey. */ + val8 = 0x4c; +#endif + } else + val8 = 0xcf; + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8)); + } + + if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) || + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) { + psta = rtw_get_stainfo(pstapriv, get_bssid(mlmepriv)); + if (psta == NULL) { + /* DEBUG_ERR( ("Set wpa_set_encryption: Obtain Sta_info fail\n")); */ + } else { + /* pairwise key */ + rtw_setstakey_cmd(padapter, psta, UNICAST_KEY, _FALSE); + /* group key */ + rtw_set_key(padapter, &padapter->securitypriv, padapter->securitypriv.dot118021XGrpKeyid, 0, _FALSE); + } + } +} + +void sreset_restore_network_station(_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 doiqk = _FALSE; + + rtw_setopmode_cmd(padapter, Ndis802_11Infrastructure, RTW_CMDF_DIRECTLY); + + { + u8 threshold; +#ifdef CONFIG_USB_HCI + /* TH=1 => means that invalidate usb rx aggregation */ + /* TH=0 => means that validate usb rx aggregation, use init value. */ +#ifdef CONFIG_80211N_HT + if (mlmepriv->htpriv.ht_option) { + if (padapter->registrypriv.wifi_spec == 1) + threshold = 1; + else + threshold = 0; + rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } else { + threshold = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold)); + } +#endif /* CONFIG_80211N_HT */ +#endif + } + + doiqk = _TRUE; + rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK , &doiqk); + + set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + doiqk = _FALSE; + rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk); + /* disable dynamic functions, such as high power, DIG */ + /*rtw_phydm_func_disable_all(padapter);*/ + + rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress); + + { + u8 join_type = 0; + + rtw_hal_rcr_set_chk_bssid(padapter, MLME_STA_CONNECTING); + rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type)); + + rtw_btcoex_connect_notify(padapter, join_type); + } + + Set_MSR(padapter, (pmlmeinfo->state & 0x3)); + + mlmeext_joinbss_event_callback(padapter, 1); + /* restore Sequence No. */ + rtw_hal_set_hwreg(padapter, HW_VAR_RESTORE_HW_SEQ, 0); + + sreset_restore_security_station(padapter); +} + + +void sreset_restore_network_status(_adapter *padapter) +{ + struct mlme_priv *mlmepriv = &padapter->mlmepriv; + + if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) { + RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); + sreset_restore_network_station(padapter); + } +#ifdef CONFIG_AP_MODE + else if (MLME_IS_AP(padapter) || MLME_IS_MESH(padapter)) { + RTW_INFO(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(padapter), MLME_IS_AP(padapter) ? "AP" : "MESH"); + rtw_ap_restore_network(padapter); + } +#endif + else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE)) + RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); + else + RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv)); +} + +void sreset_stop_adapter(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter == NULL) + return; + + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + rtw_netif_stop_queue(padapter->pnetdev); + + rtw_cancel_all_timer(padapter); + + /* TODO: OS and HCI independent */ +#if defined(PLATFORM_LINUX) && defined(CONFIG_USB_HCI) + tasklet_kill(&pxmitpriv->xmit_tasklet); +#endif + + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY)) + rtw_scan_abort(padapter); + + if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING)) { + rtw_set_to_roam(padapter, 0); + rtw_join_timeout_handler(padapter); + } + +} + +void sreset_start_adapter(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + if (padapter == NULL) + return; + + RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter)); + + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE)) + sreset_restore_network_status(padapter); + + /* TODO: OS and HCI independent */ +#if defined(PLATFORM_LINUX) && defined(CONFIG_USB_HCI) + tasklet_hi_schedule(&pxmitpriv->xmit_tasklet); +#endif + + if (is_primary_adapter(padapter)) + _set_timer(&adapter_to_dvobj(padapter)->dynamic_chk_timer, 2000); + + rtw_netif_wake_queue(padapter->pnetdev); +} + +void sreset_reset(_adapter *padapter) +{ +#ifdef DBG_CONFIG_ERROR_RESET + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + struct sreset_priv *psrtpriv = &pHalData->srestpriv; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _irqL irqL; + systime start = rtw_get_current_time(); + struct dvobj_priv *psdpriv = padapter->dvobj; + struct debug_priv *pdbgpriv = &psdpriv->drv_dbg; + + RTW_INFO("%s\n", __FUNCTION__); + + psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS; + + +#ifdef CONFIG_LPS + rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "SRESET"); +#endif/* #ifdef CONFIG_LPS */ + + _enter_pwrlock(&pwrpriv->lock); + + psrtpriv->silent_reset_inprogress = _TRUE; + pwrpriv->change_rfpwrstate = rf_off; + + rtw_mi_sreset_adapter_hdl(padapter, _FALSE);/*sreset_stop_adapter*/ +#ifdef CONFIG_IPS + _ips_enter(padapter); + _ips_leave(padapter); +#endif +#if defined(CONFIG_AP_MODE) && defined(CONFIG_CONCURRENT_MODE) + rtw_mi_ap_info_restore(padapter); +#endif + rtw_mi_sreset_adapter_hdl(padapter, _TRUE);/*sreset_start_adapter*/ + + psrtpriv->silent_reset_inprogress = _FALSE; + + _exit_pwrlock(&pwrpriv->lock); + + RTW_INFO("%s done in %d ms\n", __FUNCTION__, rtw_get_passing_time_ms(start)); + pdbgpriv->dbg_sreset_cnt++; + + psrtpriv->self_dect_fw = _FALSE; + psrtpriv->rx_cnt = 0; +#endif +} diff --git a/core/rtw_sta_mgt.c b/core/rtw_sta_mgt.c new file mode 100644 index 0000000..c2e27b9 --- /dev/null +++ b/core/rtw_sta_mgt.c @@ -0,0 +1,1383 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_STA_MGT_C_ + +#include + +bool test_st_match_rule(_adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port) +{ + if (ntohs(*((u16 *)local_port)) == 5001 || ntohs(*((u16 *)remote_port)) == 5001) + return _TRUE; + return _FALSE; +} + +struct st_register test_st_reg = { + .s_proto = 0x06, + .rule = test_st_match_rule, +}; + +inline void rtw_st_ctl_init(struct st_ctl_t *st_ctl) +{ + _rtw_memset(st_ctl->reg, 0 , sizeof(struct st_register) * SESSION_TRACKER_REG_ID_NUM); + _rtw_init_queue(&st_ctl->tracker_q); +} + +inline void rtw_st_ctl_clear_tracker_q(struct st_ctl_t *st_ctl) +{ + _irqL irqL; + _list *plist, *phead; + struct session_tracker *st; + + _enter_critical_bh(&st_ctl->tracker_q.lock, &irqL); + phead = &st_ctl->tracker_q.queue; + plist = get_next(phead); + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + st = LIST_CONTAINOR(plist, struct session_tracker, list); + plist = get_next(plist); + rtw_list_delete(&st->list); + rtw_mfree((u8 *)st, sizeof(struct session_tracker)); + } + _exit_critical_bh(&st_ctl->tracker_q.lock, &irqL); +} + +inline void rtw_st_ctl_deinit(struct st_ctl_t *st_ctl) +{ + rtw_st_ctl_clear_tracker_q(st_ctl); + _rtw_deinit_queue(&st_ctl->tracker_q); +} + +inline void rtw_st_ctl_register(struct st_ctl_t *st_ctl, u8 st_reg_id, struct st_register *reg) +{ + if (st_reg_id >= SESSION_TRACKER_REG_ID_NUM) { + rtw_warn_on(1); + return; + } + + st_ctl->reg[st_reg_id].s_proto = reg->s_proto; + st_ctl->reg[st_reg_id].rule = reg->rule; +} + +inline void rtw_st_ctl_unregister(struct st_ctl_t *st_ctl, u8 st_reg_id) +{ + int i; + + if (st_reg_id >= SESSION_TRACKER_REG_ID_NUM) { + rtw_warn_on(1); + return; + } + + st_ctl->reg[st_reg_id].s_proto = 0; + st_ctl->reg[st_reg_id].rule = NULL; + + /* clear tracker queue if no session trecker registered */ + for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) + if (st_ctl->reg[i].s_proto != 0) + break; + if (i >= SESSION_TRACKER_REG_ID_NUM) + rtw_st_ctl_clear_tracker_q(st_ctl); +} + +inline bool rtw_st_ctl_chk_reg_s_proto(struct st_ctl_t *st_ctl, u8 s_proto) +{ + bool ret = _FALSE; + int i; + + for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) { + if (st_ctl->reg[i].s_proto == s_proto) { + ret = _TRUE; + break; + } + } + + return ret; +} + +inline bool rtw_st_ctl_chk_reg_rule(struct st_ctl_t *st_ctl, _adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port) +{ + bool ret = _FALSE; + int i; + st_match_rule rule; + + for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) { + rule = st_ctl->reg[i].rule; + if (rule && rule(adapter, local_naddr, local_port, remote_naddr, remote_port) == _TRUE) { + ret = _TRUE; + break; + } + } + + return ret; +} + +void rtw_st_ctl_rx(struct sta_info *sta, u8 *ehdr_pos) +{ + _adapter *adapter = sta->padapter; + struct ethhdr *etherhdr = (struct ethhdr *)ehdr_pos; + + if (ntohs(etherhdr->h_proto) == ETH_P_IP) { + u8 *ip = ehdr_pos + ETH_HLEN; + + if (GET_IPV4_PROTOCOL(ip) == 0x06 /* TCP */ + && rtw_st_ctl_chk_reg_s_proto(&sta->st_ctl, 0x06) == _TRUE + ) { + u8 *tcp = ip + GET_IPV4_IHL(ip) * 4; + + if (rtw_st_ctl_chk_reg_rule(&sta->st_ctl, adapter, IPV4_DST(ip), TCP_DST(tcp), IPV4_SRC(ip), TCP_SRC(tcp)) == _TRUE) { + if (GET_TCP_SYN(tcp) && GET_TCP_ACK(tcp)) { + session_tracker_add_cmd(adapter, sta + , IPV4_DST(ip), TCP_DST(tcp) + , IPV4_SRC(ip), TCP_SRC(tcp)); + if (DBG_SESSION_TRACKER) + RTW_INFO(FUNC_ADPT_FMT" local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT" SYN-ACK\n" + , FUNC_ADPT_ARG(adapter) + , IP_ARG(IPV4_DST(ip)), PORT_ARG(TCP_DST(tcp)) + , IP_ARG(IPV4_SRC(ip)), PORT_ARG(TCP_SRC(tcp))); + } + if (GET_TCP_FIN(tcp)) { + session_tracker_del_cmd(adapter, sta + , IPV4_DST(ip), TCP_DST(tcp) + , IPV4_SRC(ip), TCP_SRC(tcp)); + if (DBG_SESSION_TRACKER) + RTW_INFO(FUNC_ADPT_FMT" local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT" FIN\n" + , FUNC_ADPT_ARG(adapter) + , IP_ARG(IPV4_DST(ip)), PORT_ARG(TCP_DST(tcp)) + , IP_ARG(IPV4_SRC(ip)), PORT_ARG(TCP_SRC(tcp))); + } + } + + } + } +} + +#define SESSION_TRACKER_FMT IP_FMT":"PORT_FMT" "IP_FMT":"PORT_FMT" %u %d" +#define SESSION_TRACKER_ARG(st) IP_ARG(&(st)->local_naddr), PORT_ARG(&(st)->local_port), IP_ARG(&(st)->remote_naddr), PORT_ARG(&(st)->remote_port), (st)->status, rtw_get_passing_time_ms((st)->set_time) + +void dump_st_ctl(void *sel, struct st_ctl_t *st_ctl) +{ + int i; + _irqL irqL; + _list *plist, *phead; + struct session_tracker *st; + + if (!DBG_SESSION_TRACKER) + return; + + for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) + RTW_PRINT_SEL(sel, "reg%d: %u %p\n", i, st_ctl->reg[i].s_proto, st_ctl->reg[i].rule); + + _enter_critical_bh(&st_ctl->tracker_q.lock, &irqL); + phead = &st_ctl->tracker_q.queue; + plist = get_next(phead); + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + st = LIST_CONTAINOR(plist, struct session_tracker, list); + plist = get_next(plist); + + RTW_PRINT_SEL(sel, SESSION_TRACKER_FMT"\n", SESSION_TRACKER_ARG(st)); + } + _exit_critical_bh(&st_ctl->tracker_q.lock, &irqL); + +} + +void _rtw_init_stainfo(struct sta_info *psta); +void _rtw_init_stainfo(struct sta_info *psta) +{ + _rtw_memset((u8 *)psta, 0, sizeof(struct sta_info)); + + _rtw_spinlock_init(&psta->lock); + _rtw_init_listhead(&psta->list); + _rtw_init_listhead(&psta->hash_list); + /* _rtw_init_listhead(&psta->asoc_list); */ + /* _rtw_init_listhead(&psta->sleep_list); */ + /* _rtw_init_listhead(&psta->wakeup_list); */ + + _rtw_init_queue(&psta->sleep_q); +#ifdef CONFIG_RTW_MGMT_QUEUE + _rtw_init_queue(&psta->mgmt_sleep_q); +#endif + _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); + _rtw_init_sta_recv_priv(&psta->sta_recvpriv); + +#ifdef CONFIG_AP_MODE + _rtw_init_listhead(&psta->asoc_list); + _rtw_init_listhead(&psta->auth_list); + psta->bpairwise_key_installed = _FALSE; + +#ifdef CONFIG_RTW_80211R + psta->ft_pairwise_key_installed = _FALSE; +#endif +#endif /* CONFIG_AP_MODE */ + + rtw_st_ctl_init(&psta->st_ctl); +} + +u32 _rtw_init_sta_priv(struct sta_priv *pstapriv) +{ + _adapter *adapter = container_of(pstapriv, _adapter, stapriv); + struct macid_ctl_t *macid_ctl = adapter_to_macidctl(adapter); + struct sta_info *psta; + s32 i; + u32 ret = _FAIL; + + pstapriv->padapter = adapter; + + pstapriv->pallocated_stainfo_buf = rtw_zvmalloc( + sizeof(struct sta_info) * NUM_STA + MEM_ALIGNMENT_OFFSET); + if (!pstapriv->pallocated_stainfo_buf) + goto exit; + + pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf; + if ((SIZE_PTR)pstapriv->pstainfo_buf & MEM_ALIGNMENT_PADDING) + pstapriv->pstainfo_buf += MEM_ALIGNMENT_OFFSET - + ((SIZE_PTR)pstapriv->pstainfo_buf & MEM_ALIGNMENT_PADDING); + + _rtw_init_queue(&pstapriv->free_sta_queue); + + _rtw_spinlock_init(&pstapriv->sta_hash_lock); + + /* _rtw_init_queue(&pstapriv->asoc_q); */ + pstapriv->asoc_sta_count = 0; + _rtw_init_queue(&pstapriv->sleep_q); + _rtw_init_queue(&pstapriv->wakeup_q); + + psta = (struct sta_info *)(pstapriv->pstainfo_buf); + + + for (i = 0; i < NUM_STA; i++) { + _rtw_init_stainfo(psta); + + _rtw_init_listhead(&(pstapriv->sta_hash[i])); + + rtw_list_insert_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue)); + + psta++; + } + + pstapriv->adhoc_expire_to = 4; /* 4 * 2 = 8 sec */ + +#ifdef CONFIG_AP_MODE + pstapriv->max_aid = macid_ctl->num; + pstapriv->rr_aid = 0; + pstapriv->started_aid = 1; + pstapriv->sta_aid = rtw_zmalloc(pstapriv->max_aid * sizeof(struct sta_info *)); + if (!pstapriv->sta_aid) + goto exit; + pstapriv->aid_bmp_len = AID_BMP_LEN(pstapriv->max_aid); + pstapriv->sta_dz_bitmap = rtw_zmalloc(pstapriv->aid_bmp_len); + if (!pstapriv->sta_dz_bitmap) + goto exit; + pstapriv->tim_bitmap = rtw_zmalloc(pstapriv->aid_bmp_len); + if (!pstapriv->tim_bitmap) + goto exit; + + _rtw_init_listhead(&pstapriv->asoc_list); + _rtw_init_listhead(&pstapriv->auth_list); + _rtw_spinlock_init(&pstapriv->asoc_list_lock); + _rtw_spinlock_init(&pstapriv->auth_list_lock); + pstapriv->asoc_list_cnt = 0; + pstapriv->auth_list_cnt = 0; +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + pstapriv->tbtx_asoc_list_cnt = 0; +#endif + + pstapriv->auth_to = 3; /* 3*2 = 6 sec */ + pstapriv->assoc_to = 3; + /* pstapriv->expire_to = 900; */ /* 900*2 = 1800 sec = 30 min, expire after no any traffic. */ + /* pstapriv->expire_to = 30; */ /* 30*2 = 60 sec = 1 min, expire after no any traffic. */ +#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK + pstapriv->expire_to = 3; /* 3*2 = 6 sec */ +#else + pstapriv->expire_to = 60;/* 60*2 = 120 sec = 2 min, expire after no any traffic. */ +#endif +#ifdef CONFIG_ATMEL_RC_PATCH + _rtw_memset(pstapriv->atmel_rc_pattern, 0, ETH_ALEN); +#endif + pstapriv->max_num_sta = NUM_STA; + +#if CONFIG_RTW_MACADDR_ACL + for (i = 0; i < RTW_ACL_PERIOD_NUM; i++) + rtw_macaddr_acl_init(adapter, i); +#endif +#endif /* CONFIG_AP_MODE */ + +#if CONFIG_RTW_PRE_LINK_STA + rtw_pre_link_sta_ctl_init(pstapriv); +#endif + +#if defined(DBG_ROAMING_TEST) || defined(CONFIG_RTW_REPEATER_SON) + rtw_set_rx_chk_limit(adapter,1); +#elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER) + rtw_set_rx_chk_limit(adapter,4); +#else + rtw_set_rx_chk_limit(adapter,8); +#endif + + ret = _SUCCESS; + +exit: + if (ret != _SUCCESS) { + if (pstapriv->pallocated_stainfo_buf) + rtw_vmfree(pstapriv->pallocated_stainfo_buf, + sizeof(struct sta_info) * NUM_STA + MEM_ALIGNMENT_OFFSET); + #ifdef CONFIG_AP_MODE + if (pstapriv->sta_aid) + rtw_mfree(pstapriv->sta_aid, pstapriv->max_aid * sizeof(struct sta_info *)); + if (pstapriv->sta_dz_bitmap) + rtw_mfree(pstapriv->sta_dz_bitmap, pstapriv->aid_bmp_len); + #endif + } + + return ret; +} + +inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta) +{ + int offset = (((u8 *)sta) - stapriv->pstainfo_buf) / sizeof(struct sta_info); + + if (!stainfo_offset_valid(offset)) + RTW_INFO("%s invalid offset(%d), out of range!!!", __func__, offset); + + return offset; +} + +inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset) +{ + if (!stainfo_offset_valid(offset)) + RTW_INFO("%s invalid offset(%d), out of range!!!", __func__, offset); + + return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info)); +} + +void _rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv); +void _rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv) +{ + + _rtw_spinlock_free(&psta_xmitpriv->lock); + + _rtw_spinlock_free(&(psta_xmitpriv->be_q.sta_pending.lock)); + _rtw_spinlock_free(&(psta_xmitpriv->bk_q.sta_pending.lock)); + _rtw_spinlock_free(&(psta_xmitpriv->vi_q.sta_pending.lock)); + _rtw_spinlock_free(&(psta_xmitpriv->vo_q.sta_pending.lock)); +#ifdef CONFIG_RTW_MGMT_QUEUE + _rtw_spinlock_free(&(psta_xmitpriv->mgmt_q.sta_pending.lock)); +#endif +} + +static void _rtw_free_sta_recv_priv_lock(struct sta_recv_priv *psta_recvpriv) +{ + + _rtw_spinlock_free(&psta_recvpriv->lock); + + _rtw_spinlock_free(&(psta_recvpriv->defrag_q.lock)); + + +} + +void rtw_mfree_stainfo(struct sta_info *psta); +void rtw_mfree_stainfo(struct sta_info *psta) +{ + + if (&psta->lock != NULL) + _rtw_spinlock_free(&psta->lock); + + _rtw_free_sta_xmit_priv_lock(&psta->sta_xmitpriv); + _rtw_free_sta_recv_priv_lock(&psta->sta_recvpriv); + +} + + +/* this function is used to free the memory of lock || sema for all stainfos */ +void rtw_mfree_all_stainfo(struct sta_priv *pstapriv); +void rtw_mfree_all_stainfo(struct sta_priv *pstapriv) +{ + _irqL irqL; + _list *plist, *phead; + struct sta_info *psta = NULL; + + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + phead = get_list_head(&pstapriv->free_sta_queue); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info , list); + plist = get_next(plist); + + rtw_mfree_stainfo(psta); + } + + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + +} + +void rtw_mfree_sta_priv_lock(struct sta_priv *pstapriv); +void rtw_mfree_sta_priv_lock(struct sta_priv *pstapriv) +{ + rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */ + + _rtw_spinlock_free(&pstapriv->free_sta_queue.lock); + + _rtw_spinlock_free(&pstapriv->sta_hash_lock); + _rtw_spinlock_free(&pstapriv->wakeup_q.lock); + _rtw_spinlock_free(&pstapriv->sleep_q.lock); + +#ifdef CONFIG_AP_MODE + _rtw_spinlock_free(&pstapriv->asoc_list_lock); + _rtw_spinlock_free(&pstapriv->auth_list_lock); +#endif + +} + +u32 _rtw_free_sta_priv(struct sta_priv *pstapriv) +{ + _irqL irqL; + _list *phead, *plist; + struct sta_info *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + int index; + + if (pstapriv) { + + /* delete all reordering_ctrl_timer */ + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (index = 0; index < NUM_STA; index++) { + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + int i; + psta = LIST_CONTAINOR(plist, struct sta_info , hash_list); + plist = get_next(plist); + + for (i = 0; i < 16 ; i++) { + preorder_ctrl = &psta->recvreorder_ctrl[i]; + _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer); + } + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + /*===============================*/ + + rtw_mfree_sta_priv_lock(pstapriv); + +#if CONFIG_RTW_MACADDR_ACL + for (index = 0; index < RTW_ACL_PERIOD_NUM; index++) + rtw_macaddr_acl_deinit(pstapriv->padapter, index); +#endif + +#if CONFIG_RTW_PRE_LINK_STA + rtw_pre_link_sta_ctl_deinit(pstapriv); +#endif + + if (pstapriv->pallocated_stainfo_buf) + rtw_vmfree(pstapriv->pallocated_stainfo_buf, + sizeof(struct sta_info) * NUM_STA + MEM_ALIGNMENT_OFFSET); + #ifdef CONFIG_AP_MODE + if (pstapriv->sta_aid) + rtw_mfree(pstapriv->sta_aid, pstapriv->max_aid * sizeof(struct sta_info *)); + if (pstapriv->sta_dz_bitmap) + rtw_mfree(pstapriv->sta_dz_bitmap, pstapriv->aid_bmp_len); + if (pstapriv->tim_bitmap) + rtw_mfree(pstapriv->tim_bitmap, pstapriv->aid_bmp_len); + #endif + } + + return _SUCCESS; +} + + +static void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl) +{ + _adapter *padapter = preorder_ctrl->padapter; + +#if defined(CONFIG_80211N_HT) && defined(CONFIG_RECV_REORDERING_CTRL) + rtw_init_timer(&(preorder_ctrl->reordering_ctrl_timer), padapter, rtw_reordering_ctrl_timeout_handler, preorder_ctrl); +#endif +} + +/* struct sta_info *rtw_alloc_stainfo(_queue *pfree_sta_queue, unsigned char *hwaddr) */ +struct sta_info *rtw_alloc_stainfo(struct sta_priv *pstapriv, const u8 *hwaddr) +{ + _irqL irqL2; + s32 index; + _list *phash_list; + struct sta_info *psta; + _queue *pfree_sta_queue; + struct recv_reorder_ctrl *preorder_ctrl; + int i = 0; + u16 wRxSeqInitialValue = 0xffff; + + + pfree_sta_queue = &pstapriv->free_sta_queue; + + /* _enter_critical_bh(&(pfree_sta_queue->lock), &irqL); */ + _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); + if (_rtw_queue_empty(pfree_sta_queue) == _TRUE) { + /* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL); */ + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */ + psta = NULL; + } else { + psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list); + + rtw_list_delete(&(psta->list)); + + /* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL); */ + _rtw_init_stainfo(psta); + + psta->padapter = pstapriv->padapter; + + _rtw_memcpy(psta->cmn.mac_addr, hwaddr, ETH_ALEN); + + index = wifi_mac_hash(hwaddr); + + + if (index >= NUM_STA) { + psta = NULL; + goto exit; + } + phash_list = &(pstapriv->sta_hash[index]); + + /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */ + + rtw_list_insert_tail(&psta->hash_list, phash_list); + + pstapriv->asoc_sta_count++; + + /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */ + + /* Commented by Albert 2009/08/13 + * For the SMC router, the sequence number of first packet of WPS handshake will be 0. + * In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable. + * So, we initialize the tid_rxseq variable as the 0xffff. */ + + for (i = 0; i < 16; i++) { + _rtw_memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2); + _rtw_memcpy(&psta->sta_recvpriv.bmc_tid_rxseq[i], &wRxSeqInitialValue, 2); + _rtw_memset(&psta->sta_recvpriv.rxcache.iv[i], 0, sizeof(psta->sta_recvpriv.rxcache.iv[i])); + } + _rtw_memcpy(&psta->sta_recvpriv.nonqos_bmc_rxseq,&wRxSeqInitialValue,2); + _rtw_memcpy(&psta->sta_recvpriv.nonqos_rxseq,&wRxSeqInitialValue,2); + + rtw_init_timer(&psta->addba_retry_timer, psta->padapter, addba_timer_hdl, psta); +#ifdef CONFIG_IEEE80211W + rtw_init_timer(&psta->dot11w_expire_timer, psta->padapter, sa_query_timer_hdl, psta); +#endif /* CONFIG_IEEE80211W */ +#ifdef CONFIG_TDLS + rtw_init_tdls_timer(pstapriv->padapter, psta); +#endif /* CONFIG_TDLS */ + + /* for A-MPDU Rx reordering buffer control */ + for (i = 0; i < 16 ; i++) { + preorder_ctrl = &psta->recvreorder_ctrl[i]; + preorder_ctrl->padapter = pstapriv->padapter; + preorder_ctrl->tid = i; + preorder_ctrl->enable = _FALSE; + preorder_ctrl->indicate_seq = 0xffff; + #ifdef DBG_RX_SEQ + RTW_INFO("DBG_RX_SEQ "FUNC_ADPT_FMT" tid:%u SN_CLEAR indicate_seq:%d\n" + , FUNC_ADPT_ARG(pstapriv->padapter), i, preorder_ctrl->indicate_seq); + #endif + preorder_ctrl->wend_b = 0xffff; + preorder_ctrl->wsize_b = 64;/* 64; */ + preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID; + + _rtw_init_queue(&preorder_ctrl->pending_recvframe_queue); + + rtw_init_recv_timer(preorder_ctrl); + rtw_clear_bit(RTW_RECV_ACK_OR_TIMEOUT, &preorder_ctrl->rec_abba_rsp_ack); + + } + ATOMIC_SET(&psta->keytrack, 0); + + /* init for DM */ + psta->cmn.rssi_stat.rssi = (-1); + psta->cmn.rssi_stat.rssi_cck = (-1); + psta->cmn.rssi_stat.rssi_ofdm = (-1); +#ifdef CONFIG_ATMEL_RC_PATCH + psta->flag_atmel_rc = 0; +#endif + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT + psta->tbtx_enable = _FALSE; +#endif + /* init for the sequence number of received management frame */ + psta->RxMgmtFrameSeqNum = 0xffff; + _rtw_memset(&psta->sta_stats, 0, sizeof(struct stainfo_stats)); + + rtw_alloc_macid(pstapriv->padapter, psta); + + psta->tx_q_enable = 0; + _rtw_init_queue(&psta->tx_queue); + _init_workitem(&psta->tx_q_work, rtw_xmit_dequeue_callback, NULL); + } + +exit: + + _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); + + + if (psta) + rtw_mi_update_iface_status(&(pstapriv->padapter->mlmepriv), 0); + + return psta; +} + + +/* using pstapriv->sta_hash_lock to protect */ +u32 rtw_free_stainfo(_adapter *padapter , struct sta_info *psta) +{ + int i; + _irqL irqL0; + _queue *pfree_sta_queue, *pdefrag_q = NULL; + struct recv_reorder_ctrl *preorder_ctrl; + struct sta_xmit_priv *pstaxmitpriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct hw_xmit *phwxmit; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + int pending_qcnt[4]; + u8 is_pre_link_sta = _FALSE; + _list *phead, *plist; + _queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue; + union recv_frame *prframe; + + if (psta == NULL) + goto exit; + +#ifdef CONFIG_RTW_80211K + rm_post_event(padapter, RM_ID_FOR_ALL(psta->cmn.aid), RM_EV_cancel); +#endif + + is_pre_link_sta = rtw_is_pre_link_sta(pstapriv, psta->cmn.mac_addr); + + if (is_pre_link_sta == _FALSE) { + _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL0); + rtw_list_delete(&psta->hash_list); + pstapriv->asoc_sta_count--; + _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL0); + rtw_mi_update_iface_status(&(padapter->mlmepriv), 0); + } else { + _enter_critical_bh(&psta->lock, &irqL0); + psta->state = WIFI_FW_PRE_LINK; + _exit_critical_bh(&psta->lock, &irqL0); + } + + _enter_critical_bh(&psta->lock, &irqL0); + psta->state &= ~WIFI_ASOC_STATE; + _exit_critical_bh(&psta->lock, &irqL0); + + pfree_sta_queue = &pstapriv->free_sta_queue; + + + pstaxmitpriv = &psta->sta_xmitpriv; + + /* rtw_list_delete(&psta->sleep_list); */ + + /* rtw_list_delete(&psta->wakeup_list); */ + + rtw_free_xmitframe_queue(pxmitpriv, &psta->tx_queue); + _rtw_deinit_queue(&psta->tx_queue); + + _enter_critical_bh(&pxmitpriv->lock, &irqL0); + + rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q); + psta->sleepq_len = 0; + +#ifdef CONFIG_RTW_MGMT_QUEUE + rtw_free_mgmt_xmitframe_queue(pxmitpriv, &psta->mgmt_sleep_q); + psta->mgmt_sleepq_len = 0; +#endif + + /* vo */ + /* _enter_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0); */ + rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending)); + phwxmit = pxmitpriv->hwxmits; + phwxmit->accnt -= pstaxmitpriv->vo_q.qcnt; + pending_qcnt[0] = pstaxmitpriv->vo_q.qcnt; + pstaxmitpriv->vo_q.qcnt = 0; + /* _exit_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0); */ + + /* vi */ + /* _enter_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0); */ + rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending)); + phwxmit = pxmitpriv->hwxmits + 1; + phwxmit->accnt -= pstaxmitpriv->vi_q.qcnt; + pending_qcnt[1] = pstaxmitpriv->vi_q.qcnt; + pstaxmitpriv->vi_q.qcnt = 0; + /* _exit_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0); */ + + /* be */ + /* _enter_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0); */ + rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending)); + phwxmit = pxmitpriv->hwxmits + 2; + phwxmit->accnt -= pstaxmitpriv->be_q.qcnt; + pending_qcnt[2] = pstaxmitpriv->be_q.qcnt; + pstaxmitpriv->be_q.qcnt = 0; + /* _exit_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0); */ + + /* bk */ + /* _enter_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0); */ + rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending)); + phwxmit = pxmitpriv->hwxmits + 3; + phwxmit->accnt -= pstaxmitpriv->bk_q.qcnt; + pending_qcnt[3] = pstaxmitpriv->bk_q.qcnt; + pstaxmitpriv->bk_q.qcnt = 0; + /* _exit_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0); */ + +#ifdef CONFIG_RTW_MGMT_QUEUE + /* mgmt */ + rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->mgmt_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->mgmt_q.tx_pending)); + phwxmit = pxmitpriv->hwxmits + 4; + phwxmit->accnt -= pstaxmitpriv->mgmt_q.qcnt; + pstaxmitpriv->mgmt_q.qcnt = 0; +#endif + + rtw_os_wake_queue_at_free_stainfo(padapter, pending_qcnt); + + _exit_critical_bh(&pxmitpriv->lock, &irqL0); + + + /* re-init sta_info; 20061114 */ /* will be init in alloc_stainfo */ + /* _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); */ + /* _rtw_init_sta_recv_priv(&psta->sta_recvpriv); */ +#ifdef CONFIG_IEEE80211W + _cancel_timer_ex(&psta->dot11w_expire_timer); +#endif /* CONFIG_IEEE80211W */ + _cancel_timer_ex(&psta->addba_retry_timer); + +#ifdef CONFIG_TDLS + psta->tdls_sta_state = TDLS_STATE_NONE; +#endif /* CONFIG_TDLS */ + + /* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */ + for (i = 0; i < 16 ; i++) { + _irqL irqL; + _queue *ppending_recvframe_queue; + + preorder_ctrl = &psta->recvreorder_ctrl[i]; + rtw_clear_bit(RTW_RECV_ACK_OR_TIMEOUT, &preorder_ctrl->rec_abba_rsp_ack); + + _cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer); + + + ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue; + + _enter_critical_bh(&ppending_recvframe_queue->lock, &irqL); + + phead = get_list_head(ppending_recvframe_queue); + plist = get_next(phead); + + while (!rtw_is_list_empty(phead)) { + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + + plist = get_next(plist); + + rtw_list_delete(&(prframe->u.hdr.list)); + + rtw_free_recvframe(prframe, pfree_recv_queue); + } + + _exit_critical_bh(&ppending_recvframe_queue->lock, &irqL); + + } + + /* CVE-2020-24586, clear defrag queue */ + pdefrag_q = &psta->sta_recvpriv.defrag_q; + enter_critical_bh(&pdefrag_q->lock); + phead = get_list_head(pdefrag_q); + plist = get_next(phead); + while (!rtw_is_list_empty(phead)) { + prframe = LIST_CONTAINOR(plist, union recv_frame, u); + plist = get_next(plist); + rtw_list_delete(&(prframe->u.hdr.list)); + rtw_free_recvframe(prframe, pfree_recv_queue); + } + exit_critical_bh(&pdefrag_q->lock); + + + if (!((psta->state & WIFI_AP_STATE) || MacAddr_isBcst(psta->cmn.mac_addr)) && is_pre_link_sta == _FALSE) + rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _FALSE); + + + /* release mac id for non-bc/mc station, */ + if (is_pre_link_sta == _FALSE) + rtw_release_macid(pstapriv->padapter, psta); + +#ifdef CONFIG_AP_MODE + + /* + _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL0); + rtw_list_delete(&psta->asoc_list); + _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL0); + */ + _enter_critical_bh(&pstapriv->auth_list_lock, &irqL0); + if (!rtw_is_list_empty(&psta->auth_list)) { + rtw_list_delete(&psta->auth_list); + pstapriv->auth_list_cnt--; + } + _exit_critical_bh(&pstapriv->auth_list_lock, &irqL0); + + psta->expire_to = 0; +#ifdef CONFIG_ATMEL_RC_PATCH + psta->flag_atmel_rc = 0; +#endif + psta->sleepq_ac_len = 0; + psta->qos_info = 0; + + psta->max_sp_len = 0; + psta->uapsd_bk = 0; + psta->uapsd_be = 0; + psta->uapsd_vi = 0; + psta->uapsd_vo = 0; + + psta->has_legacy_ac = 0; + +#ifdef CONFIG_NATIVEAP_MLME + + if (pmlmeinfo->state == _HW_STATE_AP_) { + rtw_tim_map_clear(padapter, pstapriv->sta_dz_bitmap, psta->cmn.aid); + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + /* rtw_indicate_sta_disassoc_event(padapter, psta); */ + + if ((psta->cmn.aid > 0) && (pstapriv->sta_aid[psta->cmn.aid - 1] == psta)) { + pstapriv->sta_aid[psta->cmn.aid - 1] = NULL; + psta->cmn.aid = 0; + } + } + +#endif /* CONFIG_NATIVEAP_MLME */ + +#if !defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && defined(CONFIG_80211N_HT) + psta->under_exist_checking = 0; +#endif + +#endif /* CONFIG_AP_MODE */ + + rtw_st_ctl_deinit(&psta->st_ctl); + + if (is_pre_link_sta == _FALSE) { + _rtw_spinlock_free(&psta->lock); + + /* _enter_critical_bh(&(pfree_sta_queue->lock), &irqL0); */ + _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL0); + rtw_list_insert_tail(&psta->list, get_list_head(pfree_sta_queue)); + _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL0); + /* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL0); */ + } + +exit: + return _SUCCESS; +} + +/* free all stainfo which in sta_hash[all] */ +void rtw_free_all_stainfo(_adapter *padapter) +{ + _irqL irqL; + _list *plist, *phead; + s32 index; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter); + u8 free_sta_num = 0; + char free_sta_list[NUM_STA]; + int stainfo_offset; + + + if (pstapriv->asoc_sta_count == 1) + goto exit; + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (index = 0; index < NUM_STA; index++) { + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info , hash_list); + + plist = get_next(plist); + + if (pbcmc_stainfo != psta) { + if (rtw_is_pre_link_sta(pstapriv, psta->cmn.mac_addr) == _FALSE) + rtw_list_delete(&psta->hash_list); + + stainfo_offset = rtw_stainfo_offset(pstapriv, psta); + if (stainfo_offset_valid(stainfo_offset)) + free_sta_list[free_sta_num++] = stainfo_offset; + } + + } + } + + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + + for (index = 0; index < free_sta_num; index++) { + psta = rtw_get_stainfo_by_offset(pstapriv, free_sta_list[index]); + rtw_free_stainfo(padapter , psta); + } + +exit: + return; +} + +/* any station allocated can be searched by hash list */ +struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, const u8 *hwaddr) +{ + + _irqL irqL; + + _list *plist, *phead; + + struct sta_info *psta = NULL; + + u32 index; + + const u8 *addr; + + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + + if (hwaddr == NULL) + return NULL; + + if (IS_MCAST(hwaddr)) + addr = bc_addr; + else + addr = hwaddr; + + index = wifi_mac_hash(addr); + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + if ((_rtw_memcmp(psta->cmn.mac_addr, addr, ETH_ALEN)) == _TRUE) { + /* if found the matched address */ + break; + } + psta = NULL; + plist = get_next(plist); + } + + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + return psta; + +} + +u32 rtw_init_bcmc_stainfo(_adapter *padapter) +{ + + struct sta_info *psta; + struct tx_servq *ptxservq; + u32 res = _SUCCESS; + NDIS_802_11_MAC_ADDRESS bcast_addr = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + struct sta_priv *pstapriv = &padapter->stapriv; + + + psta = rtw_alloc_stainfo(pstapriv, bcast_addr); + + if (psta == NULL) { + res = _FAIL; + goto exit; + } +#ifdef CONFIG_BEAMFORMING + psta->cmn.bf_info.g_id = 63; + psta->cmn.bf_info.p_aid = 0; +#endif + + ptxservq = &(psta->sta_xmitpriv.be_q); + + /* + _enter_critical(&pstapending->lock, &irqL0); + + if (rtw_is_list_empty(&ptxservq->tx_pending)) + rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(pstapending)); + + _exit_critical(&pstapending->lock, &irqL0); + */ + +exit: + return _SUCCESS; + +} + + +struct sta_info *rtw_get_bcmc_stainfo(_adapter *padapter) +{ + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + psta = rtw_get_stainfo(pstapriv, bc_addr); + return psta; + +} + +#ifdef CONFIG_AP_MODE +u16 rtw_aid_alloc(_adapter *adapter, struct sta_info *sta) +{ + struct sta_priv *stapriv = &adapter->stapriv; + u16 aid, i, used_cnt = 0; + + for (i = 0; i < stapriv->max_aid; i++) { + aid = ((i + stapriv->started_aid - 1) % stapriv->max_aid) + 1; + if (stapriv->sta_aid[aid - 1] == NULL) + break; + if (++used_cnt >= stapriv->max_num_sta) + break; + } + + /* check for aid limit and assoc limit */ + if (i >= stapriv->max_aid || used_cnt >= stapriv->max_num_sta) + aid = 0; + + sta->cmn.aid = aid; + if (aid) { + stapriv->sta_aid[aid - 1] = sta; + if (stapriv->rr_aid) + stapriv->started_aid = (aid % stapriv->max_aid) + 1; + } + + return aid; +} + +void dump_aid_status(void *sel, _adapter *adapter) +{ + struct sta_priv *stapriv = &adapter->stapriv; + u8 *aid_bmp; + u16 i, used_cnt = 0; + + aid_bmp = rtw_zmalloc(stapriv->aid_bmp_len); + if (!aid_bmp) + return; + + for (i = 1; i <= stapriv->max_aid; i++) { + if (stapriv->sta_aid[i - 1]) { + aid_bmp[i / 8] |= BIT(i % 8); + ++used_cnt; + } + } + + RTW_PRINT_SEL(sel, "used_cnt:%u/%u\n", used_cnt, stapriv->max_aid); + RTW_MAP_DUMP_SEL(sel, "aid_map:", aid_bmp, stapriv->aid_bmp_len); + RTW_PRINT_SEL(sel, "\n"); + + RTW_PRINT_SEL(sel, "%-2s %-11s\n", "rr", "started_aid"); + RTW_PRINT_SEL(sel, "%2d %11d\n", stapriv->rr_aid, stapriv->started_aid); + + rtw_mfree(aid_bmp, stapriv->aid_bmp_len); +} +#endif /* CONFIG_AP_MODE */ + +#if CONFIG_RTW_MACADDR_ACL +const char *const _acl_period_str[RTW_ACL_PERIOD_NUM] = { + "DEV", + "BSS", +}; + +const char *const _acl_mode_str[RTW_ACL_MODE_MAX] = { + "DISABLED", + "ACCEPT_UNLESS_LISTED", + "DENY_UNLESS_LISTED", +}; + +u8 _rtw_access_ctrl(_adapter *adapter, u8 period, const u8 *mac_addr) +{ + u8 res = _TRUE; + _irqL irqL; + _list *list, *head; + struct rtw_wlan_acl_node *acl_node; + u8 match = _FALSE; + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + _queue *acl_node_q; + + if (period >= RTW_ACL_PERIOD_NUM) { + rtw_warn_on(1); + goto exit; + } + + acl = &stapriv->acl_list[period]; + acl_node_q = &acl->acl_node_q; + + if (acl->mode != RTW_ACL_MODE_ACCEPT_UNLESS_LISTED + && acl->mode != RTW_ACL_MODE_DENY_UNLESS_LISTED) + goto exit; + + _enter_critical_bh(&(acl_node_q->lock), &irqL); + head = get_list_head(acl_node_q); + list = get_next(head); + while (rtw_end_of_queue_search(head, list) == _FALSE) { + acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list); + list = get_next(list); + + if (_rtw_memcmp(acl_node->addr, mac_addr, ETH_ALEN)) { + if (acl_node->valid == _TRUE) { + match = _TRUE; + break; + } + } + } + _exit_critical_bh(&(acl_node_q->lock), &irqL); + + if (acl->mode == RTW_ACL_MODE_ACCEPT_UNLESS_LISTED) + res = (match == _TRUE) ? _FALSE : _TRUE; + else /* RTW_ACL_MODE_DENY_UNLESS_LISTED */ + res = (match == _TRUE) ? _TRUE : _FALSE; + +exit: + return res; +} + +u8 rtw_access_ctrl(_adapter *adapter, const u8 *mac_addr) +{ + int i; + + for (i = 0; i < RTW_ACL_PERIOD_NUM; i++) + if (_rtw_access_ctrl(adapter, i, mac_addr) == _FALSE) + return _FALSE; + + return _TRUE; +} + +void dump_macaddr_acl(void *sel, _adapter *adapter) +{ + struct sta_priv *stapriv = &adapter->stapriv; + struct wlan_acl_pool *acl; + int i, j; + + for (j = 0; j < RTW_ACL_PERIOD_NUM; j++) { + RTW_PRINT_SEL(sel, "period:%s(%d)\n", acl_period_str(j), j); + + acl = &stapriv->acl_list[j]; + RTW_PRINT_SEL(sel, "mode:%s(%d)\n", acl_mode_str(acl->mode), acl->mode); + RTW_PRINT_SEL(sel, "num:%d/%d\n", acl->num, NUM_ACL); + for (i = 0; i < NUM_ACL; i++) { + if (acl->aclnode[i].valid == _FALSE) + continue; + RTW_PRINT_SEL(sel, MAC_FMT"\n", MAC_ARG(acl->aclnode[i].addr)); + } + RTW_PRINT_SEL(sel, "\n"); + } +} +#endif /* CONFIG_RTW_MACADDR_ACL */ + +bool rtw_is_pre_link_sta(struct sta_priv *stapriv, u8 *addr) +{ +#if CONFIG_RTW_PRE_LINK_STA + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + struct sta_info *sta = NULL; + u8 exist = _FALSE; + int i; + _irqL irqL; + + _enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) { + if (pre_link_sta_ctl->node[i].valid == _TRUE + && _rtw_memcmp(pre_link_sta_ctl->node[i].addr, addr, ETH_ALEN) == _TRUE + ) { + exist = _TRUE; + break; + } + } + _exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + + return exist; +#else + return _FALSE; +#endif +} + +#if CONFIG_RTW_PRE_LINK_STA +struct sta_info *rtw_pre_link_sta_add(struct sta_priv *stapriv, u8 *hwaddr) +{ + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + struct pre_link_sta_node_t *node = NULL; + struct sta_info *sta = NULL; + u8 exist = _FALSE; + int i; + _irqL irqL; + + if (rtw_check_invalid_mac_address(hwaddr, _FALSE) == _TRUE) + goto exit; + + _enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) { + if (pre_link_sta_ctl->node[i].valid == _TRUE + && _rtw_memcmp(pre_link_sta_ctl->node[i].addr, hwaddr, ETH_ALEN) == _TRUE + ) { + node = &pre_link_sta_ctl->node[i]; + exist = _TRUE; + break; + } + + if (node == NULL && pre_link_sta_ctl->node[i].valid == _FALSE) + node = &pre_link_sta_ctl->node[i]; + } + + if (exist == _FALSE && node) { + _rtw_memcpy(node->addr, hwaddr, ETH_ALEN); + node->valid = _TRUE; + pre_link_sta_ctl->num++; + } + _exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + + if (node == NULL) + goto exit; + + sta = rtw_get_stainfo(stapriv, hwaddr); + if (sta) + goto odm_hook; + + sta = rtw_alloc_stainfo(stapriv, hwaddr); + if (!sta) + goto exit; + + sta->state = WIFI_FW_PRE_LINK; + +odm_hook: + rtw_hal_set_odm_var(stapriv->padapter, HAL_ODM_STA_INFO, sta, _TRUE); + +exit: + return sta; +} + +void rtw_pre_link_sta_del(struct sta_priv *stapriv, u8 *hwaddr) +{ + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + struct pre_link_sta_node_t *node = NULL; + struct sta_info *sta = NULL; + u8 exist = _FALSE; + int i; + _irqL irqL; + + if (rtw_check_invalid_mac_address(hwaddr, _FALSE) == _TRUE) + goto exit; + + _enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) { + if (pre_link_sta_ctl->node[i].valid == _TRUE + && _rtw_memcmp(pre_link_sta_ctl->node[i].addr, hwaddr, ETH_ALEN) == _TRUE + ) { + node = &pre_link_sta_ctl->node[i]; + exist = _TRUE; + break; + } + } + + if (exist == _TRUE && node) { + node->valid = _FALSE; + pre_link_sta_ctl->num--; + } + _exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + + if (exist == _FALSE) + goto exit; + + sta = rtw_get_stainfo(stapriv, hwaddr); + if (!sta) + goto exit; + + if (sta->state == WIFI_FW_PRE_LINK) + rtw_free_stainfo(stapriv->padapter, sta); + +exit: + return; +} + +void rtw_pre_link_sta_ctl_reset(struct sta_priv *stapriv) +{ + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + struct pre_link_sta_node_t *node = NULL; + struct sta_info *sta = NULL; + int i, j = 0; + _irqL irqL; + + u8 addrs[RTW_PRE_LINK_STA_NUM][ETH_ALEN]; + + _rtw_memset(addrs, 0, RTW_PRE_LINK_STA_NUM * ETH_ALEN); + + _enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) { + if (pre_link_sta_ctl->node[i].valid == _FALSE) + continue; + _rtw_memcpy(&(addrs[j][0]), pre_link_sta_ctl->node[i].addr, ETH_ALEN); + pre_link_sta_ctl->node[i].valid = _FALSE; + pre_link_sta_ctl->num--; + j++; + } + _exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL); + + for (i = 0; i < j; i++) { + sta = rtw_get_stainfo(stapriv, &(addrs[i][0])); + if (!sta) + continue; + + if (sta->state == WIFI_FW_PRE_LINK) + rtw_free_stainfo(stapriv->padapter, sta); + } +} + +void rtw_pre_link_sta_ctl_init(struct sta_priv *stapriv) +{ + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + int i; + + _rtw_spinlock_init(&pre_link_sta_ctl->lock); + pre_link_sta_ctl->num = 0; + for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) + pre_link_sta_ctl->node[i].valid = _FALSE; +} + +void rtw_pre_link_sta_ctl_deinit(struct sta_priv *stapriv) +{ + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + int i; + + rtw_pre_link_sta_ctl_reset(stapriv); + + _rtw_spinlock_free(&pre_link_sta_ctl->lock); +} + +void dump_pre_link_sta_ctl(void *sel, struct sta_priv *stapriv) +{ + struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl; + int i; + + RTW_PRINT_SEL(sel, "num:%d/%d\n", pre_link_sta_ctl->num, RTW_PRE_LINK_STA_NUM); + + for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) { + if (pre_link_sta_ctl->node[i].valid == _FALSE) + continue; + RTW_PRINT_SEL(sel, MAC_FMT"\n", MAC_ARG(pre_link_sta_ctl->node[i].addr)); + } +} +#endif /* CONFIG_RTW_PRE_LINK_STA */ + diff --git a/core/rtw_swcrypto.c b/core/rtw_swcrypto.c new file mode 100644 index 0000000..2f1c343 --- /dev/null +++ b/core/rtw_swcrypto.c @@ -0,0 +1,296 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include + +/** + * rtw_ccmp_encrypt - + * @key: the temporal key + * @hdrlen: mac header length + * @frame: the frame including the mac header, pn and payload + * @plen: payload length, i.e., length of the plain text, without PN and MIC + */ +int _rtw_ccmp_encrypt(_adapter *padapter, u8 *key, u32 key_len, uint hdrlen, u8 *frame, uint plen) +{ + u8 *enc = NULL; + size_t enc_len = 0; + + if (key_len == 16) { /* 128 bits */ + enc = ccmp_encrypt(padapter, key, + frame, + hdrlen + plen, + hdrlen, + (hdrlen == 26) ? (frame + hdrlen - 2) : NULL, + NULL, 0, &enc_len); + } else if (key_len == 32) { /* 256 bits */ + enc = ccmp_256_encrypt(padapter, key, + frame, + hdrlen + plen, + hdrlen, + (hdrlen == 26) ? (frame + hdrlen - 2) : NULL, + NULL, 0, &enc_len); + } + + if (enc == NULL) { + RTW_INFO("Failed to encrypt CCMP(%u) frame", key_len); + return _FAIL; + } + + /* Copy @enc back to @frame and free @enc */ + _rtw_memcpy(frame, enc, enc_len); + rtw_mfree(enc, enc_len + AES_BLOCK_SIZE); + + return _SUCCESS; +} + + +/** + * rtw_ccmp_decrypt - + * @key: the temporal key + * @hdrlen: length of the mac header + * @frame: the raw frame (@hdrlen + PN + enc_data + MIC) + * @plen: length of the frame (@hdrlen + PN + enc_data + MIC) + */ +int _rtw_ccmp_decrypt(_adapter * padapter, u8 *key, u32 key_len, uint hdrlen, u8 *frame, + uint plen) +{ + u8 *plain = NULL; + size_t plain_len = 0; + const struct ieee80211_hdr *hdr; + + hdr = (const struct ieee80211_hdr *)frame; + + if (key_len == 16) { /* 128 bits */ + plain = ccmp_decrypt(padapter, key, + hdr, + frame + hdrlen, /* PN + enc_data + MIC */ + plen - hdrlen, /* PN + enc_data + MIC */ + &plain_len); + } else if (key_len == 32) { /* 256 bits */ + plain = ccmp_256_decrypt(padapter, key, + hdr, + frame + hdrlen, /* PN + enc_data + MIC */ + plen - hdrlen, /* PN + enc_data + MIC */ + &plain_len); + } + + if (plain == NULL) { + RTW_INFO("Failed to decrypt CCMP(%u) frame", key_len); + return _FAIL; + } + + /* Copy @plain back to @frame and free @plain */ + _rtw_memcpy(frame + hdrlen + 8, plain, plain_len); + rtw_mfree(plain, plen - hdrlen + AES_BLOCK_SIZE); + + RTW_DBG_DUMP("ccmp_decrypt(): decrypted frame\n", + frame, hdrlen + 8 + plen); + + return _SUCCESS; +} + + +#ifdef CONFIG_RTW_MESH_AEK +/* wrapper to ase_siv_encrypt and aes_siv_decrypt */ +int _aes_siv_encrypt(const u8 *key, size_t key_len, + const u8 *pw, size_t pwlen, + size_t num_elem, const u8 *addr[], const size_t *len, u8 *out) +{ + return aes_siv_encrypt(key, key_len, pw, pwlen, num_elem, addr, len, out); +} +int _aes_siv_decrypt(const u8 *key, size_t key_len, + const u8 *iv_crypt, size_t iv_c_len, + size_t num_elem, const u8 *addr[], const size_t *len, u8 *out) +{ + return aes_siv_decrypt(key, key_len, iv_crypt, iv_c_len, num_elem, addr, len, out); +} +#endif + + +/** + * _rtw_gcmp_encrypt - + * @key: the temporal key + * @hdrlen: mac header length + * @frame: the frame including the mac header, pn and payload + * @plen: payload length, i.e., length of the plain text, without PN and MIC + */ +int _rtw_gcmp_encrypt(_adapter * padapter, u8 *key, u32 key_len, uint hdrlen, u8 *frame, uint plen) +{ + u8 *enc = NULL; + size_t enc_len = 0; + + enc = gcmp_encrypt(padapter, key, key_len, + frame, + hdrlen + plen, + hdrlen, + (hdrlen == 26) ? (frame + hdrlen - 2) : NULL, + NULL, 0, &enc_len); + if (enc == NULL) { + RTW_INFO("Failed to encrypt GCMP frame"); + return _FAIL; + } + + /* Copy @enc back to @frame and free @enc */ + _rtw_memcpy(frame, enc, enc_len); + rtw_mfree(enc, enc_len + AES_BLOCK_SIZE); + + return _SUCCESS; +} + + +/** + * _rtw_gcmp_decrypt - + * @key: the temporal key + * @hdrlen: length of the mac header + * @frame: the raw frame (@hdrlen + PN + enc_data + MIC) + * @plen: length of the frame (@hdrlen + PN + enc_data + MIC) + */ +int _rtw_gcmp_decrypt(_adapter *padapter, u8 *key, u32 key_len, uint hdrlen, u8 *frame, uint plen) +{ + u8 *plain = NULL; + size_t plain_len = 0; + const struct ieee80211_hdr *hdr; + + hdr = (const struct ieee80211_hdr *)frame; + + plain = gcmp_decrypt(padapter, key, key_len, + hdr, + frame + hdrlen, /* PN + enc_data + MIC */ + plen - hdrlen, /* PN + enc_data + MIC */ + &plain_len); + + if (plain == NULL) { + RTW_INFO("Failed to decrypt GCMP(%u) frame", key_len); + return _FAIL; + } + + /* Copy @plain back to @frame and free @plain */ + _rtw_memcpy(frame + hdrlen + 8, plain, plain_len); + rtw_mfree(plain, plen - hdrlen + AES_BLOCK_SIZE); + + RTW_DBG_DUMP("gcmp_decipher(): decrypted frame\n", + frame, hdrlen + 8 + plen); + + return _SUCCESS; +} + + +#if defined(CONFIG_IEEE80211W) | defined(CONFIG_TDLS) +u8 _bip_ccmp_protect(const u8 *key, size_t key_len, + const u8 *data, size_t data_len, u8 *mic) +{ + u8 res = _SUCCESS; + + if (key_len == 16) { + if (omac1_aes_128(key, data, data_len, mic)) { + res = _FAIL; + RTW_ERR("%s : omac1_aes_128 fail!", __func__); + } + } else if (key_len == 32) { + if (omac1_aes_256(key, data, data_len, mic)) { + res = _FAIL; + RTW_ERR("%s : omac1_aes_256 fail!", __func__); + } + } else { + RTW_ERR("%s : key_len not match!", __func__); + res = _FAIL; + } + + return res; +} + + +u8 _bip_gcmp_protect(u8 *whdr_pos, size_t len, + const u8 *key, size_t key_len, + const u8 *data, size_t data_len, u8 *mic) +{ + u8 res = _SUCCESS; + u32 mic_len = 16; + u8 nonce[12], *npos; + const u8 *gcmp_ipn; + + gcmp_ipn = whdr_pos + len - mic_len - 6; + + /* Nonce: A2 | IPN */ + _rtw_memcpy(nonce, get_addr2_ptr(whdr_pos), ETH_ALEN); + npos = nonce + ETH_ALEN; + *npos++ = gcmp_ipn[5]; + *npos++ = gcmp_ipn[4]; + *npos++ = gcmp_ipn[3]; + *npos++ = gcmp_ipn[2]; + *npos++ = gcmp_ipn[1]; + *npos++ = gcmp_ipn[0]; + + if (aes_gmac(key, key_len, nonce, sizeof(nonce), + data, data_len, mic)) { + res = _FAIL; + RTW_ERR("%s : aes_gmac fail!", __func__); + } + + return res; +} +#endif /* CONFIG_IEEE80211W */ + + +#ifdef CONFIG_TDLS +void _tdls_generate_tpk(void *sta, const u8 *own_addr, const u8 *bssid) +{ + struct sta_info *psta = (struct sta_info *)sta; + u8 *SNonce = psta->SNonce; + u8 *ANonce = psta->ANonce; + + u8 key_input[SHA256_MAC_LEN]; + const u8 *nonce[2]; + size_t len[2]; + u8 data[3 * ETH_ALEN]; + + /* IEEE Std 802.11z-2010 8.5.9.1: + * TPK-Key-Input = SHA-256(min(SNonce, ANonce) || max(SNonce, ANonce)) + */ + len[0] = 32; + len[1] = 32; + if (_rtw_memcmp2(SNonce, ANonce, 32) < 0) { + nonce[0] = SNonce; + nonce[1] = ANonce; + } else { + nonce[0] = ANonce; + nonce[1] = SNonce; + } + + sha256_vector(2, nonce, len, key_input); + + /* + * TPK = KDF-Hash-Length(TPK-Key-Input, "TDLS PMK", + * min(MAC_I, MAC_R) || max(MAC_I, MAC_R) || BSSID) + */ + + if (_rtw_memcmp2(own_addr, psta->cmn.mac_addr, ETH_ALEN) < 0) { + _rtw_memcpy(data, own_addr, ETH_ALEN); + _rtw_memcpy(data + ETH_ALEN, psta->cmn.mac_addr, ETH_ALEN); + } else { + _rtw_memcpy(data, psta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(data + ETH_ALEN, own_addr, ETH_ALEN); + } + + _rtw_memcpy(data + 2 * ETH_ALEN, bssid, ETH_ALEN); + + sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data), (u8 *)&psta->tpk, sizeof(psta->tpk)); +} +#endif /* CONFIG_TDLS */ diff --git a/core/rtw_tdls.c b/core/rtw_tdls.c new file mode 100644 index 0000000..d557234 --- /dev/null +++ b/core/rtw_tdls.c @@ -0,0 +1,3516 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_TDLS_C_ + +#include +#include + +#ifdef CONFIG_TDLS +#define ONE_SEC 1000 /* 1000 ms */ + +extern unsigned char MCS_rate_2R[16]; +extern unsigned char MCS_rate_1R[16]; + +inline void rtw_tdls_set_link_established(_adapter *adapter, bool en) +{ + adapter->tdlsinfo.link_established = en; + rtw_mi_update_iface_status(&(adapter->mlmepriv), 0); +} + +void rtw_reset_tdls_info(_adapter *padapter) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + + ptdlsinfo->ap_prohibited = _FALSE; + + /* For TDLS channel switch, currently we only allow it to work in wifi logo test mode */ + if (padapter->registrypriv.wifi_spec == 1) + ptdlsinfo->ch_switch_prohibited = _FALSE; + else + ptdlsinfo->ch_switch_prohibited = _TRUE; + + rtw_tdls_set_link_established(padapter, _FALSE); + ptdlsinfo->sta_cnt = 0; + ptdlsinfo->sta_maximum = _FALSE; + +#ifdef CONFIG_TDLS_CH_SW + ptdlsinfo->chsw_info.ch_sw_state = TDLS_STATE_NONE; + ATOMIC_SET(&ptdlsinfo->chsw_info.chsw_on, _FALSE); + ptdlsinfo->chsw_info.off_ch_num = 0; + ptdlsinfo->chsw_info.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + ptdlsinfo->chsw_info.cur_time = 0; + ptdlsinfo->chsw_info.delay_switch_back = _FALSE; + ptdlsinfo->chsw_info.dump_stack = _FALSE; +#endif + + ptdlsinfo->ch_sensing = 0; + ptdlsinfo->watchdog_count = 0; + ptdlsinfo->dev_discovered = _FALSE; + +#ifdef CONFIG_WFD + ptdlsinfo->wfd_info = &padapter->wfd_info; +#endif + + ptdlsinfo->tdls_sctx = NULL; +} + +int rtw_init_tdls_info(_adapter *padapter) +{ + int res = _SUCCESS; + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + + rtw_reset_tdls_info(padapter); + +#ifdef CONFIG_TDLS_DRIVER_SETUP + ptdlsinfo->driver_setup = _TRUE; +#else + ptdlsinfo->driver_setup = _FALSE; +#endif /* CONFIG_TDLS_DRIVER_SETUP */ + + _rtw_spinlock_init(&ptdlsinfo->cmd_lock); + _rtw_spinlock_init(&ptdlsinfo->hdl_lock); + + return res; + +} + +void rtw_free_tdls_info(struct tdls_info *ptdlsinfo) +{ + _rtw_spinlock_free(&ptdlsinfo->cmd_lock); + _rtw_spinlock_free(&ptdlsinfo->hdl_lock); + + _rtw_memset(ptdlsinfo, 0, sizeof(struct tdls_info)); + +} + +void rtw_free_all_tdls_sta(_adapter *padapter, u8 enqueue_cmd) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + _irqL irqL; + _list *plist, *phead; + s32 index; + struct sta_info *psta = NULL; + struct sta_info *ptdls_sta[NUM_STA]; + u8 empty_hwaddr[ETH_ALEN] = { 0x00 }; + + _rtw_memset(ptdls_sta, 0x00, sizeof(ptdls_sta)); + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + for (index = 0; index < NUM_STA; index++) { + phead = &(pstapriv->sta_hash[index]); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + + plist = get_next(plist); + + if (psta->tdls_sta_state != TDLS_STATE_NONE) + ptdls_sta[index] = psta; + } + } + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (index = 0; index < NUM_STA; index++) { + if (ptdls_sta[index]) { + struct TDLSoption_param tdls_param; + + psta = ptdls_sta[index]; + + RTW_INFO("Do tear down to "MAC_FMT" by enqueue_cmd = %d\n", MAC_ARG(psta->cmn.mac_addr), enqueue_cmd); + + _rtw_memcpy(&(tdls_param.addr), psta->cmn.mac_addr, ETH_ALEN); + tdls_param.option = TDLS_TEARDOWN_STA_NO_WAIT; + tdls_hdl(padapter, (unsigned char *)&(tdls_param)); + + rtw_tdls_teardown_pre_hdl(padapter, psta); + + if (enqueue_cmd == _TRUE) + rtw_tdls_cmd(padapter, psta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + else + { + tdls_param.option = TDLS_TEARDOWN_STA_LOCALLY_POST; + tdls_hdl(padapter, (unsigned char *)&(tdls_param)); + } + } + } +} + +int check_ap_tdls_prohibited(u8 *pframe, u8 pkt_len) +{ + u8 tdls_prohibited_bit = 0x40; /* bit(38); TDLS_prohibited */ + + if (pkt_len < 5) + return _FALSE; + + pframe += 4; + if ((*pframe) & tdls_prohibited_bit) + return _TRUE; + + return _FALSE; +} + +int check_ap_tdls_ch_switching_prohibited(u8 *pframe, u8 pkt_len) +{ + u8 tdls_ch_swithcing_prohibited_bit = 0x80; /* bit(39); TDLS_channel_switching prohibited */ + + if (pkt_len < 5) + return _FALSE; + + pframe += 4; + if ((*pframe) & tdls_ch_swithcing_prohibited_bit) + return _TRUE; + + return _FALSE; +} + +u8 rtw_is_tdls_enabled(_adapter *padapter) +{ + return padapter->registrypriv.en_tdls; +} + +void rtw_set_tdls_enable(_adapter *padapter, u8 enable) +{ + padapter->registrypriv.en_tdls = enable; + RTW_INFO("%s: en_tdls = %d\n", __func__, rtw_is_tdls_enabled(padapter)); +} + +void rtw_enable_tdls_func(_adapter *padapter) +{ + if (rtw_is_tdls_enabled(padapter) == _TRUE) + return; + +#if 0 +#ifdef CONFIG_MCC_MODE + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC) == _TRUE) { + RTW_INFO("[TDLS] MCC is running, can't enable TDLS !\n"); + return; + } +#endif +#endif + rtw_set_tdls_enable(padapter, _TRUE); +} + +void rtw_disable_tdls_func(_adapter *padapter, u8 enqueue_cmd) +{ + if (rtw_is_tdls_enabled(padapter) == _FALSE) + return; + + rtw_free_all_tdls_sta(padapter, enqueue_cmd); + rtw_tdls_cmd(padapter, NULL, TDLS_RS_RCR); + rtw_reset_tdls_info(padapter); + + rtw_set_tdls_enable(padapter, _FALSE); +} + +u8 rtw_is_tdls_sta_existed(_adapter *padapter) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + int i = 0; + _irqL irqL; + _list *plist, *phead; + u8 ret = _FALSE; + + if (rtw_is_tdls_enabled(padapter) == _FALSE) + return _FALSE; + + _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + for (i = 0; i < NUM_STA; i++) { + phead = &(pstapriv->sta_hash[i]); + plist = get_next(phead); + while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) { + psta = LIST_CONTAINOR(plist, struct sta_info, hash_list); + plist = get_next(plist); + if (psta->tdls_sta_state != TDLS_STATE_NONE) { + ret = _TRUE; + goto Exit; + } + } + } + +Exit: + + _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL); + + return ret; +} + +u8 rtw_tdls_is_setup_allowed(_adapter *padapter) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + + if (is_client_associated_to_ap(padapter) == _FALSE) + return _FALSE; + + if (ptdlsinfo->ap_prohibited == _TRUE) + return _FALSE; + + return _TRUE; +} + +#ifdef CONFIG_TDLS_CH_SW +u8 rtw_tdls_is_chsw_allowed(_adapter *padapter) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + + if (ptdlsinfo->ch_switch_prohibited == _TRUE) + return _FALSE; + + if (padapter->registrypriv.wifi_spec == 0) + return _FALSE; + + return _TRUE; +} +#endif + +int _issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ms) +{ + int ret = _FAIL; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl, *qc; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + pattrib->hdrlen += 2; + pattrib->qos_en = _TRUE; + pattrib->eosp = 1; + pattrib->ack_policy = 0; + pattrib->mdata = 0; + pattrib->retry_ctrl = _FALSE; + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + if (power_mode) + SetPwrMgt(fctrl); + + qc = (unsigned short *)(pframe + pattrib->hdrlen - 2); + + SetPriority(qc, 7); /* Set priority to VO */ + + SetEOSP(qc, pattrib->eosp); + + SetAckpolicy(qc, pattrib->ack_policy); + + _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_QOS_DATA_NULL); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos); + + pattrib->last_txcmdsz = pattrib->pktlen; + + if (wait_ms) + ret = dump_mgntframe_and_wait_ack_timeout(padapter, pmgntframe, wait_ms); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + return ret; + +} + +/* + *wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT + *wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX + *try_cnt means the maximal TX count to try + */ +int issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms) +{ + int ret; + int i = 0; + systime start = rtw_get_current_time(); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + +#if 0 + psta = rtw_get_stainfo(&padapter->stapriv, da); + if (psta) { + if (power_mode) + rtw_hal_macid_sleep(padapter, psta->cmn.mac_id); + else + rtw_hal_macid_wakeup(padapter, psta->cmn.mac_id); + } else { + RTW_INFO(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode ? "sleep" : "wakeup"); + rtw_warn_on(1); + } +#endif + + do { + ret = _issue_nulldata_to_TDLS_peer_STA(padapter, da, power_mode, wait_ms); + + i++; + + if (RTW_CANNOT_RUN(padapter)) + break; + + if (i < try_cnt && wait_ms > 0 && ret == _FAIL) + rtw_msleep_os(wait_ms); + + } while ((i < try_cnt) && (ret == _FAIL || wait_ms == 0)); + + if (ret != _FAIL) { + ret = _SUCCESS; +#ifndef DBG_XMIT_ACK + goto exit; +#endif + } + + if (try_cnt && wait_ms) { + if (da) + RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + else + RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n", + FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter), + ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start)); + } +exit: + return ret; +} + +/* TDLS encryption(if needed) will always be CCMP */ +void rtw_tdls_set_key(_adapter *padapter, struct sta_info *ptdls_sta) +{ + ptdls_sta->dot118021XPrivacy = _AES_; + rtw_setstakey_cmd(padapter, ptdls_sta, TDLS_KEY, _TRUE); +} + +#ifdef CONFIG_80211N_HT +void rtw_tdls_process_ht_cap(_adapter *padapter, struct sta_info *ptdls_sta, u8 *data, u8 Length) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + u8 max_AMPDU_len, min_MPDU_spacing; + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0; + + /* Save HT capabilities in the sta object */ + _rtw_memset(&ptdls_sta->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap)); + if (data && Length >= sizeof(struct rtw_ieee80211_ht_cap)) { + ptdls_sta->flags |= WLAN_STA_HT; + ptdls_sta->flags |= WLAN_STA_WME; + + _rtw_memcpy(&ptdls_sta->htpriv.ht_cap, data, sizeof(struct rtw_ieee80211_ht_cap)); + } else { + ptdls_sta->flags &= ~WLAN_STA_HT; + return; + } + + if (ptdls_sta->flags & WLAN_STA_HT) { + if (padapter->registrypriv.ht_enable == _TRUE && is_supported_ht(padapter->registrypriv.wireless_mode) ) { + ptdls_sta->htpriv.ht_option = _TRUE; + ptdls_sta->qos_option = _TRUE; + } else { + ptdls_sta->htpriv.ht_option = _FALSE; + ptdls_sta->qos_option = _FALSE; + } + } + + /* HT related cap */ + if (ptdls_sta->htpriv.ht_option) { + /* Check if sta supports rx ampdu */ + if (padapter->registrypriv.ampdu_enable == 1) + ptdls_sta->htpriv.ampdu_enable = _TRUE; + + /* AMPDU Parameters field */ + /* Get MIN of MAX AMPDU Length Exp */ + if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (data[2] & 0x3)) + max_AMPDU_len = (data[2] & 0x3); + else + max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3); + /* Get MAX of MIN MPDU Start Spacing */ + if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (data[2] & 0x1c)) + min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c); + else + min_MPDU_spacing = (data[2] & 0x1c); + ptdls_sta->htpriv.rx_ampdu_min_spacing = max_AMPDU_len | min_MPDU_spacing; + + /* Check if sta support s Short GI 20M */ + if ((phtpriv->sgi_20m == _TRUE) && (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))) + ptdls_sta->htpriv.sgi_20m = _TRUE; + + /* Check if sta support s Short GI 40M */ + if ((phtpriv->sgi_40m == _TRUE) && (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_40))) + ptdls_sta->htpriv.sgi_40m = _TRUE; + + /* Bwmode would still followed AP's setting */ + if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH)) { + if (padapter->mlmeextpriv.cur_bwmode >= CHANNEL_WIDTH_40) + ptdls_sta->cmn.bw_mode = CHANNEL_WIDTH_40; + ptdls_sta->htpriv.ch_offset = padapter->mlmeextpriv.cur_ch_offset; + } + + /* Config LDPC Coding Capability */ + if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAP_ELE_LDPC_CAP(data)) { + SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX)); + RTW_INFO("Enable HT Tx LDPC!\n"); + } + ptdls_sta->htpriv.ldpc_cap = cur_ldpc_cap; + + /* Config STBC setting */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(data)) { + SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX)); + RTW_INFO("Enable HT Tx STBC!\n"); + } + ptdls_sta->htpriv.stbc_cap = cur_stbc_cap; + +#ifdef CONFIG_BEAMFORMING + /* Config Tx beamforming setting */ + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(data)) + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(data)) + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + ptdls_sta->htpriv.beamform_cap = cur_beamform_cap; + if (cur_beamform_cap) + RTW_INFO("Client HT Beamforming Cap = 0x%02X\n", cur_beamform_cap); +#endif /* CONFIG_BEAMFORMING */ + } + +} + +u8 *rtw_tdls_set_ht_cap(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + rtw_ht_use_default_setting(padapter); + + if (padapter->registrypriv.wifi_spec == 1) { + padapter->mlmepriv.htpriv.sgi_20m = _FALSE; + padapter->mlmepriv.htpriv.sgi_40m = _FALSE; + } + + rtw_restructure_ht_ie(padapter, NULL, pframe, 0, &(pattrib->pktlen), padapter->mlmeextpriv.cur_channel); + + return pframe + pattrib->pktlen; +} +#endif + +#ifdef CONFIG_80211AC_VHT +void rtw_tdls_process_vht_cap(_adapter *padapter, struct sta_info *ptdls_sta, u8 *data, u8 Length) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0, tx_nss = 0; + u16 cur_beamform_cap = 0; + u8 *pcap_mcs; + + _rtw_memset(&ptdls_sta->vhtpriv, 0, sizeof(struct vht_priv)); + if (data && Length == 12) { + ptdls_sta->flags |= WLAN_STA_VHT; + + _rtw_memcpy(ptdls_sta->vhtpriv.vht_cap, data, 12); + +#if 0 + if (elems.vht_op_mode_notify && elems.vht_op_mode_notify_len == 1) + _rtw_memcpy(&pstat->vhtpriv.vht_op_mode_notify, elems.vht_op_mode_notify, 1); + else /* for Frame without Operating Mode notify ie; default: 80M */ + pstat->vhtpriv.vht_op_mode_notify = CHANNEL_WIDTH_80; +#else + ptdls_sta->vhtpriv.vht_op_mode_notify = CHANNEL_WIDTH_80; +#endif + } else { + ptdls_sta->flags &= ~WLAN_STA_VHT; + return; + } + + if (ptdls_sta->flags & WLAN_STA_VHT) { + if (REGSTY_IS_11AC_ENABLE(&padapter->registrypriv) + && is_supported_vht(padapter->registrypriv.wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent))) { + ptdls_sta->vhtpriv.vht_option = _TRUE; + ptdls_sta->cmn.ra_info.is_vht_enable = _TRUE; + } + else + ptdls_sta->vhtpriv.vht_option = _FALSE; + } + + /* B4 Rx LDPC */ + if (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_TX) && + GET_VHT_CAPABILITY_ELE_RX_LDPC(data)) { + SET_FLAG(cur_ldpc_cap, (LDPC_VHT_ENABLE_TX | LDPC_VHT_CAP_TX)); + RTW_INFO("Current VHT LDPC Setting = %02X\n", cur_ldpc_cap); + } + ptdls_sta->vhtpriv.ldpc_cap = cur_ldpc_cap; + + /* B5 Short GI for 80 MHz */ + ptdls_sta->vhtpriv.sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI80M(data) & pvhtpriv->sgi_80m) ? _TRUE : _FALSE; + + /* B8 B9 B10 Rx STBC */ + if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX) && + GET_VHT_CAPABILITY_ELE_RX_STBC(data)) { + SET_FLAG(cur_stbc_cap, (STBC_VHT_ENABLE_TX | STBC_VHT_CAP_TX)); + RTW_INFO("Current VHT STBC Setting = %02X\n", cur_stbc_cap); + } + ptdls_sta->vhtpriv.stbc_cap = cur_stbc_cap; + + #ifdef CONFIG_BEAMFORMING + /* B11 SU Beamformer Capable, the target supports Beamformer and we are Beamformee */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE) && + GET_VHT_CAPABILITY_ELE_SU_BFEE(data)) + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE); + + /* B12 SU Beamformee Capable, the target supports Beamformee and we are Beamformer */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE) && + GET_VHT_CAPABILITY_ELE_SU_BFER(data)) + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + ptdls_sta->vhtpriv.beamform_cap = cur_beamform_cap; + ptdls_sta->cmn.bf_info.vht_beamform_cap = cur_beamform_cap; + if (cur_beamform_cap) + RTW_INFO("Current VHT Beamforming Setting = %02X\n", cur_beamform_cap); + #endif /*CONFIG_BEAMFORMING*/ + + /* B23 B24 B25 Maximum A-MPDU Length Exponent */ + ptdls_sta->vhtpriv.ampdu_len = GET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(data); + + pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(data); + tx_nss = GET_HAL_TX_NSS(padapter); + rtw_vht_nss_to_mcsmap(tx_nss, ptdls_sta->vhtpriv.vht_mcs_map, pcap_mcs); + ptdls_sta->vhtpriv.vht_highest_rate = rtw_get_vht_highest_rate(ptdls_sta->vhtpriv.vht_mcs_map); +} + +void rtw_tdls_process_vht_operation(_adapter *padapter, struct sta_info *ptdls_sta, u8 *data, u8 Length) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct registry_priv *regsty = adapter_to_regsty(padapter); + u8 operation_bw = 0; + + if (GET_VHT_OPERATION_ELE_CHL_WIDTH(data) >= 1) { + + operation_bw = CHANNEL_WIDTH_80; + + if (hal_is_bw_support(padapter, operation_bw) && REGSTY_IS_BW_5G_SUPPORT(regsty, operation_bw) + && (operation_bw <= pmlmeext->cur_bwmode)) + ptdls_sta->cmn.bw_mode = operation_bw; + else + ptdls_sta->cmn.bw_mode = pmlmeext->cur_bwmode; + } else + ptdls_sta->cmn.bw_mode = pmlmeext->cur_bwmode; +} + +void rtw_tdls_process_vht_op_mode_notify(_adapter *padapter, struct sta_info *ptdls_sta, u8 *data, u8 Length) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct registry_priv *regsty = adapter_to_regsty(padapter); + u8 target_bw; + u8 target_rxss, current_rxss; + + if (pvhtpriv->vht_option == _FALSE) + return; + + target_bw = GET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(data); + target_rxss = (GET_VHT_OPERATING_MODE_FIELD_RX_NSS(data) + 1); + + if (hal_is_bw_support(padapter, target_bw) && REGSTY_IS_BW_5G_SUPPORT(regsty, target_bw) + && (target_bw <= pmlmeext->cur_bwmode)) + ptdls_sta->cmn.bw_mode = target_bw; + else + ptdls_sta->cmn.bw_mode = pmlmeext->cur_bwmode; + + current_rxss = rtw_vht_mcsmap_to_nss(ptdls_sta->vhtpriv.vht_mcs_map); + if (target_rxss != current_rxss) { + u8 vht_mcs_map[2] = {}; + + rtw_vht_nss_to_mcsmap(target_rxss, vht_mcs_map, ptdls_sta->vhtpriv.vht_mcs_map); + _rtw_memcpy(ptdls_sta->vhtpriv.vht_mcs_map, vht_mcs_map, 2); + } +} + +u8 *rtw_tdls_set_aid(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + return rtw_set_ie(pframe, EID_AID, 2, (u8 *)&(padapter->mlmepriv.cur_network.aid), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_vht_cap(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + u32 ie_len = 0; + + rtw_vht_use_default_setting(padapter); + + ie_len = rtw_build_vht_cap_ie(padapter, pframe); + pattrib->pktlen += ie_len; + + return pframe + ie_len; +} + +u8 *rtw_tdls_set_vht_operation(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib, u8 channel) +{ + u32 ie_len = 0; + + ie_len = rtw_build_vht_operation_ie(padapter, pframe, channel); + pattrib->pktlen += ie_len; + + return pframe + ie_len; +} + +u8 *rtw_tdls_set_vht_op_mode_notify(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib, u8 bw) +{ + u32 ie_len = 0; + + ie_len = rtw_build_vht_op_mode_notify_ie(padapter, pframe, bw); + pattrib->pktlen += ie_len; + + return pframe + ie_len; +} +#endif + + +u8 *rtw_tdls_set_sup_ch(_adapter *adapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + u8 sup_ch[30 * 2] = {0x00}, ch_set_idx = 0, sup_ch_idx = 2; + + while (ch_set_idx < rfctl->max_chan_nums && rfctl->channel_set[ch_set_idx].ChannelNum != 0) { + if (rfctl->channel_set[ch_set_idx].ChannelNum <= 14) { + /* TODO: fix 2.4G supported channel when channel doesn't start from 1 and continuous */ + sup_ch[0] = 1; /* First channel number */ + sup_ch[1] = rfctl->channel_set[ch_set_idx].ChannelNum; /* Number of channel */ + } else { + sup_ch[sup_ch_idx++] = rfctl->channel_set[ch_set_idx].ChannelNum; + sup_ch[sup_ch_idx++] = 1; + } + ch_set_idx++; + } + + return rtw_set_ie(pframe, _SUPPORTED_CH_IE_, sup_ch_idx, sup_ch, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_rsnie(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, int init, struct sta_info *ptdls_sta) +{ + u8 *p = NULL; + int len = 0; + + if (ptxmgmt->len > 0) + p = rtw_get_ie(ptxmgmt->buf, _RSN_IE_2_, &len, ptxmgmt->len); + + if (p != NULL) + return rtw_set_ie(pframe, _RSN_IE_2_, len, p + 2, &(pattrib->pktlen)); + else if (init == _TRUE) + return rtw_set_ie(pframe, _RSN_IE_2_, sizeof(TDLS_RSNIE), TDLS_RSNIE, &(pattrib->pktlen)); + else + return rtw_set_ie(pframe, _RSN_IE_2_, sizeof(ptdls_sta->TDLS_RSNIE), ptdls_sta->TDLS_RSNIE, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_ext_cap(u8 *pframe, struct pkt_attrib *pattrib) +{ + return rtw_set_ie(pframe, _EXT_CAP_IE_ , sizeof(TDLS_EXT_CAPIE), TDLS_EXT_CAPIE, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_qos_cap(u8 *pframe, struct pkt_attrib *pattrib) +{ + return rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(TDLS_WMMIE), TDLS_WMMIE, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_ftie(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, u8 *ANonce, u8 *SNonce) +{ + struct wpa_tdls_ftie FTIE = {0}; + u8 *p = NULL; + int len = 0; + + if (ptxmgmt->len > 0) + p = rtw_get_ie(ptxmgmt->buf, _FTIE_, &len, ptxmgmt->len); + + if (p != NULL) + return rtw_set_ie(pframe, _FTIE_, len, p + 2, &(pattrib->pktlen)); + else { + if (ANonce != NULL) + _rtw_memcpy(FTIE.Anonce, ANonce, WPA_NONCE_LEN); + if (SNonce != NULL) + _rtw_memcpy(FTIE.Snonce, SNonce, WPA_NONCE_LEN); + + return rtw_set_ie(pframe, _FTIE_, TDLS_FTIE_DATA_LEN, + (u8 *)FTIE.data, &(pattrib->pktlen)); + } +} + +u8 *rtw_tdls_set_timeout_interval(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, int init, struct sta_info *ptdls_sta) +{ + u8 timeout_itvl[5]; /* set timeout interval to maximum value */ + u32 timeout_interval = TDLS_TPK_RESEND_COUNT; + u8 *p = NULL; + int len = 0; + + if (ptxmgmt->len > 0) + p = rtw_get_ie(ptxmgmt->buf, _TIMEOUT_ITVL_IE_, &len, ptxmgmt->len); + + if (p != NULL) + return rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, len, p + 2, &(pattrib->pktlen)); + else { + /* Timeout interval */ + timeout_itvl[0] = 0x02; + if (init == _TRUE) + _rtw_memcpy(timeout_itvl + 1, &timeout_interval, 4); + else + _rtw_memcpy(timeout_itvl + 1, (u8 *)(&ptdls_sta->TDLS_PeerKey_Lifetime), 4); + + return rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl, &(pattrib->pktlen)); + } +} + +u8 *rtw_tdls_set_bss_coexist(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + u8 iedata = 0; + + if (padapter->mlmepriv.num_FortyMHzIntolerant > 0) + iedata |= BIT(2); /* 20 MHz BSS Width Request */ + + /* Information Bit should be set by TDLS test plan 5.9 */ + iedata |= BIT(0); + return rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_payload_type(u8 *pframe, struct pkt_attrib *pattrib) +{ + u8 payload_type = 0x02; + return rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_category(u8 *pframe, struct pkt_attrib *pattrib, u8 category) +{ + return rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_action(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt) +{ + return rtw_set_fixed_ie(pframe, 1, &(ptxmgmt->action_code), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_status_code(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt) +{ + return rtw_set_fixed_ie(pframe, 2, (u8 *)&(ptxmgmt->status_code), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_dialog(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt) +{ + u8 dialogtoken = 1; + if (ptxmgmt->dialog_token) + return rtw_set_fixed_ie(pframe, 1, &(ptxmgmt->dialog_token), &(pattrib->pktlen)); + else + return rtw_set_fixed_ie(pframe, 1, &(dialogtoken), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_reg_class(u8 *pframe, struct pkt_attrib *pattrib, struct sta_info *ptdls_sta) +{ + u8 reg_class = 22; + return rtw_set_fixed_ie(pframe, 1, &(reg_class), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_second_channel_offset(u8 *pframe, struct pkt_attrib *pattrib, u8 ch_offset) +{ + return rtw_set_ie(pframe, EID_SecondaryChnlOffset , 1, &ch_offset, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_capability(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + u8 cap_from_ie[2] = {0}; + + _rtw_memcpy(cap_from_ie, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2); + + return rtw_set_fixed_ie(pframe, 2, cap_from_ie, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_supported_rate(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + u8 bssrate[NDIS_802_11_LENGTH_RATES_EX]; + int bssrate_len = 0; + u8 more_supportedrates = 0; + + rtw_set_supported_rate(bssrate, (padapter->registrypriv.wireless_mode == WIRELESS_MODE_MAX) ? padapter->mlmeextpriv.cur_wireless_mode : padapter->registrypriv.wireless_mode); + bssrate_len = rtw_get_rateset_len(bssrate); + + if (bssrate_len > 8) { + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen)); + more_supportedrates = 1; + } else + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen)); + + /* extended supported rates */ + if (more_supportedrates == 1) + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen)); + + return pframe; +} + +u8 *rtw_tdls_set_sup_reg_class(u8 *pframe, struct pkt_attrib *pattrib) +{ + return rtw_set_ie(pframe, _SRC_IE_ , sizeof(TDLS_SRC), TDLS_SRC, &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_linkid(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib, u8 init) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + u8 link_id_addr[18] = {0}; + + _rtw_memcpy(link_id_addr, get_my_bssid(&(pmlmeinfo->network)), 6); + + if (init == _TRUE) { + _rtw_memcpy((link_id_addr + 6), pattrib->src, 6); + _rtw_memcpy((link_id_addr + 12), pattrib->dst, 6); + } else { + _rtw_memcpy((link_id_addr + 6), pattrib->dst, 6); + _rtw_memcpy((link_id_addr + 12), pattrib->src, 6); + } + return rtw_set_ie(pframe, _LINK_ID_IE_, 18, link_id_addr, &(pattrib->pktlen)); +} + +#ifdef CONFIG_TDLS_CH_SW +u8 *rtw_tdls_set_target_ch(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + u8 target_ch = 1; + if (padapter->tdlsinfo.chsw_info.off_ch_num) + return rtw_set_fixed_ie(pframe, 1, &(padapter->tdlsinfo.chsw_info.off_ch_num), &(pattrib->pktlen)); + else + return rtw_set_fixed_ie(pframe, 1, &(target_ch), &(pattrib->pktlen)); +} + +u8 *rtw_tdls_set_ch_sw(u8 *pframe, struct pkt_attrib *pattrib, struct sta_info *ptdls_sta) +{ + u8 ch_switch_timing[4] = {0}; + u16 switch_time = (ptdls_sta->ch_switch_time >= TDLS_CH_SWITCH_TIME * 1000) ? + ptdls_sta->ch_switch_time : TDLS_CH_SWITCH_TIME; + u16 switch_timeout = (ptdls_sta->ch_switch_timeout >= TDLS_CH_SWITCH_TIMEOUT * 1000) ? + ptdls_sta->ch_switch_timeout : TDLS_CH_SWITCH_TIMEOUT; + + _rtw_memcpy(ch_switch_timing, &switch_time, 2); + _rtw_memcpy(ch_switch_timing + 2, &switch_timeout, 2); + + return rtw_set_ie(pframe, _CH_SWITCH_TIMING_, 4, ch_switch_timing, &(pattrib->pktlen)); +} + +void rtw_tdls_set_ch_sw_oper_control(_adapter *padapter, u8 enable) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + u8 bcn_early_case; + + if (enable == _TRUE) { +#ifdef CONFIG_TDLS_CH_SW_V2 + pHalData->ch_switch_offload = _TRUE; +#endif + +#ifdef CONFIG_TDLS_CH_SW_BY_DRV + pHalData->ch_switch_offload = _FALSE; +#endif + bcn_early_case = TDLS_BCN_ERLY_ON; + } + else { + pHalData->ch_switch_offload = _FALSE; + bcn_early_case = TDLS_BCN_ERLY_OFF; + } + + if (ATOMIC_READ(&padapter->tdlsinfo.chsw_info.chsw_on) != enable) + ATOMIC_SET(&padapter->tdlsinfo.chsw_info.chsw_on, enable); + + rtw_hal_set_hwreg(padapter, HW_VAR_BCN_EARLY_C2H_RPT, &enable); + rtw_hal_set_hwreg(padapter, HW_VAR_SET_DRV_ERLY_INT, &bcn_early_case); + RTW_INFO("[TDLS] %s Bcn Early C2H Report\n", (enable == _TRUE) ? "Start" : "Stop"); +} + +void rtw_tdls_ch_sw_back_to_base_chnl(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv; + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + + pmlmepriv = &padapter->mlmepriv; + + if ((ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) && + (padapter->mlmeextpriv.cur_channel != rtw_get_oper_ch(padapter))) + rtw_tdls_cmd(padapter, pchsw_info->addr, TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED); +} + +#ifndef CONFIG_TDLS_CH_SW_V2 +static void rtw_tdls_chsw_oper_init(_adapter *padapter, u32 timeout_ms) +{ + struct submit_ctx *chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx; + + rtw_sctx_init(chsw_sctx, timeout_ms); +} + +static int rtw_tdls_chsw_oper_wait(_adapter *padapter) +{ + struct submit_ctx *chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx; + + return rtw_sctx_wait(chsw_sctx, __func__); +} + +void rtw_tdls_chsw_oper_done(_adapter *padapter) +{ + struct submit_ctx *chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx; + + rtw_sctx_done(&chsw_sctx); +} +#endif + +s32 rtw_tdls_do_ch_sw(_adapter *padapter, struct sta_info *ptdls_sta, u8 chnl_type, u8 channel, u8 channel_offset, u16 bwmode, u16 ch_switch_time) +{ + HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter); + u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + u32 ch_sw_time_start, ch_sw_time_spent, wait_time; + u8 take_care_iqk; + s32 ret = _FAIL; + + ch_sw_time_start = rtw_systime_to_ms(rtw_get_current_time()); + + /* set mac_id sleep before channel switch */ + rtw_hal_macid_sleep(padapter, ptdls_sta->cmn.mac_id); + +#if defined(CONFIG_TDLS_CH_SW_BY_DRV) || defined(CONFIG_TDLS_CH_SW_V2) + set_channel_bwmode(padapter, channel, channel_offset, bwmode); + ret = _SUCCESS; +#else + rtw_tdls_chsw_oper_init(padapter, TDLS_CH_SWITCH_OPER_OFFLOAD_TIMEOUT); + + /* channel switch IOs offload to FW */ + if (rtw_hal_ch_sw_oper_offload(padapter, channel, channel_offset, bwmode) == _SUCCESS) { + if (rtw_tdls_chsw_oper_wait(padapter) == _SUCCESS) { + /* set channel and bw related variables in driver */ + _enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL); + + rtw_set_oper_ch(padapter, channel); + rtw_set_oper_choffset(padapter, channel_offset); + rtw_set_oper_bw(padapter, bwmode); + + center_ch = rtw_get_center_ch(channel, bwmode, channel_offset); + pHalData->current_channel = center_ch; + pHalData->CurrentCenterFrequencyIndex1 = center_ch; + pHalData->current_channel_bw = bwmode; + pHalData->nCur40MhzPrimeSC = channel_offset; + + if (bwmode == CHANNEL_WIDTH_80) { + if (center_ch > channel) + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_LOWER; + else if (center_ch < channel) + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_UPPER; + else + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + pHalData->nCur80MhzPrimeSC = chnl_offset80; + + pHalData->CurrentCenterFrequencyIndex1 = center_ch; + + _exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL); + + rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk); + if (take_care_iqk == _TRUE) + rtw_hal_ch_sw_iqk_info_restore(padapter, CH_SW_USE_CASE_TDLS); + + ret = _SUCCESS; + } else + RTW_INFO("[TDLS] chsw oper wait fail !!\n"); + } +#endif + + if (ret == _SUCCESS) { + ch_sw_time_spent = rtw_systime_to_ms(rtw_get_current_time()) - ch_sw_time_start; + if (chnl_type == TDLS_CH_SW_OFF_CHNL) { + if ((u32)ch_switch_time / 1000 > ch_sw_time_spent) + wait_time = (u32)ch_switch_time / 1000 - ch_sw_time_spent; + else + wait_time = 0; + + if (wait_time > 0) + rtw_msleep_os(wait_time); + } + } + + /* set mac_id wakeup after channel switch */ + rtw_hal_macid_wakeup(padapter, ptdls_sta->cmn.mac_id); + + return ret; +} +#endif + +u8 *rtw_tdls_set_wmm_params(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 wmm_param_ele[24] = {0}; + + if (&pmlmeinfo->WMM_param) { + _rtw_memcpy(wmm_param_ele, WMM_PARA_OUI, 6); + if (_rtw_memcmp(&pmlmeinfo->WMM_param, &wmm_param_ele[6], 18) == _TRUE) + /* Use default WMM Param */ + _rtw_memcpy(wmm_param_ele + 6, (u8 *)&TDLS_WMM_PARAM_IE, sizeof(TDLS_WMM_PARAM_IE)); + else + _rtw_memcpy(wmm_param_ele + 6, (u8 *)&pmlmeinfo->WMM_param, sizeof(pmlmeinfo->WMM_param)); + return rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 24, wmm_param_ele, &(pattrib->pktlen)); + } else + return pframe; +} + +#ifdef CONFIG_WFD +void rtw_tdls_process_wfd_ie(struct tdls_info *ptdlsinfo, u8 *ptr, u8 length) +{ + u8 *wfd_ie; + u32 wfd_ielen = 0; + + if (!hal_chk_wl_func(tdls_info_to_adapter(ptdlsinfo), WL_FUNC_MIRACAST)) + return; + + /* Try to get the TCP port information when receiving the negotiation response. */ + + wfd_ie = rtw_get_wfd_ie(ptr, length, NULL, &wfd_ielen); + while (wfd_ie) { + u8 *attr_content; + u32 attr_contentlen = 0; + int i; + + RTW_INFO("[%s] WFD IE Found!!\n", __FUNCTION__); + attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen); + if (attr_content && attr_contentlen) { + ptdlsinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + RTW_INFO("[%s] Peer PORT NUM = %d\n", __FUNCTION__, ptdlsinfo->wfd_info->peer_rtsp_ctrlport); + } + + attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_LOCAL_IP_ADDR, NULL, &attr_contentlen); + if (attr_content && attr_contentlen) { + _rtw_memcpy(ptdlsinfo->wfd_info->peer_ip_address, (attr_content + 1), 4); + RTW_INFO("[%s] Peer IP = %02u.%02u.%02u.%02u\n", __FUNCTION__, + ptdlsinfo->wfd_info->peer_ip_address[0], ptdlsinfo->wfd_info->peer_ip_address[1], + ptdlsinfo->wfd_info->peer_ip_address[2], ptdlsinfo->wfd_info->peer_ip_address[3]); + } + + wfd_ie = rtw_get_wfd_ie(wfd_ie + wfd_ielen, (ptr + length) - (wfd_ie + wfd_ielen), NULL, &wfd_ielen); + } +} + +int issue_tunneled_probe_req(_adapter *padapter) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + u8 baddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct tdls_txmgmt txmgmt; + int ret = _FAIL; + + RTW_INFO("[%s]\n", __FUNCTION__); + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.action_code = TUNNELED_PROBE_REQ; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, baddr, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; +exit: + + return ret; +} + +int issue_tunneled_probe_rsp(_adapter *padapter, union recv_frame *precv_frame) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct tdls_txmgmt txmgmt; + int ret = _FAIL; + + RTW_INFO("[%s]\n", __FUNCTION__); + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.action_code = TUNNELED_PROBE_RSP; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, precv_frame->u.hdr.attrib.src, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; +exit: + + return ret; +} +#endif /* CONFIG_WFD */ + +int issue_tdls_setup_req(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, int wait_ack) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *ptdls_sta = NULL; + _irqL irqL; + int ret = _FAIL; + /* Retry timer should be set at least 301 sec, using TPK_count counting 301 times. */ + u32 timeout_interval = TDLS_TPK_RESEND_COUNT; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + if (rtw_tdls_is_setup_allowed(padapter) == _FALSE) + goto exit; + + if (IS_MCAST(ptxmgmt->peer)) + goto exit; + + ptdls_sta = rtw_get_stainfo(pstapriv, ptxmgmt->peer); + if (ptdlsinfo->sta_maximum == _TRUE) { + if (ptdls_sta == NULL) + goto exit; + else if (!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) + goto exit; + } + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + if (ptdls_sta == NULL) { + ptdls_sta = rtw_alloc_stainfo(pstapriv, ptxmgmt->peer); + if (ptdls_sta == NULL) { + RTW_INFO("[%s] rtw_alloc_stainfo fail\n", __FUNCTION__); + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + ptdlsinfo->sta_cnt++; + } + + ptxmgmt->action_code = TDLS_SETUP_REQUEST; + + pattrib = &pmgntframe->attrib; + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + + if (ptdlsinfo->sta_cnt == MAX_ALLOWED_TDLS_STA_NUM) + ptdlsinfo->sta_maximum = _TRUE; + + ptdls_sta->tdls_sta_state |= TDLS_RESPONDER_STATE; + + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) { + ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval; + _set_timer(&ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME); + } + + pattrib->qsel = pattrib->priority; + + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + + return ret; +} + +int _issue_tdls_teardown(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta, u8 wait_ack) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct sta_priv *pstapriv = &padapter->stapriv; + _irqL irqL; + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + ptxmgmt->action_code = TDLS_TEARDOWN; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + rtw_mi_set_scan_deny(padapter, 550); + rtw_mi_scan_abort(padapter, _TRUE); + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + + if (ptxmgmt->status_code == _RSON_TDLS_TEAR_UN_RSN_) + _rtw_memcpy(pattrib->ra, ptxmgmt->peer, ETH_ALEN); + else + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) + if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) + if (pattrib->encrypt) + _cancel_timer_ex(&ptdls_sta->TPK_timer); + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } + +exit: + + return ret; +} + +int issue_tdls_teardown(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 wait_ack) +{ + struct sta_info *ptdls_sta = NULL; + int ret = _FAIL; + + ptdls_sta = rtw_get_stainfo(&(padapter->stapriv), ptxmgmt->peer); + if (ptdls_sta == NULL) { + RTW_INFO("No tdls_sta for tearing down\n"); + goto exit; + } + + ret = _issue_tdls_teardown(padapter, ptxmgmt, ptdls_sta, wait_ack); + if ((ptxmgmt->status_code == _RSON_TDLS_TEAR_UN_RSN_) && (ret == _FAIL)) { + /* Change status code and send teardown again via AP */ + ptxmgmt->status_code = _RSON_TDLS_TEAR_TOOFAR_; + ret = _issue_tdls_teardown(padapter, ptxmgmt, ptdls_sta, wait_ack); + } + + if (rtw_tdls_is_driver_setup(padapter)) { + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptxmgmt->peer, TDLS_TEARDOWN_STA_LOCALLY_POST); + } + +exit: + return ret; +} + +int issue_tdls_dis_req(_adapter *padapter, struct tdls_txmgmt *ptxmgmt) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + ptxmgmt->action_code = TDLS_DISCOVERY_REQUEST; + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + dump_mgntframe(padapter, pmgntframe); + RTW_INFO("issue tdls dis req\n"); + + ret = _SUCCESS; +exit: + + return ret; +} + +int issue_tdls_setup_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + ptxmgmt->action_code = TDLS_SETUP_RESPONSE; + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(&(padapter->mlmepriv)), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + dump_mgntframe(padapter, pmgntframe); + + ret = _SUCCESS; +exit: + + return ret; + +} + +int issue_tdls_setup_cfm(_adapter *padapter, struct tdls_txmgmt *ptxmgmt) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + ptxmgmt->action_code = TDLS_SETUP_CONFIRM; + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(&padapter->mlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + dump_mgntframe(padapter, pmgntframe); + + ret = _SUCCESS; +exit: + + return ret; + +} + +/* TDLS Discovery Response frame is a management action frame */ +int issue_tdls_dis_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 privacy) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + /* unicast probe request frame */ + _rtw_memcpy(pwlanhdr->addr1, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->dst, pwlanhdr->addr1, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->src, pwlanhdr->addr2, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ra, pwlanhdr->addr3, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + rtw_build_tdls_dis_rsp_ies(padapter, pmgntframe, pframe, ptxmgmt, privacy); + + pattrib->nr_frags = 1; + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + +exit: + return ret; +} + +int issue_tdls_peer_traffic_rsp(_adapter *padapter, struct sta_info *ptdls_sta, struct tdls_txmgmt *ptxmgmt) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + ptxmgmt->action_code = TDLS_PEER_TRAFFIC_RESPONSE; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptdls_sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, ptdls_sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + +exit: + + return ret; +} + +int issue_tdls_peer_traffic_indication(_adapter *padapter, struct sta_info *ptdls_sta) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct tdls_txmgmt txmgmt; + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.action_code = TDLS_PEER_TRAFFIC_INDICATION; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptdls_sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + /* PTI frame's priority should be AC_VO */ + pattrib->priority = 7; + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + +exit: + + return ret; +} + +#ifdef CONFIG_TDLS_CH_SW +int issue_tdls_ch_switch_req(_adapter *padapter, struct sta_info *ptdls_sta) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct tdls_txmgmt txmgmt; + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) { + RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__); + goto exit; + } + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + txmgmt.action_code = TDLS_CHANNEL_SWITCH_REQUEST; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptdls_sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, ptdls_sta->cmn.mac_addr, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; +exit: + + return ret; +} + +int issue_tdls_ch_switch_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, int wait_ack) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + int ret = _FAIL; + + RTW_INFO("[TDLS] %s\n", __FUNCTION__); + + if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) { + RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__); + goto exit; + } + + ptxmgmt->action_code = TDLS_CHANNEL_SWITCH_RESPONSE; + + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) + goto exit; + + pattrib = &pmgntframe->attrib; + + pmgntframe->frame_tag = DATA_FRAMETAG; + pattrib->ether_type = 0x890d; + + _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pattrib->ra, ptxmgmt->peer, ETH_ALEN); + _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN); + + update_tdls_attrib(padapter, pattrib); + pattrib->qsel = pattrib->priority; + /* + _enter_critical_bh(&pxmitpriv->lock, &irqL); + if(xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pmgntframe)==_TRUE){ + _exit_critical_bh(&pxmitpriv->lock, &irqL); + return _FALSE; + } + */ + if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) { + rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pmgntframe); + goto exit; + } + + if (wait_ack) + ret = dump_mgntframe_and_wait_ack_timeout(padapter, pmgntframe, 10); + else { + dump_mgntframe(padapter, pmgntframe); + ret = _SUCCESS; + } +exit: + + return ret; +} +#endif + +int On_TDLS_Dis_Rsp(_adapter *padapter, union recv_frame *precv_frame) +{ + struct sta_info *ptdls_sta = NULL, *psta = rtw_get_stainfo(&(padapter->stapriv), get_bssid(&(padapter->mlmepriv))); + struct recv_priv *precvpriv = &(padapter->recvpriv); + u8 *ptr = precv_frame->u.hdr.rx_data, *psa; + struct rx_pkt_attrib *pattrib = &(precv_frame->u.hdr.attrib); + struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo); + u8 empty_addr[ETH_ALEN] = { 0x00 }; + int rssi = 0; + struct tdls_txmgmt txmgmt; + int ret = _SUCCESS; + + if (psta) + rssi = psta->cmn.rssi_stat.rssi; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + /* WFDTDLS: for sigma test, not to setup direct link automatically */ + ptdlsinfo->dev_discovered = _TRUE; + + psa = get_sa(ptr); + ptdls_sta = rtw_get_stainfo(&(padapter->stapriv), psa); + if (ptdls_sta != NULL) + ptdls_sta->sta_stats.rx_tdls_disc_rsp_pkts++; + +#ifdef CONFIG_TDLS_AUTOSETUP + if (ptdls_sta != NULL) { + /* Record the tdls sta with lowest signal strength */ + if (ptdlsinfo->sta_maximum == _TRUE && ptdls_sta->alive_count >= 1) { + if (_rtw_memcmp(ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN)) { + _rtw_memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN); + ptdlsinfo->ss_record.RxPWDBAll = pattrib->phy_info.rx_pwdb_all; + } else { + if (ptdlsinfo->ss_record.RxPWDBAll < pattrib->phy_info.rx_pwdb_all) { + _rtw_memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN); + ptdlsinfo->ss_record.RxPWDBAll = pattrib->phy_info.rx_pwdb_all; + } + } + } + } else { + if (ptdlsinfo->sta_maximum == _TRUE) { + if (_rtw_memcmp(ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN)) { + /* All traffics are busy, do not set up another direct link. */ + ret = _FAIL; + goto exit; + } else { + if (pattrib->phy_info.rx_pwdb_all > ptdlsinfo->ss_record.RxPWDBAll) { + _rtw_memcpy(txmgmt.peer, ptdlsinfo->ss_record.macaddr, ETH_ALEN); + /* issue_tdls_teardown(padapter, ptdlsinfo->ss_record.macaddr, _FALSE); */ + } else { + ret = _FAIL; + goto exit; + } + } + } + + + if (pattrib->phy_info.rx_pwdb_all + TDLS_SIGNAL_THRESH >= rssi) { + RTW_INFO("pattrib->RxPWDBAll=%d, pdmpriv->undecorated_smoothed_pwdb=%d\n", pattrib->phy_info.rx_pwdb_all, rssi); + _rtw_memcpy(txmgmt.peer, psa, ETH_ALEN); + issue_tdls_setup_req(padapter, &txmgmt, _FALSE); + } + } +exit: +#endif /* CONFIG_TDLS_AUTOSETUP */ + + return ret; + +} + +sint On_TDLS_Setup_Req(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + u8 *psa, *pmyid; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct security_priv *psecuritypriv = &padapter->securitypriv; + _irqL irqL; + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + u8 *prsnie, *ppairwise_cipher; + u8 i, k; + u8 ccmp_included = 0, rsnie_included = 0; + u16 j, pairwise_count; + u8 SNonce[32]; + u32 timeout_interval = TDLS_TPK_RESEND_COUNT; + sint parsing_length; /* Frame body length, without icv_len */ + PNDIS_802_11_VARIABLE_IEs pIE; + u8 FIXED_IE = 5; + unsigned char supportRate[16]; + int supportRateNum = 0; + struct tdls_txmgmt txmgmt; + + if (rtw_tdls_is_setup_allowed(padapter) == _FALSE) + goto exit; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + psa = get_sa(ptr); + + if (ptdlsinfo->sta_maximum == _TRUE) { + if (ptdls_sta == NULL) + goto exit; + else if (!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) + goto exit; + } + + pmyid = adapter_mac_addr(padapter); + ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len + - prx_pkt_attrib->hdrlen + - prx_pkt_attrib->iv_len + - prx_pkt_attrib->icv_len + - LLC_HEADER_SIZE + - ETH_TYPE_LEN + - PAYLOAD_TYPE_LEN; + + if (ptdls_sta == NULL) { + ptdls_sta = rtw_alloc_stainfo(pstapriv, psa); + if (ptdls_sta == NULL) + goto exit; + + ptdlsinfo->sta_cnt++; + } + else { + if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) { + /* If the direct link is already set up */ + /* Process as re-setup after tear down */ + RTW_INFO("re-setup a direct link\n"); + } + /* Already receiving TDLS setup request */ + else if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) { + RTW_INFO("receive duplicated TDLS setup request frame in handshaking\n"); + goto exit; + } + /* When receiving and sending setup_req to the same link at the same time */ + /* STA with higher MAC_addr would be initiator */ + else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) { + RTW_INFO("receive setup_req after sending setup_req\n"); + for (i = 0; i < 6; i++) { + if (*(pmyid + i) == *(psa + i)) { + } else if (*(pmyid + i) > *(psa + i)) { + ptdls_sta->tdls_sta_state = TDLS_INITIATOR_STATE; + break; + } else if (*(pmyid + i) < *(psa + i)) + goto exit; + } + } + } + + if (ptdls_sta) { + txmgmt.dialog_token = *(ptr + 2); /* Copy dialog token */ + txmgmt.status_code = _STATS_SUCCESSFUL_; + + /* Parsing information element */ + for (j = FIXED_IE; j < parsing_length;) { + + pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j); + + switch (pIE->ElementID) { + case _SUPPORTEDRATES_IE_: + _rtw_memcpy(supportRate, pIE->data, pIE->Length); + supportRateNum = pIE->Length; + break; + case _COUNTRY_IE_: + break; + case _EXT_SUPPORTEDRATES_IE_: + if (supportRateNum < sizeof(supportRate)) { + _rtw_memcpy(supportRate + supportRateNum, pIE->data, pIE->Length); + supportRateNum += pIE->Length; + } + break; + case _SUPPORTED_CH_IE_: + break; + case _RSN_IE_2_: + rsnie_included = 1; + if (prx_pkt_attrib->encrypt) { + prsnie = (u8 *)pIE; + /* Check CCMP pairwise_cipher presence. */ + ppairwise_cipher = prsnie + 10; + _rtw_memcpy(ptdls_sta->TDLS_RSNIE, pIE->data, pIE->Length); + pairwise_count = *(u16 *)(ppairwise_cipher - 2); + for (k = 0; k < pairwise_count; k++) { + if (_rtw_memcmp(ppairwise_cipher + 4 * k, RSN_CIPHER_SUITE_CCMP, 4) == _TRUE) + ccmp_included = 1; + } + + if (ccmp_included == 0) + txmgmt.status_code = _STATS_INVALID_RSNIE_; + } + break; + case _EXT_CAP_IE_: + break; + case _VENDOR_SPECIFIC_IE_: + break; + case _FTIE_: + if (prx_pkt_attrib->encrypt) + _rtw_memcpy(SNonce, (ptr + j + 52), 32); + break; + case _TIMEOUT_ITVL_IE_: + if (prx_pkt_attrib->encrypt) + timeout_interval = cpu_to_le32(*(u32 *)(ptr + j + 3)); + break; + case _RIC_Descriptor_IE_: + break; +#ifdef CONFIG_80211N_HT + case _HT_CAPABILITY_IE_: + rtw_tdls_process_ht_cap(padapter, ptdls_sta, pIE->data, pIE->Length); + break; +#endif +#ifdef CONFIG_80211AC_VHT + case EID_AID: + break; + case EID_VHTCapability: + rtw_tdls_process_vht_cap(padapter, ptdls_sta, pIE->data, pIE->Length); + break; +#endif + case EID_BSSCoexistence: + break; + case _LINK_ID_IE_: + if (_rtw_memcmp(get_bssid(pmlmepriv), pIE->data, 6) == _FALSE) + txmgmt.status_code = _STATS_NOT_IN_SAME_BSS_; + break; + default: + break; + } + + j += (pIE->Length + 2); + + } + + /* Check status code */ + /* If responder STA has/hasn't security on AP, but request hasn't/has RSNIE, it should reject */ + if (txmgmt.status_code == _STATS_SUCCESSFUL_) { + if (rsnie_included && prx_pkt_attrib->encrypt == 0) + txmgmt.status_code = _STATS_SEC_DISABLED_; + else if (rsnie_included == 0 && prx_pkt_attrib->encrypt) + txmgmt.status_code = _STATS_INVALID_PARAMETERS_; + +#ifdef CONFIG_WFD + /* WFD test plan version 0.18.2 test item 5.1.5 */ + /* SoUT does not use TDLS if AP uses weak security */ + if (padapter->wdinfo.wfd_tdls_enable && (rsnie_included && prx_pkt_attrib->encrypt != _AES_)) + txmgmt.status_code = _STATS_SEC_DISABLED_; +#endif /* CONFIG_WFD */ + } + + ptdls_sta->tdls_sta_state |= TDLS_INITIATOR_STATE; + if (prx_pkt_attrib->encrypt) { + _rtw_memcpy(ptdls_sta->SNonce, SNonce, 32); + + if (timeout_interval <= 300) + ptdls_sta->TDLS_PeerKey_Lifetime = TDLS_TPK_RESEND_COUNT; + else + ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval; + } + + /* Update station supportRate */ + ptdls_sta->bssratelen = supportRateNum; + _rtw_memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum); + + /* -2: AP + BC/MC sta, -4: default key */ + if (ptdlsinfo->sta_cnt == MAX_ALLOWED_TDLS_STA_NUM) + ptdlsinfo->sta_maximum = _TRUE; + +#ifdef CONFIG_WFD + rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length); +#endif + + } else + goto exit; + + _rtw_memcpy(txmgmt.peer, prx_pkt_attrib->src, ETH_ALEN); + + if (rtw_tdls_is_driver_setup(padapter)) { + issue_tdls_setup_rsp(padapter, &txmgmt); + + if (txmgmt.status_code == _STATS_SUCCESSFUL_) + _set_timer(&ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME); + else { + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + } + } + +exit: + + return _SUCCESS; +} + +int On_TDLS_Setup_Rsp(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *ptr = precv_frame->u.hdr.rx_data; + _irqL irqL; + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + u8 *psa; + u16 status_code = 0; + sint parsing_length; /* Frame body length, without icv_len */ + PNDIS_802_11_VARIABLE_IEs pIE; + u8 FIXED_IE = 7; + u8 ANonce[32]; + u8 *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL, *ppairwise_cipher = NULL; + u16 pairwise_count, j, k; + u8 verify_ccmp = 0; + unsigned char supportRate[16]; + int supportRateNum = 0; + struct tdls_txmgmt txmgmt; + int ret = _SUCCESS; + u32 timeout_interval = TDLS_TPK_RESEND_COUNT; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + psa = get_sa(ptr); + + ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len + - prx_pkt_attrib->hdrlen + - prx_pkt_attrib->iv_len + - prx_pkt_attrib->icv_len + - LLC_HEADER_SIZE + - ETH_TYPE_LEN + - PAYLOAD_TYPE_LEN; + + _rtw_memcpy(&status_code, ptr + 2, 2); + + if (status_code != 0) { + RTW_INFO("[TDLS] %s status_code = %d, free_tdls_sta\n", __FUNCTION__, status_code); + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + ret = _FAIL; + goto exit; + } + + status_code = 0; + + /* parsing information element */ + for (j = FIXED_IE; j < parsing_length;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j); + + switch (pIE->ElementID) { + case _SUPPORTEDRATES_IE_: + _rtw_memcpy(supportRate, pIE->data, pIE->Length); + supportRateNum = pIE->Length; + break; + case _COUNTRY_IE_: + break; + case _EXT_SUPPORTEDRATES_IE_: + if (supportRateNum < sizeof(supportRate)) { + _rtw_memcpy(supportRate + supportRateNum, pIE->data, pIE->Length); + supportRateNum += pIE->Length; + } + break; + case _SUPPORTED_CH_IE_: + break; + case _RSN_IE_2_: + prsnie = (u8 *)pIE; + /* Check CCMP pairwise_cipher presence. */ + ppairwise_cipher = prsnie + 10; + _rtw_memcpy(&pairwise_count, (u16 *)(ppairwise_cipher - 2), 2); + for (k = 0; k < pairwise_count; k++) { + if (_rtw_memcmp(ppairwise_cipher + 4 * k, RSN_CIPHER_SUITE_CCMP, 4) == _TRUE) + verify_ccmp = 1; + } + case _EXT_CAP_IE_: + break; + case _VENDOR_SPECIFIC_IE_: + if (_rtw_memcmp((u8 *)pIE + 2, WMM_INFO_OUI, 6) == _TRUE) { + /* WMM Info ID and OUI */ + if ((pregistrypriv->wmm_enable == _TRUE) || (padapter->mlmepriv.htpriv.ht_option == _TRUE)) + ptdls_sta->qos_option = _TRUE; + } + break; + case _FTIE_: + pftie = (u8 *)pIE; + _rtw_memcpy(ANonce, (ptr + j + 20), 32); + break; + case _TIMEOUT_ITVL_IE_: + ptimeout_ie = (u8 *)pIE; + timeout_interval = cpu_to_le32(*(u32 *)(ptimeout_ie + 3)); + break; + case _RIC_Descriptor_IE_: + break; +#ifdef CONFIG_80211N_HT + case _HT_CAPABILITY_IE_: + rtw_tdls_process_ht_cap(padapter, ptdls_sta, pIE->data, pIE->Length); + break; +#endif +#ifdef CONFIG_80211AC_VHT + case EID_AID: + /* todo in the future if necessary */ + break; + case EID_VHTCapability: + rtw_tdls_process_vht_cap(padapter, ptdls_sta, pIE->data, pIE->Length); + break; + case EID_OpModeNotification: + rtw_tdls_process_vht_op_mode_notify(padapter, ptdls_sta, pIE->data, pIE->Length); + break; +#endif + case EID_BSSCoexistence: + break; + case _LINK_ID_IE_: + plinkid_ie = (u8 *)pIE; + break; + default: + break; + } + + j += (pIE->Length + 2); + + } + + ptdls_sta->bssratelen = supportRateNum; + _rtw_memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum); + _rtw_memcpy(ptdls_sta->ANonce, ANonce, 32); + +#ifdef CONFIG_WFD + rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length); +#endif + + if (prx_pkt_attrib->encrypt) { + if (verify_ccmp == 1) { + txmgmt.status_code = _STATS_SUCCESSFUL_; + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) { + wpa_tdls_generate_tpk(padapter, ptdls_sta); + if (tdls_verify_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie) == _FAIL) { + RTW_INFO("[TDLS] %s tdls_verify_mic fail, free_tdls_sta\n", __FUNCTION__); + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + ret = _FAIL; + goto exit; + } + ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval; + } + } else + txmgmt.status_code = _STATS_INVALID_RSNIE_; + } else + txmgmt.status_code = _STATS_SUCCESSFUL_; + + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) { + _rtw_memcpy(txmgmt.peer, prx_pkt_attrib->src, ETH_ALEN); + issue_tdls_setup_cfm(padapter, &txmgmt); + + if (txmgmt.status_code == _STATS_SUCCESSFUL_) { + rtw_tdls_set_link_established(padapter, _TRUE); + + if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) { + ptdls_sta->tdls_sta_state |= TDLS_LINKED_STATE; + ptdls_sta->state |= WIFI_ASOC_STATE; + _cancel_timer_ex(&ptdls_sta->handshake_timer); + } + + if (prx_pkt_attrib->encrypt) + rtw_tdls_set_key(padapter, ptdls_sta); + + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_ESTABLISHED); + + } + } + +exit: + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) + return ret; + else + return _SUCCESS; + +} + +int On_TDLS_Setup_Cfm(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *ptr = precv_frame->u.hdr.rx_data; + _irqL irqL; + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + u8 *psa; + u16 status_code = 0; + sint parsing_length; + PNDIS_802_11_VARIABLE_IEs pIE; + u8 FIXED_IE = 5; + u8 *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL, *ppairwise_cipher = NULL; + u16 j, pairwise_count; + int ret = _SUCCESS; + + psa = get_sa(ptr); + + ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len + - prx_pkt_attrib->hdrlen + - prx_pkt_attrib->iv_len + - prx_pkt_attrib->icv_len + - LLC_HEADER_SIZE + - ETH_TYPE_LEN + - PAYLOAD_TYPE_LEN; + + _rtw_memcpy(&status_code, ptr + 2, 2); + + if (status_code != 0) { + RTW_INFO("[%s] status_code = %d\n, free_tdls_sta", __FUNCTION__, status_code); + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + ret = _FAIL; + goto exit; + } + + /* Parsing information element */ + for (j = FIXED_IE; j < parsing_length;) { + + pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j); + + switch (pIE->ElementID) { + case _RSN_IE_2_: + prsnie = (u8 *)pIE; + break; + case _VENDOR_SPECIFIC_IE_: + if (_rtw_memcmp((u8 *)pIE + 2, WMM_PARA_OUI, 6) == _TRUE) { + /* WMM Parameter ID and OUI */ + ptdls_sta->qos_option = _TRUE; + } + break; + case _FTIE_: + pftie = (u8 *)pIE; + break; + case _TIMEOUT_ITVL_IE_: + ptimeout_ie = (u8 *)pIE; + break; +#ifdef CONFIG_80211N_HT + case _HT_EXTRA_INFO_IE_: + break; +#endif +#ifdef CONFIG_80211AC_VHT + case EID_VHTOperation: + rtw_tdls_process_vht_operation(padapter, ptdls_sta, pIE->data, pIE->Length); + break; + case EID_OpModeNotification: + rtw_tdls_process_vht_op_mode_notify(padapter, ptdls_sta, pIE->data, pIE->Length); + break; +#endif + case _LINK_ID_IE_: + plinkid_ie = (u8 *)pIE; + break; + default: + break; + } + + j += (pIE->Length + 2); + + } + + if (prx_pkt_attrib->encrypt) { + /* Verify mic in FTIE MIC field */ + if (rtw_tdls_is_driver_setup(padapter) && + (tdls_verify_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie) == _FAIL)) { + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + ret = _FAIL; + goto exit; + } + } + + if (rtw_tdls_is_driver_setup(padapter)) { + rtw_tdls_set_link_established(padapter, _TRUE); + + if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) { + ptdls_sta->tdls_sta_state |= TDLS_LINKED_STATE; + ptdls_sta->state |= WIFI_ASOC_STATE; + _cancel_timer_ex(&ptdls_sta->handshake_timer); + } + + if (prx_pkt_attrib->encrypt) { + rtw_tdls_set_key(padapter, ptdls_sta); + + /* Start TPK timer */ + ptdls_sta->TPK_count = 0; + _set_timer(&ptdls_sta->TPK_timer, ONE_SEC); + } + + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_ESTABLISHED); + } + +exit: + return ret; + +} + +int On_TDLS_Dis_Req(_adapter *padapter, union recv_frame *precv_frame) +{ + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta_ap; + u8 *ptr = precv_frame->u.hdr.rx_data; + sint parsing_length; /* Frame body length, without icv_len */ + PNDIS_802_11_VARIABLE_IEs pIE; + u8 FIXED_IE = 3, *dst; + u16 j; + struct tdls_txmgmt txmgmt; + int ret = _SUCCESS; + + if (rtw_tdls_is_driver_setup(padapter) == _FALSE) + goto exit; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + txmgmt.dialog_token = *(ptr + 2); + _rtw_memcpy(&txmgmt.peer, precv_frame->u.hdr.attrib.src, ETH_ALEN); + txmgmt.action_code = TDLS_DISCOVERY_RESPONSE; + parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len + - prx_pkt_attrib->hdrlen + - prx_pkt_attrib->iv_len + - prx_pkt_attrib->icv_len + - LLC_HEADER_SIZE + - ETH_TYPE_LEN + - PAYLOAD_TYPE_LEN; + + /* Parsing information element */ + for (j = FIXED_IE; j < parsing_length;) { + + pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j); + + switch (pIE->ElementID) { + case _LINK_ID_IE_: + psta_ap = rtw_get_stainfo(pstapriv, pIE->data); + if (psta_ap == NULL) + goto exit; + dst = pIE->data + 12; + if (MacAddr_isBcst(dst) == _FALSE && (_rtw_memcmp(adapter_mac_addr(padapter), dst, ETH_ALEN) == _FALSE)) + goto exit; + break; + default: + break; + } + + j += (pIE->Length + 2); + + } + + issue_tdls_dis_rsp(padapter, &txmgmt, prx_pkt_attrib->privacy); + +exit: + return ret; + +} + +int On_TDLS_Teardown(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct sta_priv *pstapriv = &padapter->stapriv; + _irqL irqL; + u8 reason; + + reason = *(ptr + prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN + 2); + RTW_INFO("[TDLS] %s Reason code(%d)\n", __FUNCTION__, reason); + + if (rtw_tdls_is_driver_setup(padapter)) { + rtw_tdls_teardown_pre_hdl(padapter, ptdls_sta); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY_POST); + } + + return _SUCCESS; + +} + +#if 0 +u8 TDLS_check_ch_state(uint state) +{ + if (state & TDLS_CH_SWITCH_ON_STATE && + state & TDLS_PEER_AT_OFF_STATE) { + if (state & TDLS_PEER_SLEEP_STATE) + return 2; /* U-APSD + ch. switch */ + else + return 1; /* ch. switch */ + } else + return 0; +} +#endif + +int On_TDLS_Peer_Traffic_Indication(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct tdls_txmgmt txmgmt; + + ptr += pattrib->hdrlen + pattrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + + txmgmt.dialog_token = *(ptr + 2); + issue_tdls_peer_traffic_rsp(padapter, ptdls_sta, &txmgmt); + /* issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->cmn.mac_addr, 0, 0, 0); */ + + return _SUCCESS; +} + +/* We process buffered data for 1. U-APSD, 2. ch. switch, 3. U-APSD + ch. switch here */ +int On_TDLS_Peer_Traffic_Rsp(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 wmmps_ac = 0; + /* u8 state=TDLS_check_ch_state(ptdls_sta->tdls_sta_state); */ + int i; + + ptdls_sta->sta_stats.rx_data_pkts++; + + ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE); + + /* Check 4-AC queue bit */ + if (ptdls_sta->uapsd_vo || ptdls_sta->uapsd_vi || ptdls_sta->uapsd_be || ptdls_sta->uapsd_bk) + wmmps_ac = 1; + + /* If it's a direct link and have buffered frame */ + if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) { + if (wmmps_ac) { + _irqL irqL; + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + + _enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL); + + xmitframe_phead = get_list_head(&ptdls_sta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + /* transmit buffered frames */ + while (rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + xmitframe_plist = get_next(xmitframe_plist); + rtw_list_delete(&pxmitframe->list); + + ptdls_sta->sleepq_len--; + ptdls_sta->sleepq_ac_len--; + if (ptdls_sta->sleepq_len > 0) { + pxmitframe->attrib.mdata = 1; + pxmitframe->attrib.eosp = 0; + } else { + pxmitframe->attrib.mdata = 0; + pxmitframe->attrib.eosp = 1; + } + pxmitframe->attrib.triggered = 1; + + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + } + + if (ptdls_sta->sleepq_len == 0) + RTW_INFO("no buffered packets for tdls to xmit\n"); + else { + RTW_INFO("error!psta->sleepq_len=%d\n", ptdls_sta->sleepq_len); + ptdls_sta->sleepq_len = 0; + } + + _exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL); + + } + + } + + return _SUCCESS; +} + +#ifdef CONFIG_TDLS_CH_SW +sint On_TDLS_Ch_Switch_Req(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + sint parsing_length; + PNDIS_802_11_VARIABLE_IEs pIE; + u8 FIXED_IE = 4; + u16 j; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + u8 zaddr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + u16 switch_time = TDLS_CH_SWITCH_TIME * 1000, switch_timeout = TDLS_CH_SWITCH_TIMEOUT * 1000; + u8 take_care_iqk; + + if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) { + RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__); + return _FAIL; + } + + ptdls_sta->ch_switch_time = switch_time; + ptdls_sta->ch_switch_timeout = switch_timeout; + + ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len + - prx_pkt_attrib->hdrlen + - prx_pkt_attrib->iv_len + - prx_pkt_attrib->icv_len + - LLC_HEADER_SIZE + - ETH_TYPE_LEN + - PAYLOAD_TYPE_LEN; + + pchsw_info->off_ch_num = *(ptr + 2); + + if ((*(ptr + 2) == 2) && (hal_is_band_support(padapter, BAND_ON_5G))) + pchsw_info->off_ch_num = 44; + + if (pchsw_info->off_ch_num != pmlmeext->cur_channel) + pchsw_info->delay_switch_back = _FALSE; + + /* Parsing information element */ + for (j = FIXED_IE; j < parsing_length;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j); + + switch (pIE->ElementID) { + case EID_SecondaryChnlOffset: + switch (*(pIE->data)) { + case EXTCHNL_OFFSET_UPPER: + pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + + case EXTCHNL_OFFSET_LOWER: + pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + + default: + pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + break; + case _LINK_ID_IE_: + break; + case _CH_SWITCH_TIMING_: + ptdls_sta->ch_switch_time = (RTW_GET_LE16(pIE->data) >= TDLS_CH_SWITCH_TIME * 1000) ? + RTW_GET_LE16(pIE->data) : TDLS_CH_SWITCH_TIME * 1000; + ptdls_sta->ch_switch_timeout = (RTW_GET_LE16(pIE->data + 2) >= TDLS_CH_SWITCH_TIMEOUT * 1000) ? + RTW_GET_LE16(pIE->data + 2) : TDLS_CH_SWITCH_TIMEOUT * 1000; + RTW_INFO("[TDLS] %s ch_switch_time:%d, ch_switch_timeout:%d\n" + , __FUNCTION__, RTW_GET_LE16(pIE->data), RTW_GET_LE16(pIE->data + 2)); + default: + break; + } + + j += (pIE->Length + 2); + } + +#ifndef CONFIG_TDLS_CH_SW_V2 + rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk); + if (take_care_iqk == _TRUE) { + u8 central_chnl; + u8 bw_mode; + + bw_mode = (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20; + central_chnl = rtw_get_center_ch(pchsw_info->off_ch_num, bw_mode, pchsw_info->ch_offset); + if (rtw_hal_ch_sw_iqk_info_search(padapter, central_chnl, bw_mode) < 0) { + if (!(pchsw_info->ch_sw_state & TDLS_CH_SWITCH_PREPARE_STATE)) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_PREPARE); + + return _FAIL; + } + } +#endif + + /* cancel ch sw monitor timer for responder */ + if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE)) + _cancel_timer_ex(&ptdls_sta->ch_sw_monitor_timer); + + if (_rtw_memcmp(pchsw_info->addr, zaddr, ETH_ALEN) == _TRUE) + _rtw_memcpy(pchsw_info->addr, ptdls_sta->cmn.mac_addr, ETH_ALEN); + + if (ATOMIC_READ(&pchsw_info->chsw_on) == _FALSE) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_START); + + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_RESP); + + return _SUCCESS; +} + +sint On_TDLS_Ch_Switch_Rsp(_adapter *padapter, union recv_frame *precv_frame, struct sta_info *ptdls_sta) +{ + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *ptr = precv_frame->u.hdr.rx_data; + struct rx_pkt_attrib *prx_pkt_attrib = &precv_frame->u.hdr.attrib; + sint parsing_length; + PNDIS_802_11_VARIABLE_IEs pIE; + u8 FIXED_IE = 4; + u16 status_code, j, switch_time, switch_timeout; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + int ret = _SUCCESS; + + if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) { + RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__); + return _SUCCESS; + } + + /* If we receive Unsolicited TDLS Channel Switch Response when channel switch is running, */ + /* we will go back to base channel and terminate this channel switch procedure */ + if (ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) { + if (pmlmeext->cur_channel != rtw_get_oper_ch(padapter)) { + RTW_INFO("[TDLS] Rx unsolicited channel switch response\n"); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_TO_BASE_CHNL); + goto exit; + } + } + + ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN; + parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len + - prx_pkt_attrib->hdrlen + - prx_pkt_attrib->iv_len + - prx_pkt_attrib->icv_len + - LLC_HEADER_SIZE + - ETH_TYPE_LEN + - PAYLOAD_TYPE_LEN; + + _rtw_memcpy(&status_code, ptr + 2, 2); + + if (status_code != 0) { + RTW_INFO("[TDLS] %s status_code:%d\n", __func__, status_code); + pchsw_info->ch_sw_state &= ~(TDLS_CH_SW_INITIATOR_STATE); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_END); + ret = _FAIL; + goto exit; + } + + /* Parsing information element */ + for (j = FIXED_IE; j < parsing_length;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j); + + switch (pIE->ElementID) { + case _LINK_ID_IE_: + break; + case _CH_SWITCH_TIMING_: + _rtw_memcpy(&switch_time, pIE->data, 2); + if (switch_time > ptdls_sta->ch_switch_time) + _rtw_memcpy(&ptdls_sta->ch_switch_time, &switch_time, 2); + + _rtw_memcpy(&switch_timeout, pIE->data + 2, 2); + if (switch_timeout > ptdls_sta->ch_switch_timeout) + _rtw_memcpy(&ptdls_sta->ch_switch_timeout, &switch_timeout, 2); + break; + default: + break; + } + + j += (pIE->Length + 2); + } + + if ((pmlmeext->cur_channel == rtw_get_oper_ch(padapter)) && + (pchsw_info->ch_sw_state & TDLS_WAIT_CH_RSP_STATE)) { + if (ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_TO_OFF_CHNL); + } + +exit: + return ret; +} +#endif /* CONFIG_TDLS_CH_SW */ + +#ifdef CONFIG_WFD +void wfd_ie_tdls(_adapter *padapter, u8 *pframe, u32 *pktlen) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info *pwfd_info = padapter->tdlsinfo.wfd_info; + u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 }; + u32 wfdielen = 0; + u16 v16 = 0; + + if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST)) + return; + + /* WFD OUI */ + wfdielen = 0; + wfdie[wfdielen++] = 0x50; + wfdie[wfdielen++] = 0x6F; + wfdie[wfdielen++] = 0x9A; + wfdie[wfdielen++] = 0x0A; /* WFA WFD v1.0 */ + + /* + * Commented by Albert 20110825 + * According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes + * 1. WFD Device Information + * 2. Associated BSSID ( Optional ) + * 3. Local IP Adress ( Optional ) + */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* available for WFD session + Preferred TDLS + WSD ( WFD Service Discovery ) */ + v16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL + | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_WSD; + RTW_PUT_BE16(wfdie + wfdielen, v16); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->tdls_rtsp_ctrlport); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + RTW_PUT_BE16(wfdie + wfdielen, 300); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0006); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == _TRUE) + _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN); + else + _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN); + + /* Local IP Address ATTR */ + wfdie[wfdielen++] = WFD_ATTR_LOCAL_IP_ADDR; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + RTW_PUT_BE16(wfdie + wfdielen, 0x0005); + wfdielen += 2; + + /* Version: */ + /* 0x01: Version1;IPv4 */ + wfdie[wfdielen++] = 0x01; + + /* IPv4 Address */ + _rtw_memcpy(wfdie + wfdielen, pwfd_info->ip_address, 4); + wfdielen += 4; + + pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, pktlen); + +} +#endif /* CONFIG_WFD */ + +void rtw_build_tdls_setup_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct pkt_attrib *pattrib = &pxmitframe->attrib; + int i = 0 ; + u32 time; + u8 *pframe_head; + + /* SNonce */ + if (pattrib->encrypt) { + for (i = 0; i < 8; i++) { + time = rtw_get_current_time(); + _rtw_memcpy(&ptdls_sta->SNonce[4 * i], (u8 *)&time, 4); + } + } + + pframe_head = pframe; /* For rtw_tdls_set_ht_cap() */ + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + + pframe = rtw_tdls_set_capability(padapter, pframe, pattrib); + pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib); + pframe = rtw_tdls_set_sup_ch(padapter, pframe, pattrib); + pframe = rtw_tdls_set_sup_reg_class(pframe, pattrib); + + if (pattrib->encrypt) + pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta); + + pframe = rtw_tdls_set_ext_cap(pframe, pattrib); + + if (pattrib->encrypt) { + pframe = rtw_tdls_set_ftie(ptxmgmt + , pframe + , pattrib + , NULL + , ptdls_sta->SNonce); + + pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta); + } + +#ifdef CONFIG_80211N_HT + /* Sup_reg_classes(optional) */ + if (pregistrypriv->ht_enable == _TRUE) + pframe = rtw_tdls_set_ht_cap(padapter, pframe_head, pattrib); +#endif + + pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib); + + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + + if ((pregistrypriv->wmm_enable == _TRUE) || (padapter->mlmepriv.htpriv.ht_option == _TRUE)) + pframe = rtw_tdls_set_qos_cap(pframe, pattrib); + +#ifdef CONFIG_80211AC_VHT + if ((padapter->mlmepriv.htpriv.ht_option == _TRUE) && (pmlmeext->cur_channel > 14) + && REGSTY_IS_11AC_ENABLE(pregistrypriv) + && is_supported_vht(pregistrypriv->wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + pframe = rtw_tdls_set_aid(padapter, pframe, pattrib); + pframe = rtw_tdls_set_vht_cap(padapter, pframe, pattrib); + } +#endif + +#ifdef CONFIG_WFD + if (padapter->wdinfo.wfd_tdls_enable == 1) + wfd_ie_tdls(padapter, pframe, &(pattrib->pktlen)); +#endif + +} + +void rtw_build_tdls_setup_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct pkt_attrib *pattrib = &pxmitframe->attrib; + u8 k; /* for random ANonce */ + u8 *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL; + u32 time; + u8 *pframe_head; + + if (pattrib->encrypt) { + for (k = 0; k < 8; k++) { + time = rtw_get_current_time(); + _rtw_memcpy(&ptdls_sta->ANonce[4 * k], (u8 *)&time, 4); + } + } + + pframe_head = pframe; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt); + + if (ptxmgmt->status_code != 0) { + RTW_INFO("[%s] status_code:%04x\n", __FUNCTION__, ptxmgmt->status_code); + return; + } + + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_capability(padapter, pframe, pattrib); + pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib); + pframe = rtw_tdls_set_sup_ch(padapter, pframe, pattrib); + pframe = rtw_tdls_set_sup_reg_class(pframe, pattrib); + + if (pattrib->encrypt) { + prsnie = pframe; + pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, _FALSE, ptdls_sta); + } + + pframe = rtw_tdls_set_ext_cap(pframe, pattrib); + + if (pattrib->encrypt) { + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) + wpa_tdls_generate_tpk(padapter, ptdls_sta); + + pftie = pframe; + pftie_mic = pframe + 4; + pframe = rtw_tdls_set_ftie(ptxmgmt + , pframe + , pattrib + , ptdls_sta->ANonce + , ptdls_sta->SNonce); + + ptimeout_ie = pframe; + pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _FALSE, ptdls_sta); + } + +#ifdef CONFIG_80211N_HT + /* Sup_reg_classes(optional) */ + if (pregistrypriv->ht_enable == _TRUE) + pframe = rtw_tdls_set_ht_cap(padapter, pframe_head, pattrib); +#endif + + pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib); + + plinkid_ie = pframe; + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + + /* Fill FTIE mic */ + if (pattrib->encrypt && rtw_tdls_is_driver_setup(padapter) == _TRUE) + wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic); + + if ((pregistrypriv->wmm_enable == _TRUE) || (padapter->mlmepriv.htpriv.ht_option == _TRUE)) + pframe = rtw_tdls_set_qos_cap(pframe, pattrib); + +#ifdef CONFIG_80211AC_VHT + if ((padapter->mlmepriv.htpriv.ht_option == _TRUE) && (pmlmeext->cur_channel > 14) + && REGSTY_IS_11AC_ENABLE(pregistrypriv) + && is_supported_vht(pregistrypriv->wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + pframe = rtw_tdls_set_aid(padapter, pframe, pattrib); + pframe = rtw_tdls_set_vht_cap(padapter, pframe, pattrib); + pframe = rtw_tdls_set_vht_op_mode_notify(padapter, pframe, pattrib, pmlmeext->cur_bwmode); + } +#endif + +#ifdef CONFIG_WFD + if (padapter->wdinfo.wfd_tdls_enable) + wfd_ie_tdls(padapter, pframe, &(pattrib->pktlen)); +#endif + +} + +void rtw_build_tdls_setup_cfm_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + unsigned int ie_len; + unsigned char *p; + u8 wmm_param_ele[24] = {0}; + u8 *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + + if (ptxmgmt->status_code != 0) + return; + + if (pattrib->encrypt) { + prsnie = pframe; + pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta); + } + + if (pattrib->encrypt) { + pftie = pframe; + pftie_mic = pframe + 4; + pframe = rtw_tdls_set_ftie(ptxmgmt + , pframe + , pattrib + , ptdls_sta->ANonce + , ptdls_sta->SNonce); + + ptimeout_ie = pframe; + pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta); + + if (rtw_tdls_is_driver_setup(padapter) == _TRUE) { + /* Start TPK timer */ + ptdls_sta->TPK_count = 0; + _set_timer(&ptdls_sta->TPK_timer, ONE_SEC); + } + } + + /* HT operation; todo */ + + plinkid_ie = pframe; + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + + if (pattrib->encrypt && (rtw_tdls_is_driver_setup(padapter) == _TRUE)) + wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic); + + if (ptdls_sta->qos_option == _TRUE) + pframe = rtw_tdls_set_wmm_params(padapter, pframe, pattrib); + +#ifdef CONFIG_80211AC_VHT + if ((padapter->mlmepriv.htpriv.ht_option == _TRUE) + && (ptdls_sta->vhtpriv.vht_option == _TRUE) && (pmlmeext->cur_channel > 14) + && REGSTY_IS_11AC_ENABLE(pregistrypriv) + && is_supported_vht(pregistrypriv->wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + pframe = rtw_tdls_set_vht_operation(padapter, pframe, pattrib, pmlmeext->cur_channel); + pframe = rtw_tdls_set_vht_op_mode_notify(padapter, pframe, pattrib, pmlmeext->cur_bwmode); + } +#endif +} + +void rtw_build_tdls_teardown_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + u8 *pftie = NULL, *pftie_mic = NULL, *plinkid_ie = NULL; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt); + + if (pattrib->encrypt) { + pftie = pframe; + pftie_mic = pframe + 4; + pframe = rtw_tdls_set_ftie(ptxmgmt + , pframe + , pattrib + , ptdls_sta->ANonce + , ptdls_sta->SNonce); + } + + plinkid_ie = pframe; + if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + + if (pattrib->encrypt && (rtw_tdls_is_driver_setup(padapter) == _TRUE)) + wpa_tdls_teardown_ftie_mic(ptdls_sta->tpk.kck, plinkid_ie, ptxmgmt->status_code, 1, 4, pftie, pftie_mic); +} + +void rtw_build_tdls_dis_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt) +{ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + +} + +void rtw_build_tdls_dis_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, u8 privacy) +{ + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + u8 *pframe_head, pktlen_index; + + pktlen_index = pattrib->pktlen; + pframe_head = pframe; + + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_PUBLIC); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_capability(padapter, pframe, pattrib); + + pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib); + + pframe = rtw_tdls_set_sup_ch(padapter, pframe, pattrib); + + if (privacy) + pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, _TRUE, NULL); + + pframe = rtw_tdls_set_ext_cap(pframe, pattrib); + + if (privacy) { + pframe = rtw_tdls_set_ftie(ptxmgmt, pframe, pattrib, NULL, NULL); + pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _TRUE, NULL); + } + +#ifdef CONFIG_80211N_HT + if (pregistrypriv->ht_enable == _TRUE) + pframe = rtw_tdls_set_ht_cap(padapter, pframe_head - pktlen_index, pattrib); +#endif + + pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib); + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + +} + + +void rtw_build_tdls_peer_traffic_indication_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + u8 AC_queue = 0; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + + if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + + /* PTI control */ + /* PU buffer status */ + if (ptdls_sta->uapsd_bk & BIT(1)) + AC_queue = BIT(0); + if (ptdls_sta->uapsd_be & BIT(1)) + AC_queue = BIT(1); + if (ptdls_sta->uapsd_vi & BIT(1)) + AC_queue = BIT(2); + if (ptdls_sta->uapsd_vo & BIT(1)) + AC_queue = BIT(3); + pframe = rtw_set_ie(pframe, _PTI_BUFFER_STATUS_, 1, &AC_queue, &(pattrib->pktlen)); + +} + +void rtw_build_tdls_peer_traffic_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt); + + if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); +} + +#ifdef CONFIG_TDLS_CH_SW +void rtw_build_tdls_ch_switch_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + u16 switch_time = TDLS_CH_SWITCH_TIME * 1000, switch_timeout = TDLS_CH_SWITCH_TIMEOUT * 1000; + + ptdls_sta->ch_switch_time = switch_time; + ptdls_sta->ch_switch_timeout = switch_timeout; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_target_ch(padapter, pframe, pattrib); + pframe = rtw_tdls_set_reg_class(pframe, pattrib, ptdls_sta); + + if (ptdlsinfo->chsw_info.ch_offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE) { + switch (ptdlsinfo->chsw_info.ch_offset) { + case HAL_PRIME_CHNL_OFFSET_LOWER: + pframe = rtw_tdls_set_second_channel_offset(pframe, pattrib, SCA); + break; + case HAL_PRIME_CHNL_OFFSET_UPPER: + pframe = rtw_tdls_set_second_channel_offset(pframe, pattrib, SCB); + break; + } + } + + if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + + pframe = rtw_tdls_set_ch_sw(pframe, pattrib, ptdls_sta); + +} + +void rtw_build_tdls_ch_switch_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, struct sta_info *ptdls_sta) +{ + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct sta_priv *pstapriv = &padapter->stapriv; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS); + pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt); + pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt); + + if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _FALSE); + else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) + pframe = rtw_tdls_set_linkid(padapter, pframe, pattrib, _TRUE); + + pframe = rtw_tdls_set_ch_sw(pframe, pattrib, ptdls_sta); +} +#endif + +#ifdef CONFIG_WFD +void rtw_build_tunneled_probe_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe) +{ + u8 i; + _adapter *iface = NULL; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct wifidirect_info *pwdinfo; + + u8 category = RTW_WLAN_CATEGORY_P2P; + u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a}; + u8 probe_req = 4; + u8 wfdielen = 0; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(probe_req), &(pattrib->pktlen)); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + pwdinfo = &iface->wdinfo; + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) { + wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } + } + } +} + +void rtw_build_tunneled_probe_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe) +{ + u8 i; + _adapter *iface = NULL; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct wifidirect_info *pwdinfo; + u8 category = RTW_WLAN_CATEGORY_P2P; + u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a}; + u8 probe_rsp = 5; + u8 wfdielen = 0; + + pframe = rtw_tdls_set_payload_type(pframe, pattrib); + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(probe_rsp), &(pattrib->pktlen)); + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && rtw_is_adapter_up(iface)) { + pwdinfo = &iface->wdinfo; + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) { + wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 1); + pframe += wfdielen; + pattrib->pktlen += wfdielen; + } + } + } +} +#endif /* CONFIG_WFD */ + +void _tdls_tpk_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + struct tdls_txmgmt txmgmt; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + ptdls_sta->TPK_count++; + /* TPK_timer expired in a second */ + /* Retry timer should set at least 301 sec. */ + if (ptdls_sta->TPK_count >= (ptdls_sta->TDLS_PeerKey_Lifetime - 3)) { + RTW_INFO("[TDLS] %s, Re-Setup TDLS link with "MAC_FMT" since TPK lifetime expires!\n", + __FUNCTION__, MAC_ARG(ptdls_sta->cmn.mac_addr)); + ptdls_sta->TPK_count = 0; + _rtw_memcpy(txmgmt.peer, ptdls_sta->cmn.mac_addr, ETH_ALEN); + issue_tdls_setup_req(ptdls_sta->padapter, &txmgmt, _FALSE); + } + + _set_timer(&ptdls_sta->TPK_timer, ONE_SEC); +} + +#ifdef CONFIG_TDLS_CH_SW +void _tdls_ch_switch_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + _adapter *padapter = ptdls_sta->padapter; + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_END_TO_BASE_CHNL); + RTW_INFO("[TDLS] %s, can't get traffic from op_ch:%d\n", __func__, rtw_get_oper_ch(padapter)); +} + +void _tdls_delay_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + _adapter *padapter = ptdls_sta->padapter; + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + + RTW_INFO("[TDLS] %s, op_ch:%d, tdls_state:0x%08x\n", __func__, rtw_get_oper_ch(padapter), ptdls_sta->tdls_sta_state); + pchsw_info->delay_switch_back = _TRUE; +} + +void _tdls_stay_on_base_chnl_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + _adapter *padapter = ptdls_sta->padapter; + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + + if (ptdls_sta != NULL) { + issue_tdls_ch_switch_req(padapter, ptdls_sta); + pchsw_info->ch_sw_state |= TDLS_WAIT_CH_RSP_STATE; + } +} + +void _tdls_ch_switch_monitor_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + _adapter *padapter = ptdls_sta->padapter; + struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info; + + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_CH_SW_END); + RTW_INFO("[TDLS] %s, does not receive ch sw req\n", __func__); +} + +#endif + +void _tdls_handshake_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + _adapter *padapter = NULL; + struct tdls_txmgmt txmgmt; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + _rtw_memcpy(txmgmt.peer, ptdls_sta->cmn.mac_addr, ETH_ALEN); + txmgmt.status_code = _RSON_TDLS_TEAR_UN_RSN_; + + if (ptdls_sta != NULL) { + padapter = ptdls_sta->padapter; + + RTW_INFO("[TDLS] Handshake time out\n"); + if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA); + else + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA_LOCALLY); + } +} + +void _tdls_pti_timer_hdl(void *FunctionContext) +{ + struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext; + _adapter *padapter = NULL; + struct tdls_txmgmt txmgmt; + + _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt)); + _rtw_memcpy(txmgmt.peer, ptdls_sta->cmn.mac_addr, ETH_ALEN); + txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_; + + if (ptdls_sta != NULL) { + padapter = ptdls_sta->padapter; + + if (ptdls_sta->tdls_sta_state & TDLS_WAIT_PTR_STATE) { + RTW_INFO("[TDLS] Doesn't receive PTR from peer dev:"MAC_FMT"; " + "Send TDLS Tear Down\n", MAC_ARG(ptdls_sta->cmn.mac_addr)); + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_TEARDOWN_STA); + } + } +} + +void rtw_init_tdls_timer(_adapter *padapter, struct sta_info *psta) +{ + psta->padapter = padapter; + rtw_init_timer(&psta->TPK_timer, padapter, _tdls_tpk_timer_hdl, psta); +#ifdef CONFIG_TDLS_CH_SW + rtw_init_timer(&psta->ch_sw_timer, padapter, _tdls_ch_switch_timer_hdl, psta); + rtw_init_timer(&psta->delay_timer, padapter, _tdls_delay_timer_hdl, psta); + rtw_init_timer(&psta->stay_on_base_chnl_timer, padapter, _tdls_stay_on_base_chnl_timer_hdl, psta); + rtw_init_timer(&psta->ch_sw_monitor_timer, padapter, _tdls_ch_switch_monitor_timer_hdl, psta); +#endif + rtw_init_timer(&psta->handshake_timer, padapter, _tdls_handshake_timer_hdl, psta); + rtw_init_timer(&psta->pti_timer, padapter, _tdls_pti_timer_hdl, psta); +} + +void rtw_cancel_tdls_timer(struct sta_info *psta) +{ + _cancel_timer_ex(&psta->TPK_timer); +#ifdef CONFIG_TDLS_CH_SW + _cancel_timer_ex(&psta->ch_sw_timer); + _cancel_timer_ex(&psta->delay_timer); + _cancel_timer_ex(&psta->stay_on_base_chnl_timer); + _cancel_timer_ex(&psta->ch_sw_monitor_timer); +#endif + _cancel_timer_ex(&psta->handshake_timer); + _cancel_timer_ex(&psta->pti_timer); +} + +void rtw_tdls_teardown_pre_hdl(_adapter *padapter, struct sta_info *psta) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + struct sta_priv *pstapriv = &padapter->stapriv; + _irqL irqL; + + rtw_cancel_tdls_timer(psta); + + _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); + if (ptdlsinfo->sta_cnt != 0) + ptdlsinfo->sta_cnt--; + _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); + + if (ptdlsinfo->sta_cnt < MAX_ALLOWED_TDLS_STA_NUM) { + ptdlsinfo->sta_maximum = _FALSE; + _rtw_memset(&ptdlsinfo->ss_record, 0x00, sizeof(struct tdls_ss_record)); + } + + if (ptdlsinfo->sta_cnt == 0) + rtw_tdls_set_link_established(padapter, _FALSE); + else + RTW_INFO("Remain tdls sta:%02x\n", ptdlsinfo->sta_cnt); +} + +void rtw_tdls_teardown_post_hdl(_adapter *padapter, struct sta_info *psta, u8 enqueue_cmd) +{ + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + + /* Clear cam */ + rtw_clearstakey_cmd(padapter, psta, enqueue_cmd); + + /* Update sta media status */ + if (enqueue_cmd) + rtw_sta_media_status_rpt_cmd(padapter, psta, 0); + else + rtw_sta_media_status_rpt(padapter, psta, 0); + + /* Set RCR if necessary */ + if (ptdlsinfo->sta_cnt == 0) { + if (enqueue_cmd) + rtw_tdls_cmd(padapter, NULL, TDLS_RS_RCR); + else + rtw_hal_rcr_set_chk_bssid(padapter, MLME_TDLS_NOLINK); + } + + /* Free tdls sta info */ + rtw_free_stainfo(padapter, psta); +} + +int rtw_tdls_is_driver_setup(_adapter *padapter) +{ + return padapter->tdlsinfo.driver_setup; +} + +const char *rtw_tdls_action_txt(enum TDLS_ACTION_FIELD action) +{ + switch (action) { + case TDLS_SETUP_REQUEST: + return "TDLS_SETUP_REQUEST"; + case TDLS_SETUP_RESPONSE: + return "TDLS_SETUP_RESPONSE"; + case TDLS_SETUP_CONFIRM: + return "TDLS_SETUP_CONFIRM"; + case TDLS_TEARDOWN: + return "TDLS_TEARDOWN"; + case TDLS_PEER_TRAFFIC_INDICATION: + return "TDLS_PEER_TRAFFIC_INDICATION"; + case TDLS_CHANNEL_SWITCH_REQUEST: + return "TDLS_CHANNEL_SWITCH_REQUEST"; + case TDLS_CHANNEL_SWITCH_RESPONSE: + return "TDLS_CHANNEL_SWITCH_RESPONSE"; + case TDLS_PEER_PSM_REQUEST: + return "TDLS_PEER_PSM_REQUEST"; + case TDLS_PEER_PSM_RESPONSE: + return "TDLS_PEER_PSM_RESPONSE"; + case TDLS_PEER_TRAFFIC_RESPONSE: + return "TDLS_PEER_TRAFFIC_RESPONSE"; + case TDLS_DISCOVERY_REQUEST: + return "TDLS_DISCOVERY_REQUEST"; + case TDLS_DISCOVERY_RESPONSE: + return "TDLS_DISCOVERY_RESPONSE"; + default: + return "UNKNOWN"; + } +} + +#endif /* CONFIG_TDLS */ diff --git a/core/rtw_vht.c b/core/rtw_vht.c new file mode 100644 index 0000000..55b1709 --- /dev/null +++ b/core/rtw_vht.c @@ -0,0 +1,1141 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_VHT_C + +#include +#include + +#ifdef CONFIG_80211AC_VHT +const u16 _vht_max_mpdu_len[] = { + 3895, + 7991, + 11454, + 0, +}; + +const u8 _vht_sup_ch_width_set_to_bw_cap[] = { + BW_CAP_80M, + BW_CAP_80M | BW_CAP_160M, + BW_CAP_80M | BW_CAP_160M | BW_CAP_80_80M, + 0, +}; + +#ifdef CONFIG_RTW_DEBUG +const char *const _vht_sup_ch_width_set_str[] = { + "80MHz", + "160MHz", + "160MHz & 80+80MHz", + "BW-RSVD", +}; + +void dump_vht_cap_ie_content(void *sel, const u8 *buf, u32 buf_len) +{ + if (buf_len != VHT_CAP_IE_LEN) { + RTW_PRINT_SEL(sel, "Invalid VHT capability IE len:%d != %d\n", buf_len, VHT_CAP_IE_LEN); + return; + } + + RTW_PRINT_SEL(sel, "cap_info:%02x %02x %02x %02x: MAX_MPDU_LEN:%u %s%s%s%s%s RX-STBC:%u MAX_AMPDU_LEN:%u\n" + , *(buf), *(buf + 1), *(buf + 2), *(buf + 3) + , vht_max_mpdu_len(GET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(buf)) + , vht_sup_ch_width_set_str(GET_VHT_CAPABILITY_ELE_CHL_WIDTH(buf)) + , GET_VHT_CAPABILITY_ELE_RX_LDPC(buf) ? " RX-LDPC" : "" + , GET_VHT_CAPABILITY_ELE_SHORT_GI80M(buf) ? " SGI-80" : "" + , GET_VHT_CAPABILITY_ELE_SHORT_GI160M(buf) ? " SGI-160" : "" + , GET_VHT_CAPABILITY_ELE_TX_STBC(buf) ? " TX-STBC" : "" + , GET_VHT_CAPABILITY_ELE_RX_STBC(buf) + , VHT_MAX_AMPDU_LEN(GET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(buf)) + ); +} + +void dump_vht_cap_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *vht_cap_ie; + sint vht_cap_ielen; + + vht_cap_ie = rtw_get_ie(ie, WLAN_EID_VHT_CAPABILITY, &vht_cap_ielen, ie_len); + if (!ie || vht_cap_ie != ie) + return; + + dump_vht_cap_ie_content(sel, vht_cap_ie + 2, vht_cap_ielen); +} + +const char *const _vht_op_ch_width_str[] = { + "20 or 40MHz", + "80MHz", + "160MHz", + "80+80MHz", + "BW-RSVD", +}; + +void dump_vht_op_ie_content(void *sel, const u8 *buf, u32 buf_len) +{ + if (buf_len != VHT_OP_IE_LEN) { + RTW_PRINT_SEL(sel, "Invalid VHT operation IE len:%d != %d\n", buf_len, VHT_OP_IE_LEN); + return; + } + + RTW_PRINT_SEL(sel, "%s, ch0:%u, ch1:%u\n" + , vht_op_ch_width_str(GET_VHT_OPERATION_ELE_CHL_WIDTH(buf)) + , GET_VHT_OPERATION_ELE_CENTER_FREQ1(buf) + , GET_VHT_OPERATION_ELE_CENTER_FREQ2(buf) + ); +} + +void dump_vht_op_ie(void *sel, const u8 *ie, u32 ie_len) +{ + const u8 *vht_op_ie; + sint vht_op_ielen; + + vht_op_ie = rtw_get_ie(ie, WLAN_EID_VHT_OPERATION, &vht_op_ielen, ie_len); + if (!ie || vht_op_ie != ie) + return; + + dump_vht_op_ie_content(sel, vht_op_ie + 2, vht_op_ielen); +} +#endif + +/* 20/40/80, ShortGI, MCS Rate */ +const u16 VHT_MCS_DATA_RATE[3][2][40] = { /* unit: 0.5M */ + { { + 13, 26, 39, 52, 78, 104, 117, 130, 156, 156, + 26, 52, 78, 104, 156, 208, 234, 260, 312, 312, + 39, 78, 117, 156, 234, 312, 351, 390, 468, 520, + 52, 104, 156, 208, 312, 416, 468, 520, 624, 624, + }, /* Long GI, 20MHz */ + { + 14, 29, 43, 58, 87, 116, 130, 144, 173, 173, + 29, 58, 87, 116, 173, 231, 260, 289, 347, 347, + 43, 87, 130, 173, 260, 347, 390, 433, 520, 578, + 58, 116, 173, 231, 347, 462, 520, 578, 693, 693, + } + }, /* Short GI, 20MHz */ + { { + 27, 54, 81, 108, 162, 216, 243, 270, 324, 360, + 54, 108, 162, 216, 324, 432, 486, 540, 648, 720, + 81, 162, 243, 324, 486, 648, 729, 810, 972, 1080, + 108, 216, 324, 432, 648, 864, 972, 1080, 1296, 1440, + }, /* Long GI, 40MHz */ + { + 30, 60, 90, 120, 180, 240, 270, 300, 360, 400, + 60, 120, 180, 240, 360, 480, 540, 600, 720, 800, + 90, 180, 270, 360, 540, 720, 810, 900, 1080, 1200, + 120, 240, 360, 480, 720, 960, 1080, 1200, 1440, 1600, + } + }, /* Short GI, 40MHz */ + { { + 59, 117, 176, 234, 351, 468, 527, 585, 702, 780, + 117, 234, 351, 468, 702, 936, 1053, 1170, 1404, 1560, + 176, 351, 527, 702, 1053, 1404, 1580, 1755, 2106, 2340, + 234, 468, 702, 936, 1404, 1872, 2106, 2340, 2808, 3120, + }, /* Long GI, 80MHz */ + { + 65, 130, 195, 260, 390, 520, 585, 650, 780, 867, + 130, 260, 390, 520, 780, 1040, 1170, 1300, 1560, 1734, + 195, 390, 585, 780, 1170, 1560, 1755, 1950, 2340, 2600, + 260, 520, 780, 1040, 1560, 2080, 2340, 2600, 3120, 3467, + } + } /* Short GI, 80MHz */ +}; + +u8 rtw_get_vht_highest_rate(u8 *pvht_mcs_map) +{ + u8 i, j; + u8 bit_map; + u8 vht_mcs_rate = 0; + + for (i = 0; i < 2; i++) { + if (pvht_mcs_map[i] != 0xff) { + for (j = 0; j < 8; j += 2) { + bit_map = (pvht_mcs_map[i] >> j) & 3; + + if (bit_map != 3) + vht_mcs_rate = MGN_VHT1SS_MCS7 + 10 * j / 2 + i * 40 + bit_map; /* VHT rate indications begin from 0x90 */ + } + } + } + + /* RTW_INFO("HighestVHTMCSRate is %x\n", vht_mcs_rate); */ + return vht_mcs_rate; +} + +u8 rtw_vht_mcsmap_to_nss(u8 *pvht_mcs_map) +{ + u8 i, j; + u8 bit_map; + u8 nss = 0; + + for (i = 0; i < 2; i++) { + if (pvht_mcs_map[i] != 0xff) { + for (j = 0; j < 8; j += 2) { + bit_map = (pvht_mcs_map[i] >> j) & 3; + + if (bit_map != 3) + nss++; + } + } + } + + /* RTW_INFO("%s : %dSS\n", __FUNCTION__, nss); */ + return nss; +} + +void rtw_vht_nss_to_mcsmap(u8 nss, u8 *target_mcs_map, u8 *cur_mcs_map) +{ + u8 i, j; + u8 cur_rate, target_rate; + + for (i = 0; i < 2; i++) { + target_mcs_map[i] = 0; + for (j = 0; j < 8; j += 2) { + cur_rate = (cur_mcs_map[i] >> j) & 3; + if (cur_rate == 3) /* 0x3 indicates not supported that num of SS */ + target_rate = 3; + else if (nss <= ((j / 2) + i * 4)) + target_rate = 3; + else + target_rate = cur_rate; + + target_mcs_map[i] |= (target_rate << j); + } + } + + /* RTW_INFO("%s : %dSS\n", __FUNCTION__, nss); */ +} + +u16 rtw_vht_mcs_to_data_rate(u8 bw, u8 short_GI, u8 vht_mcs_rate) +{ + if (vht_mcs_rate > MGN_VHT4SS_MCS9) + vht_mcs_rate = MGN_VHT4SS_MCS9; + /* RTW_INFO("bw=%d, short_GI=%d, ((vht_mcs_rate - MGN_VHT1SS_MCS0)&0x3f)=%d\n", bw, short_GI, ((vht_mcs_rate - MGN_VHT1SS_MCS0)&0x3f)); */ + return VHT_MCS_DATA_RATE[bw][short_GI][((vht_mcs_rate - MGN_VHT1SS_MCS0) & 0x3f)]; +} + +void rtw_vht_use_default_setting(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + BOOLEAN bHwLDPCSupport = _FALSE, bHwSTBCSupport = _FALSE; +#ifdef CONFIG_BEAMFORMING + BOOLEAN bHwSupportBeamformer = _FALSE, bHwSupportBeamformee = _FALSE; + u8 mu_bfer, mu_bfee; +#endif /* CONFIG_BEAMFORMING */ + u8 tx_nss, rx_nss; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + pvhtpriv->sgi_80m = TEST_FLAG(pregistrypriv->short_gi, BIT2) ? _TRUE : _FALSE; + + /* LDPC support */ + rtw_hal_get_def_var(padapter, HAL_DEF_RX_LDPC, (u8 *)&bHwLDPCSupport); + CLEAR_FLAGS(pvhtpriv->ldpc_cap); + if (bHwLDPCSupport) { + if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT0)) + SET_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_RX); + } + rtw_hal_get_def_var(padapter, HAL_DEF_TX_LDPC, (u8 *)&bHwLDPCSupport); + if (bHwLDPCSupport) { + if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT1)) + SET_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_TX); + } + if (pvhtpriv->ldpc_cap) + RTW_INFO("[VHT] Support LDPC = 0x%02X\n", pvhtpriv->ldpc_cap); + + /* STBC */ + rtw_hal_get_def_var(padapter, HAL_DEF_TX_STBC, (u8 *)&bHwSTBCSupport); + CLEAR_FLAGS(pvhtpriv->stbc_cap); + if (bHwSTBCSupport) { + if (TEST_FLAG(pregistrypriv->stbc_cap, BIT1)) + SET_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX); + } + rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)&bHwSTBCSupport); + if (bHwSTBCSupport) { + if (TEST_FLAG(pregistrypriv->stbc_cap, BIT0)) + SET_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_RX); + } + if (pvhtpriv->stbc_cap) + RTW_INFO("[VHT] Support STBC = 0x%02X\n", pvhtpriv->stbc_cap); + + /* Beamforming setting */ + CLEAR_FLAGS(pvhtpriv->beamform_cap); +#ifdef CONFIG_BEAMFORMING +#ifdef RTW_BEAMFORMING_VERSION_2 +#ifdef CONFIG_CONCURRENT_MODE + /* only enable beamforming in STA client mode */ + if (MLME_IS_STA(padapter) && !MLME_IS_GC(padapter)) +#else + if ((MLME_IS_AP(padapter) && !MLME_IS_GO(padapter)) || + (MLME_IS_STA(padapter) && !MLME_IS_GC(padapter))) +#endif +#endif + { + rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, + (u8 *)&bHwSupportBeamformer); + rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, + (u8 *)&bHwSupportBeamformee); + mu_bfer = _FALSE; + mu_bfee = _FALSE; + rtw_hal_get_def_var(padapter, HAL_DEF_VHT_MU_BEAMFORMER, &mu_bfer); + rtw_hal_get_def_var(padapter, HAL_DEF_VHT_MU_BEAMFORMEE, &mu_bfee); + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT0) && bHwSupportBeamformer) { +#ifdef CONFIG_CONCURRENT_MODE + if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) { + SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + RTW_INFO("[VHT] CONCURRENT AP Support Beamformer\n"); + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT(2)) + && (_TRUE == mu_bfer)) { + SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE); + RTW_INFO("[VHT] Support MU-MIMO AP\n"); + } + } else + RTW_INFO("[VHT] CONCURRENT not AP ;not allow Support Beamformer\n"); +#else + SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + RTW_INFO("[VHT] Support Beamformer\n"); + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT(2)) + && (_TRUE == mu_bfer) + && ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) { + SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE); + RTW_INFO("[VHT] Support MU-MIMO AP\n"); + } +#endif + } + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT1) && bHwSupportBeamformee) { + SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE); + RTW_INFO("[VHT] Support Beamformee\n"); + if (TEST_FLAG(pregistrypriv->beamform_cap, BIT(3)) + && (_TRUE == mu_bfee) + && ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)) { + SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE); + RTW_INFO("[VHT] Support MU-MIMO STA\n"); + } + } + } +#endif /* CONFIG_BEAMFORMING */ + + pvhtpriv->ampdu_len = pregistrypriv->ampdu_factor; + + tx_nss = GET_HAL_TX_NSS(padapter); + rx_nss = GET_HAL_RX_NSS(padapter); + + /* for now, vhtpriv.vht_mcs_map comes from RX NSS */ + rtw_vht_nss_to_mcsmap(rx_nss, pvhtpriv->vht_mcs_map, pregistrypriv->vht_rx_mcs_map); + pvhtpriv->vht_highest_rate = rtw_get_vht_highest_rate(pvhtpriv->vht_mcs_map); +} + +u64 rtw_vht_mcs_map_to_bitmap(u8 *mcs_map, u8 nss) +{ + u8 i, j, tmp; + u64 bitmap = 0; + u8 bits_nss = nss * 2; + + for (i = j = 0; i < bits_nss; i += 2, j += 10) { + /* every two bits means single sptial stream */ + tmp = (mcs_map[i / 8] >> i) & 3; + + switch (tmp) { + case 2: + bitmap = bitmap | ((u64)0x03ff << j); + break; + case 1: + bitmap = bitmap | ((u64)0x01ff << j); + break; + case 0: + bitmap = bitmap | ((u64)0x00ff << j); + break; + default: + break; + } + } + + RTW_INFO("vht_mcs_map=%02x %02x, nss=%u => bitmap=%016llx\n" + , mcs_map[0], mcs_map[1], nss, bitmap); + + return bitmap; +} + +#ifdef CONFIG_BEAMFORMING +void update_sta_vht_info_apmode_bf_cap(_adapter *padapter, struct sta_info *psta) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct vht_priv *pvhtpriv_ap = &pmlmepriv->vhtpriv; + struct vht_priv *pvhtpriv_sta = &psta->vhtpriv; + u16 cur_beamform_cap = 0; + + /* B11 SU Beamformer Capable, the target supports Beamformer and we are Beamformee */ + if (TEST_FLAG(pvhtpriv_ap->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE) && + GET_VHT_CAPABILITY_ELE_SU_BFEE(pvhtpriv_sta->vht_cap)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE); + /*Shift to BEAMFORMING_VHT_BEAMFORMER_STS_CAP*/ + SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFEE_STS_CAP(pvhtpriv_sta->vht_cap) << 8); + } + + /* B12 SU Beamformee Capable, the target supports Beamformee and we are Beamformer */ + if (TEST_FLAG(pvhtpriv_ap->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE) && + GET_VHT_CAPABILITY_ELE_SU_BFER(pvhtpriv_sta->vht_cap)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + /*Shit to BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM*/ + SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pvhtpriv_sta->vht_cap) << 12); + } + + if (cur_beamform_cap) + RTW_INFO("Current STA(%d) VHT Beamforming Setting = %02X\n", psta->cmn.aid, cur_beamform_cap); + + pvhtpriv_sta->beamform_cap = cur_beamform_cap; + psta->cmn.bf_info.vht_beamform_cap = cur_beamform_cap; +} +#endif + +void update_sta_vht_info_apmode(_adapter *padapter, void *sta) +{ + struct sta_info *psta = (struct sta_info *)sta; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct vht_priv *pvhtpriv_ap = &pmlmepriv->vhtpriv; + struct vht_priv *pvhtpriv_sta = &psta->vhtpriv; + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0; + s8 bw_mode = -1; + u8 *pcap_mcs; + + if (pvhtpriv_sta->vht_option == _FALSE) + return; + + if (pvhtpriv_sta->op_present) { + switch (GET_VHT_OPERATION_ELE_CHL_WIDTH(pvhtpriv_sta->vht_op)) { + case 1: /* 80MHz */ + case 2: /* 160MHz */ + case 3: /* 80+80 */ + bw_mode = CHANNEL_WIDTH_80; /* only support up to 80MHz for now */ + break; + } + } + + if (pvhtpriv_sta->notify_present) + bw_mode = GET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(&pvhtpriv_sta->vht_op_mode_notify); + else if (MLME_IS_AP(padapter)) { + /* for VHT client without Operating Mode Notify IE; minimal 80MHz */ + if (bw_mode < CHANNEL_WIDTH_80) + bw_mode = CHANNEL_WIDTH_80; + } + + if (bw_mode != -1) + psta->cmn.bw_mode = bw_mode; /* update bw_mode only if get value from VHT IEs */ + + psta->cmn.ra_info.is_vht_enable = _TRUE; + + /* B4 Rx LDPC */ + if (TEST_FLAG(pvhtpriv_ap->ldpc_cap, LDPC_VHT_ENABLE_TX) && + GET_VHT_CAPABILITY_ELE_RX_LDPC(pvhtpriv_sta->vht_cap)) { + SET_FLAG(cur_ldpc_cap, (LDPC_VHT_ENABLE_TX | LDPC_VHT_CAP_TX)); + RTW_INFO("Current STA(%d) VHT LDPC = %02X\n", psta->cmn.aid, cur_ldpc_cap); + } + pvhtpriv_sta->ldpc_cap = cur_ldpc_cap; + + if (psta->cmn.bw_mode > pmlmeext->cur_bwmode) + psta->cmn.bw_mode = pmlmeext->cur_bwmode; + + if (psta->cmn.bw_mode == CHANNEL_WIDTH_80) { + /* B5 Short GI for 80 MHz */ + pvhtpriv_sta->sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI80M(pvhtpriv_sta->vht_cap) & pvhtpriv_ap->sgi_80m) ? _TRUE : _FALSE; + /* RTW_INFO("Current STA ShortGI80MHz = %d\n", pvhtpriv_sta->sgi_80m); */ + } else if (psta->cmn.bw_mode >= CHANNEL_WIDTH_160) { + /* B5 Short GI for 80 MHz */ + pvhtpriv_sta->sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI160M(pvhtpriv_sta->vht_cap) & pvhtpriv_ap->sgi_80m) ? _TRUE : _FALSE; + /* RTW_INFO("Current STA ShortGI160MHz = %d\n", pvhtpriv_sta->sgi_80m); */ + } + + /* B8 B9 B10 Rx STBC */ + if (TEST_FLAG(pvhtpriv_ap->stbc_cap, STBC_VHT_ENABLE_TX) && + GET_VHT_CAPABILITY_ELE_RX_STBC(pvhtpriv_sta->vht_cap)) { + SET_FLAG(cur_stbc_cap, (STBC_VHT_ENABLE_TX | STBC_VHT_CAP_TX)); + RTW_INFO("Current STA(%d) VHT STBC = %02X\n", psta->cmn.aid, cur_stbc_cap); + } + pvhtpriv_sta->stbc_cap = cur_stbc_cap; + +#ifdef CONFIG_BEAMFORMING + update_sta_vht_info_apmode_bf_cap(padapter, psta); +#endif + + /* B23 B24 B25 Maximum A-MPDU Length Exponent */ + pvhtpriv_sta->ampdu_len = GET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pvhtpriv_sta->vht_cap); + + pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pvhtpriv_sta->vht_cap); + _rtw_memcpy(pvhtpriv_sta->vht_mcs_map, pcap_mcs, 2); + pvhtpriv_sta->vht_highest_rate = rtw_get_vht_highest_rate(pvhtpriv_sta->vht_mcs_map); +} + +void update_hw_vht_param(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 ht_AMPDU_len; + + ht_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03; + + if (pvhtpriv->ampdu_len > ht_AMPDU_len) + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&pvhtpriv->ampdu_len)); +} + +#ifdef ROKU_PRIVATE +u8 VHT_get_ss_from_map(u8 *vht_mcs_map) +{ + u8 i, j; + u8 ss = 0; + + for (i = 0; i < 2; i++) { + if (vht_mcs_map[i] != 0xff) { + for (j = 0; j < 8; j += 2) { + if (((vht_mcs_map[i] >> j) & 0x03) == 0x03) + break; + ss++; + } + } + + } + +return ss; +} + +void VHT_caps_handler_infra_ap(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv_infra_ap *pvhtpriv = &pmlmepriv->vhtpriv_infra_ap; + u8 cur_stbc_cap_infra_ap = 0; + u16 cur_beamform_cap_infra_ap = 0; + u8 *pcap_mcs; + u8 *pcap_mcs_tx; + u8 Rx_ss = 0, Tx_ss = 0; + + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pIE == NULL) + return; + + pmlmeinfo->ht_vht_received |= BIT(1); + + pvhtpriv->ldpc_cap_infra_ap = GET_VHT_CAPABILITY_ELE_RX_LDPC(pIE->data); + + if (GET_VHT_CAPABILITY_ELE_RX_STBC(pIE->data)) + SET_FLAG(cur_stbc_cap_infra_ap, STBC_VHT_ENABLE_RX); + if (GET_VHT_CAPABILITY_ELE_TX_STBC(pIE->data)) + SET_FLAG(cur_stbc_cap_infra_ap, STBC_VHT_ENABLE_TX); + pvhtpriv->stbc_cap_infra_ap = cur_stbc_cap_infra_ap; + + /*store ap info for channel bandwidth*/ + pvhtpriv->channel_width_infra_ap = GET_VHT_CAPABILITY_ELE_CHL_WIDTH(pIE->data); + + /*check B11: SU Beamformer Capable and B12: SU Beamformee B19: MU Beamformer B20:MU Beamformee*/ + if (GET_VHT_CAPABILITY_ELE_SU_BFER(pIE->data)) + SET_FLAG(cur_beamform_cap_infra_ap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + if (GET_VHT_CAPABILITY_ELE_SU_BFEE(pIE->data)) + SET_FLAG(cur_beamform_cap_infra_ap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE); + if (GET_VHT_CAPABILITY_ELE_MU_BFER(pIE->data)) + SET_FLAG(cur_beamform_cap_infra_ap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE); + if (GET_VHT_CAPABILITY_ELE_MU_BFEE(pIE->data)) + SET_FLAG(cur_beamform_cap_infra_ap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE); + pvhtpriv->beamform_cap_infra_ap = cur_beamform_cap_infra_ap; + + /*store information about vht_mcs_set*/ + pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pIE->data); + pcap_mcs_tx = GET_VHT_CAPABILITY_ELE_TX_MCS(pIE->data); + _rtw_memcpy(pvhtpriv->vht_mcs_map_infra_ap, pcap_mcs, 2); + _rtw_memcpy(pvhtpriv->vht_mcs_map_tx_infra_ap, pcap_mcs_tx, 2); + + Rx_ss = VHT_get_ss_from_map(pvhtpriv->vht_mcs_map_infra_ap); + Tx_ss = VHT_get_ss_from_map(pvhtpriv->vht_mcs_map_tx_infra_ap); + if (Rx_ss >= Tx_ss) { + pvhtpriv->number_of_streams_infra_ap = Rx_ss; + } else{ + pvhtpriv->number_of_streams_infra_ap = Tx_ss; + } + +} +#endif /* ROKU_PRIVATE */ + +void VHT_caps_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0, rx_nss = 0; + u16 cur_beamform_cap = 0; + u8 *pcap_mcs; + + if (pIE == NULL) + return; + + if (pvhtpriv->vht_option == _FALSE) + return; + + pmlmeinfo->VHT_enable = 1; + + /* B4 Rx LDPC */ + if (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_TX) && + GET_VHT_CAPABILITY_ELE_RX_LDPC(pIE->data)) { + SET_FLAG(cur_ldpc_cap, (LDPC_VHT_ENABLE_TX | LDPC_VHT_CAP_TX)); + RTW_INFO("Current VHT LDPC Setting = %02X\n", cur_ldpc_cap); + } + pvhtpriv->ldpc_cap = cur_ldpc_cap; + + /* B5 Short GI for 80 MHz */ + pvhtpriv->sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI80M(pIE->data) & pvhtpriv->sgi_80m) ? _TRUE : _FALSE; + /* RTW_INFO("Current ShortGI80MHz = %d\n", pvhtpriv->sgi_80m); */ + + /* B8 B9 B10 Rx STBC */ + if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX) && + GET_VHT_CAPABILITY_ELE_RX_STBC(pIE->data)) { + SET_FLAG(cur_stbc_cap, (STBC_VHT_ENABLE_TX | STBC_VHT_CAP_TX)); + RTW_INFO("Current VHT STBC Setting = %02X\n", cur_stbc_cap); + } + pvhtpriv->stbc_cap = cur_stbc_cap; +#ifdef CONFIG_BEAMFORMING +#ifdef RTW_BEAMFORMING_VERSION_2 + /* + * B11 SU Beamformer Capable, + * the target supports Beamformer and we are Beamformee + */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE) + && GET_VHT_CAPABILITY_ELE_SU_BFER(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE); + + /* Shift to BEAMFORMING_VHT_BEAMFORMEE_STS_CAP */ + SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFEE_STS_CAP(pIE->data) << 8); + + /* + * B19 MU Beamformer Capable, + * the target supports Beamformer and we are Beamformee + */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE) + && GET_VHT_CAPABILITY_ELE_MU_BFER(pIE->data)) + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE); + } + + /* + * B12 SU Beamformee Capable, + * the target supports Beamformee and we are Beamformer + */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE) + && GET_VHT_CAPABILITY_ELE_SU_BFEE(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + + /* Shit to BEAMFORMING_VHT_BEAMFORMER_SOUND_DIM */ + SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pIE->data) << 12); + + /* + * B20 MU Beamformee Capable, + * the target supports Beamformee and we are Beamformer + */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE) + && GET_VHT_CAPABILITY_ELE_MU_BFEE(pIE->data)) + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE); + } + + pvhtpriv->beamform_cap = cur_beamform_cap; + RTW_INFO("Current VHT Beamforming Setting=0x%04X\n", cur_beamform_cap); +#else /* !RTW_BEAMFORMING_VERSION_2 */ + /* B11 SU Beamformer Capable, the target supports Beamformer and we are Beamformee */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE) && + GET_VHT_CAPABILITY_ELE_SU_BFEE(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE); + /*Shift to BEAMFORMING_VHT_BEAMFORMER_STS_CAP*/ + SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFEE_STS_CAP(pIE->data) << 8); + } + + /* B12 SU Beamformee Capable, the target supports Beamformee and we are Beamformer */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE) && + GET_VHT_CAPABILITY_ELE_SU_BFER(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE); + /*Shit to BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM*/ + SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pIE->data) << 12); + + } + pvhtpriv->beamform_cap = cur_beamform_cap; + if (cur_beamform_cap) + RTW_INFO("Current VHT Beamforming Setting = %02X\n", cur_beamform_cap); +#endif /* !RTW_BEAMFORMING_VERSION_2 */ +#endif /* CONFIG_BEAMFORMING */ + /* B23 B24 B25 Maximum A-MPDU Length Exponent */ + pvhtpriv->ampdu_len = GET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pIE->data); + + pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pIE->data); + rx_nss = GET_HAL_RX_NSS(padapter); + rtw_vht_nss_to_mcsmap(rx_nss, pvhtpriv->vht_mcs_map, pcap_mcs); + pvhtpriv->vht_highest_rate = rtw_get_vht_highest_rate(pvhtpriv->vht_mcs_map); +} + +void VHT_operation_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + + if (pIE == NULL) + return; + + if (pvhtpriv->vht_option == _FALSE) + return; +} + +void rtw_process_vht_op_mode_notify(_adapter *padapter, u8 *pframe, void *sta) +{ + struct sta_info *psta = (struct sta_info *)sta; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct registry_priv *regsty = adapter_to_regsty(padapter); + u8 target_bw; + u8 target_rxss, current_rxss; + u8 update_ra = _FALSE; + u8 tx_nss = 0; + + if (pvhtpriv->vht_option == _FALSE) + return; + + target_bw = GET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(pframe); + tx_nss = GET_HAL_TX_NSS(padapter); + target_rxss = rtw_min(tx_nss, (GET_VHT_OPERATING_MODE_FIELD_RX_NSS(pframe) + 1)); + + if (target_bw != psta->cmn.bw_mode) { + if (hal_is_bw_support(padapter, target_bw) + && REGSTY_IS_BW_5G_SUPPORT(regsty, target_bw) + ) { + update_ra = _TRUE; + psta->cmn.bw_mode = target_bw; + } + } + + current_rxss = rtw_vht_mcsmap_to_nss(psta->vhtpriv.vht_mcs_map); + if (target_rxss != current_rxss) { + u8 vht_mcs_map[2] = {}; + + update_ra = _TRUE; + + rtw_vht_nss_to_mcsmap(target_rxss, vht_mcs_map, psta->vhtpriv.vht_mcs_map); + _rtw_memcpy(psta->vhtpriv.vht_mcs_map, vht_mcs_map, 2); + + rtw_hal_update_sta_ra_info(padapter, psta); + } + + if (update_ra) + rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta); +} + +u32 rtw_build_vht_operation_ie(_adapter *padapter, u8 *pbuf, u8 channel) +{ + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + /* struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; */ + u8 ChnlWidth, center_freq, bw_mode; + u32 len = 0; + u8 operation[5]; + + _rtw_memset(operation, 0, 5); + + bw_mode = REGSTY_BW_5G(pregistrypriv); /* TODO: control op bw with other info */ + + if (hal_chk_bw_cap(padapter, BW_CAP_80M | BW_CAP_160M) + && REGSTY_BW_5G(pregistrypriv) >= CHANNEL_WIDTH_80 + ) { + center_freq = rtw_get_center_ch(channel, bw_mode, HAL_PRIME_CHNL_OFFSET_LOWER); + ChnlWidth = 1; + } else { + center_freq = 0; + ChnlWidth = 0; + } + + + SET_VHT_OPERATION_ELE_CHL_WIDTH(operation, ChnlWidth); + /* center frequency */ + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(operation, center_freq);/* Todo: need to set correct center channel */ + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(operation, 0); + + _rtw_memcpy(operation + 3, pvhtpriv->vht_mcs_map, 2); + + rtw_set_ie(pbuf, EID_VHTOperation, 5, operation, &len); + + return len; +} + +u32 rtw_build_vht_op_mode_notify_ie(_adapter *padapter, u8 *pbuf, u8 bw) +{ + /* struct registry_priv *pregistrypriv = &padapter->registrypriv; */ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + u32 len = 0; + u8 opmode = 0; + u8 chnl_width, rx_nss; + + chnl_width = bw; + rx_nss = rtw_vht_mcsmap_to_nss(pvhtpriv->vht_mcs_map); + + SET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(&opmode, chnl_width); + SET_VHT_OPERATING_MODE_FIELD_RX_NSS(&opmode, (rx_nss - 1)); + SET_VHT_OPERATING_MODE_FIELD_RX_NSS_TYPE(&opmode, 0); /* Todo */ + + pvhtpriv->vht_op_mode_notify = opmode; + + pbuf = rtw_set_ie(pbuf, EID_OpModeNotification, 1, &opmode, &len); + + return len; +} + +u32 rtw_build_vht_cap_ie(_adapter *padapter, u8 *pbuf) +{ + u8 bw, rf_num, rx_stbc_nss = 0; + u16 HighestRate; + u8 *pcap, *pcap_mcs; + u32 len = 0; + u32 rx_packet_offset, max_recvbuf_sz; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + pcap = pvhtpriv->vht_cap; + _rtw_memset(pcap, 0, 32); + + /* B0 B1 Maximum MPDU Length */ + rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset); + rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz); + + RTW_DBG("%s, line%d, Available RX buf size = %d bytes\n", __FUNCTION__, __LINE__, max_recvbuf_sz - rx_packet_offset); + + if ((max_recvbuf_sz - rx_packet_offset) >= 11454) { + SET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(pcap, 2); + RTW_INFO("%s, line%d, Set MAX MPDU len = 11454 bytes\n", __FUNCTION__, __LINE__); + } else if ((max_recvbuf_sz - rx_packet_offset) >= 7991) { + SET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(pcap, 1); + RTW_INFO("%s, line%d, Set MAX MPDU len = 7991 bytes\n", __FUNCTION__, __LINE__); + } else if ((max_recvbuf_sz - rx_packet_offset) >= 3895) { + SET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(pcap, 0); + RTW_INFO("%s, line%d, Set MAX MPDU len = 3895 bytes\n", __FUNCTION__, __LINE__); + } else + RTW_ERR("%s, line%d, Error!! Available RX buf size < 3895 bytes\n", __FUNCTION__, __LINE__); + + /* B2 B3 Supported Channel Width Set */ + if (hal_chk_bw_cap(padapter, BW_CAP_160M) && REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_160)) { + if (hal_chk_bw_cap(padapter, BW_CAP_80_80M) && REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_80_80)) + SET_VHT_CAPABILITY_ELE_CHL_WIDTH(pcap, 2); + else + SET_VHT_CAPABILITY_ELE_CHL_WIDTH(pcap, 1); + } else + SET_VHT_CAPABILITY_ELE_CHL_WIDTH(pcap, 0); + + /* B4 Rx LDPC */ + if (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_RX)) { + SET_VHT_CAPABILITY_ELE_RX_LDPC(pcap, 1); + RTW_INFO("[VHT] Declare supporting RX LDPC\n"); + } + + /* B5 ShortGI for 80MHz */ + SET_VHT_CAPABILITY_ELE_SHORT_GI80M(pcap, pvhtpriv->sgi_80m ? 1 : 0); /* We can receive Short GI of 80M */ + if (pvhtpriv->sgi_80m) + RTW_INFO("[VHT] Declare supporting SGI 80MHz\n"); + + /* B6 ShortGI for 160MHz */ + /* SET_VHT_CAPABILITY_ELE_SHORT_GI160M(pcap, pvhtpriv->sgi_80m? 1 : 0); */ + + /* B7 Tx STBC */ + if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX)) { + SET_VHT_CAPABILITY_ELE_TX_STBC(pcap, 1); + RTW_INFO("[VHT] Declare supporting TX STBC\n"); + } + + /* B8 B9 B10 Rx STBC */ + if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_RX)) { + rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)(&rx_stbc_nss)); + + SET_VHT_CAPABILITY_ELE_RX_STBC(pcap, rx_stbc_nss); + RTW_INFO("[VHT] Declare supporting RX STBC = %d\n", rx_stbc_nss); + } + #ifdef CONFIG_BEAMFORMING + /* B11 SU Beamformer Capable */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)) { + SET_VHT_CAPABILITY_ELE_SU_BFER(pcap, 1); + RTW_INFO("[VHT] Declare supporting SU Bfer\n"); + /* B16 17 18 Number of Sounding Dimensions */ + rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMER_CAP, (u8 *)&rf_num); + SET_VHT_CAPABILITY_ELE_SOUNDING_DIMENSIONS(pcap, rf_num); + /* B19 MU Beamformer Capable */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE)) { + SET_VHT_CAPABILITY_ELE_MU_BFER(pcap, 1); + RTW_INFO("[VHT] Declare supporting MU Bfer\n"); + } + } + + /* B12 SU Beamformee Capable */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)) { + SET_VHT_CAPABILITY_ELE_SU_BFEE(pcap, 1); + RTW_INFO("[VHT] Declare supporting SU Bfee\n"); + + rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMEE_CAP, (u8 *)&rf_num); + + /* IOT action suggested by Yu Chen 2017/3/3 */ +#ifdef CONFIG_80211AC_VHT + if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_BROADCOM) && + !pvhtpriv->ap_bf_cap.is_mu_bfer && + pvhtpriv->ap_bf_cap.su_sound_dim == 2) + rf_num = (rf_num >= 2 ? 2 : rf_num); +#endif + /* B13 14 15 Compressed Steering Number of Beamformer Antennas Supported */ + SET_VHT_CAPABILITY_ELE_BFER_ANT_SUPP(pcap, rf_num); + /* B20 SU Beamformee Capable */ + if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE)) { + SET_VHT_CAPABILITY_ELE_MU_BFEE(pcap, 1); + RTW_INFO("[VHT] Declare supporting MU Bfee\n"); + } + } + #endif/*CONFIG_BEAMFORMING*/ + + /* B21 VHT TXOP PS */ + SET_VHT_CAPABILITY_ELE_TXOP_PS(pcap, 0); + /* B22 +HTC-VHT Capable */ + SET_VHT_CAPABILITY_ELE_HTC_VHT(pcap, 1); + /* B23 24 25 Maximum A-MPDU Length Exponent */ + if (pregistrypriv->ampdu_factor != 0xFE) + SET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pcap, pregistrypriv->ampdu_factor); + else + SET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pcap, 7); + /* B26 27 VHT Link Adaptation Capable */ + SET_VHT_CAPABILITY_ELE_LINK_ADAPTION(pcap, 0); + + pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pcap); + _rtw_memcpy(pcap_mcs, pvhtpriv->vht_mcs_map, 2); + + pcap_mcs = GET_VHT_CAPABILITY_ELE_TX_MCS(pcap); + _rtw_memcpy(pcap_mcs, pvhtpriv->vht_mcs_map, 2); + + /* find the largest bw supported by both registry and hal */ + bw = hal_largest_bw(padapter, REGSTY_BW_5G(pregistrypriv)); + + HighestRate = VHT_MCS_DATA_RATE[bw][pvhtpriv->sgi_80m][((pvhtpriv->vht_highest_rate - MGN_VHT1SS_MCS0) & 0x3f)]; + HighestRate = (HighestRate + 1) >> 1; + + SET_VHT_CAPABILITY_ELE_MCS_RX_HIGHEST_RATE(pcap, HighestRate); /* indicate we support highest rx rate is 600Mbps. */ + SET_VHT_CAPABILITY_ELE_MCS_TX_HIGHEST_RATE(pcap, HighestRate); /* indicate we support highest tx rate is 600Mbps. */ + + pbuf = rtw_set_ie(pbuf, EID_VHTCapability, 12, pcap, &len); + + return len; +} + +u32 rtw_restructure_vht_ie(_adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + RT_CHANNEL_INFO *chset = rfctl->channel_set; + u32 ielen; + u8 max_bw; + u8 oper_ch, oper_bw = CHANNEL_WIDTH_20, oper_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + u8 *out_vht_op_ie, *ht_op_ie, *vht_cap_ie, *vht_op_ie; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + + rtw_vht_use_default_setting(padapter); + + ht_op_ie = rtw_get_ie(in_ie + 12, WLAN_EID_HT_OPERATION, &ielen, in_len - 12); + if (!ht_op_ie || ielen != HT_OP_IE_LEN) + goto exit; + vht_cap_ie = rtw_get_ie(in_ie + 12, EID_VHTCapability, &ielen, in_len - 12); + if (!vht_cap_ie || ielen != VHT_CAP_IE_LEN) + goto exit; + vht_op_ie = rtw_get_ie(in_ie + 12, EID_VHTOperation, &ielen, in_len - 12); + if (!vht_op_ie || ielen != VHT_OP_IE_LEN) + goto exit; + + /* VHT Capabilities element */ + *pout_len += rtw_build_vht_cap_ie(padapter, out_ie + *pout_len); + + + /* VHT Operation element */ + out_vht_op_ie = out_ie + *pout_len; + rtw_set_ie(out_vht_op_ie, EID_VHTOperation, VHT_OP_IE_LEN, vht_op_ie + 2 , pout_len); + + /* get primary channel from HT_OP_IE */ + oper_ch = GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2); + + /* find the largest bw supported by both registry and hal */ + max_bw = hal_largest_bw(padapter, REGSTY_BW_5G(pregistrypriv)); + + if (max_bw >= CHANNEL_WIDTH_40) { + /* get bw offset form HT_OP_IE */ + if (GET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2)) { + switch (GET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2)) { + case SCA: + oper_bw = CHANNEL_WIDTH_40; + oper_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + case SCB: + oper_bw = CHANNEL_WIDTH_40; + oper_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + } + } + + if (oper_bw == CHANNEL_WIDTH_40) { + switch (GET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2)) { + case 1: /* 80MHz */ + case 2: /* 160MHz */ + case 3: /* 80+80 */ + oper_bw = CHANNEL_WIDTH_80; /* only support up to 80MHz for now */ + break; + } + + oper_bw = rtw_min(oper_bw, max_bw); + + /* try downgrage bw to fit in channel plan setting */ + while (!rtw_chset_is_chbw_valid(chset, oper_ch, oper_bw, oper_offset, 1, 1) + || (IS_DFS_SLAVE_WITH_RD(rfctl) + && !rtw_rfctl_dfs_domain_unknown(rfctl) + && rtw_chset_is_chbw_non_ocp(chset, oper_ch, oper_bw, oper_offset)) + ) { + oper_bw--; + if (oper_bw == CHANNEL_WIDTH_20) { + oper_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + } + } + } + + rtw_warn_on(!rtw_chset_is_chbw_valid(chset, oper_ch, oper_bw, oper_offset, 1, 1)); + if (IS_DFS_SLAVE_WITH_RD(rfctl) && !rtw_rfctl_dfs_domain_unknown(rfctl)) + rtw_warn_on(rtw_chset_is_chbw_non_ocp(chset, oper_ch, oper_bw, oper_offset)); + + /* update VHT_OP_IE */ + if (oper_bw < CHANNEL_WIDTH_80) { + SET_VHT_OPERATION_ELE_CHL_WIDTH(out_vht_op_ie + 2, 0); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(out_vht_op_ie + 2, 0); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(out_vht_op_ie + 2, 0); + } else if (oper_bw == CHANNEL_WIDTH_80) { + u8 cch = rtw_get_center_ch(oper_ch, oper_bw, oper_offset); + + SET_VHT_OPERATION_ELE_CHL_WIDTH(out_vht_op_ie + 2, 1); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(out_vht_op_ie + 2, cch); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(out_vht_op_ie + 2, 0); + } else { + RTW_ERR(FUNC_ADPT_FMT" unsupported BW:%u\n", FUNC_ADPT_ARG(padapter), oper_bw); + rtw_warn_on(1); + } + + /* Operating Mode Notification element */ + *pout_len += rtw_build_vht_op_mode_notify_ie(padapter, out_ie + *pout_len, oper_bw); + + pvhtpriv->vht_option = _TRUE; + +exit: + return pvhtpriv->vht_option; + +} + +void VHTOnAssocRsp(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct vht_priv *pvhtpriv = &pmlmepriv->vhtpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + u8 ht_AMPDU_len; + + RTW_INFO("%s\n", __FUNCTION__); + + if (!pmlmeinfo->HT_enable) + return; + + if (!pmlmeinfo->VHT_enable) + return; + + ht_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03; + + if (pvhtpriv->ampdu_len > ht_AMPDU_len) + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&pvhtpriv->ampdu_len)); + + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MAX_TIME, (u8 *)(&pvhtpriv->vht_highest_rate)); +} + +#ifdef CONFIG_AP_MODE +void rtw_vht_ies_attach(_adapter *padapter, WLAN_BSSID_EX *pnetwork) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u8 cap_len, operation_len; + uint len = 0; + sint ie_len = 0; + u8 *p = NULL; + + p = rtw_get_ie(pnetwork->IEs + _BEACON_IE_OFFSET_, EID_VHTCapability, &ie_len, + (pnetwork->IELength - _BEACON_IE_OFFSET_)); + if (p && ie_len > 0) + return; + + rtw_vht_use_default_setting(padapter); + + /* VHT Operation mode notifiy bit in Extended IE (127) */ + SET_EXT_CAPABILITY_ELE_OP_MODE_NOTIF(pmlmepriv->ext_capab_ie_data, 1); + pmlmepriv->ext_capab_ie_len = 10; + rtw_set_ie(pnetwork->IEs + pnetwork->IELength, EID_EXTCapability, 8, pmlmepriv->ext_capab_ie_data, &len); + pnetwork->IELength += pmlmepriv->ext_capab_ie_len; + + /* VHT Capabilities element */ + cap_len = rtw_build_vht_cap_ie(padapter, pnetwork->IEs + pnetwork->IELength); + pnetwork->IELength += cap_len; + + /* VHT Operation element */ + operation_len = rtw_build_vht_operation_ie(padapter, pnetwork->IEs + pnetwork->IELength, + pnetwork->Configuration.DSConfig); + pnetwork->IELength += operation_len; + + rtw_check_for_vht20(padapter, pnetwork->IEs + _BEACON_IE_OFFSET_, pnetwork->IELength - _BEACON_IE_OFFSET_); + + pmlmepriv->vhtpriv.vht_option = _TRUE; +} + +void rtw_vht_ies_detach(_adapter *padapter, WLAN_BSSID_EX *pnetwork) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + rtw_remove_bcn_ie(padapter, pnetwork, EID_EXTCapability); + rtw_remove_bcn_ie(padapter, pnetwork, EID_VHTCapability); + rtw_remove_bcn_ie(padapter, pnetwork, EID_VHTOperation); + + pmlmepriv->vhtpriv.vht_option = _FALSE; +} + +void rtw_check_for_vht20(_adapter *adapter, u8 *ies, int ies_len) +{ + u8 ht_ch, ht_bw, ht_offset; + u8 vht_ch, vht_bw, vht_offset; + + rtw_ies_get_chbw(ies, ies_len, &ht_ch, &ht_bw, &ht_offset, 1, 0); + rtw_ies_get_chbw(ies, ies_len, &vht_ch, &vht_bw, &vht_offset, 1, 1); + + if (ht_bw == CHANNEL_WIDTH_20 && vht_bw >= CHANNEL_WIDTH_80) { + u8 *vht_op_ie; + int vht_op_ielen; + + RTW_INFO(FUNC_ADPT_FMT" vht80 is not allowed without ht40\n", FUNC_ADPT_ARG(adapter)); + vht_op_ie = rtw_get_ie(ies, EID_VHTOperation, &vht_op_ielen, ies_len); + if (vht_op_ie && vht_op_ielen) { + RTW_INFO(FUNC_ADPT_FMT" switch to vht20\n", FUNC_ADPT_ARG(adapter)); + SET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2, 0); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(vht_op_ie + 2, 0); + SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(vht_op_ie + 2, 0); + } + } +} +#endif /* CONFIG_AP_MODE */ +#endif /* CONFIG_80211AC_VHT */ diff --git a/core/rtw_wapi.c b/core/rtw_wapi.c new file mode 100644 index 0000000..94b26e7 --- /dev/null +++ b/core/rtw_wapi.c @@ -0,0 +1,1320 @@ +/****************************************************************************** + * + * Copyright(c) 2016 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifdef CONFIG_WAPI_SUPPORT + +#include +#include +#include +#include + + +u32 wapi_debug_component = + /* WAPI_INIT | + * WAPI_API | + * WAPI_TX | + * WAPI_RX | */ + WAPI_ERR ; /* always open err flags on */ + +void WapiFreeAllStaInfo(_adapter *padapter) +{ + PRT_WAPI_T pWapiInfo; + PRT_WAPI_STA_INFO pWapiStaInfo; + PRT_WAPI_BKID pWapiBkid; + + WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__); + pWapiInfo = &padapter->wapiInfo; + + /* Pust to Idle List */ + rtw_wapi_return_all_sta_info(padapter); + + /* Sta Info List */ + while (!list_empty(&(pWapiInfo->wapiSTAIdleList))) { + pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAIdleList.next, RT_WAPI_STA_INFO, list); + list_del_init(&pWapiStaInfo->list); + } + + /* BKID List */ + while (!list_empty(&(pWapiInfo->wapiBKIDIdleList))) { + pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDIdleList.next, RT_WAPI_BKID, list); + list_del_init(&pWapiBkid->list); + } + WAPI_TRACE(WAPI_INIT, "<=========== %s\n", __FUNCTION__); + return; +} + +void WapiSetIE(_adapter *padapter) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + /* PRT_WAPI_BKID pWapiBkid; */ + u16 protocolVer = 1; + u16 akmCnt = 1; + u16 suiteCnt = 1; + u16 capability = 0; + u8 OUI[3]; + + OUI[0] = 0x00; + OUI[1] = 0x14; + OUI[2] = 0x72; + + pWapiInfo->wapiIELength = 0; + /* protocol version */ + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, &protocolVer, 2); + pWapiInfo->wapiIELength += 2; + /* akm */ + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, &akmCnt, 2); + pWapiInfo->wapiIELength += 2; + + if (pWapiInfo->bWapiPSK) { + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, OUI, 3); + pWapiInfo->wapiIELength += 3; + pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x2; + pWapiInfo->wapiIELength += 1; + } else { + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, OUI, 3); + pWapiInfo->wapiIELength += 3; + pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x1; + pWapiInfo->wapiIELength += 1; + } + + /* usk */ + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, &suiteCnt, 2); + pWapiInfo->wapiIELength += 2; + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, OUI, 3); + pWapiInfo->wapiIELength += 3; + pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x1; + pWapiInfo->wapiIELength += 1; + + /* msk */ + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, OUI, 3); + pWapiInfo->wapiIELength += 3; + pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x1; + pWapiInfo->wapiIELength += 1; + + /* Capbility */ + memcpy(pWapiInfo->wapiIE + pWapiInfo->wapiIELength, &capability, 2); + pWapiInfo->wapiIELength += 2; +} + + +/* PN1 > PN2, return 1, + * else return 0. + */ +u32 WapiComparePN(u8 *PN1, u8 *PN2) +{ + char i; + + if ((NULL == PN1) || (NULL == PN2)) + return 1; + + /* overflow case */ + if ((PN2[15] - PN1[15]) & 0x80) + return 1; + + for (i = 16; i > 0; i--) { + if (PN1[i - 1] == PN2[i - 1]) + continue; + else if (PN1[i - 1] > PN2[i - 1]) + return 1; + else + return 0; + } + + return 0; +} + +u8 +WapiGetEntryForCamWrite(_adapter *padapter, u8 *pMacAddr, u8 KID, BOOLEAN IsMsk) +{ + PRT_WAPI_T pWapiInfo = NULL; + /* PRT_WAPI_CAM_ENTRY pEntry=NULL; */ + u8 i = 0; + u8 ret = 0xff; + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + pWapiInfo = &padapter->wapiInfo; + + /* exist? */ + for (i = 0; i < WAPI_CAM_ENTRY_NUM; i++) { + if (pWapiInfo->wapiCamEntry[i].IsUsed + && (_rtw_memcmp(pMacAddr, pWapiInfo->wapiCamEntry[i].PeerMacAddr, ETH_ALEN) == _TRUE) + && pWapiInfo->wapiCamEntry[i].keyidx == KID + && pWapiInfo->wapiCamEntry[i].type == IsMsk) { + ret = pWapiInfo->wapiCamEntry[i].entry_idx; /* cover it */ + break; + } + } + + if (i == WAPI_CAM_ENTRY_NUM) { /* not found */ + for (i = 0; i < WAPI_CAM_ENTRY_NUM; i++) { + if (pWapiInfo->wapiCamEntry[i].IsUsed == 0) { + pWapiInfo->wapiCamEntry[i].IsUsed = 1; + pWapiInfo->wapiCamEntry[i].type = IsMsk; + pWapiInfo->wapiCamEntry[i].keyidx = KID; + _rtw_memcpy(pWapiInfo->wapiCamEntry[i].PeerMacAddr, pMacAddr, ETH_ALEN); + ret = pWapiInfo->wapiCamEntry[i].entry_idx; + break; + } + } + } + + WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__); + return ret; + + /* + if(RTIsListEmpty(&pWapiInfo->wapiCamIdleList)) { + return 0; + } + + pEntry = (PRT_WAPI_CAM_ENTRY)RTRemoveHeadList(&pWapiInfo->wapiCamIdleList); + RTInsertTailList(&pWapiInfo->wapiCamUsedList, &pEntry->list); + + + return pEntry->entry_idx;*/ +} + +u8 WapiGetEntryForCamClear(_adapter *padapter, u8 *pPeerMac, u8 keyid, u8 IsMsk) +{ + PRT_WAPI_T pWapiInfo = NULL; + u8 i = 0; + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + pWapiInfo = &padapter->wapiInfo; + + for (i = 0; i < WAPI_CAM_ENTRY_NUM; i++) { + if (pWapiInfo->wapiCamEntry[i].IsUsed + && (_rtw_memcmp(pPeerMac, pWapiInfo->wapiCamEntry[i].PeerMacAddr, ETH_ALEN) == _TRUE) + && pWapiInfo->wapiCamEntry[i].keyidx == keyid + && pWapiInfo->wapiCamEntry[i].type == IsMsk) { + pWapiInfo->wapiCamEntry[i].IsUsed = 0; + pWapiInfo->wapiCamEntry[i].keyidx = 2; + _rtw_memset(pWapiInfo->wapiCamEntry[i].PeerMacAddr, 0, ETH_ALEN); + + WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__); + return pWapiInfo->wapiCamEntry[i].entry_idx; + } + } + + WAPI_TRACE(WAPI_API, "<====WapiGetReturnCamEntry(), No this cam entry.\n"); + return 0xff; + /* + if(RTIsListEmpty(&pWapiInfo->wapiCamUsedList)) { + return FALSE; + } + + pList = &pWapiInfo->wapiCamUsedList; + while(pList->Flink != &pWapiInfo->wapiCamUsedList) + { + pEntry = (PRT_WAPI_CAM_ENTRY)pList->Flink; + if(PlatformCompareMemory(pPeerMac,pEntry->PeerMacAddr, ETHER_ADDRLEN)== 0 + && keyid == pEntry->keyidx) + { + RTRemoveEntryList(pList); + RTInsertHeadList(&pWapiInfo->wapiCamIdleList, pList); + return pEntry->entry_idx; + } + pList = pList->Flink; + } + + return 0; + */ +} + +void +WapiResetAllCamEntry(_adapter *padapter) +{ + PRT_WAPI_T pWapiInfo; + int i; + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + pWapiInfo = &padapter->wapiInfo; + + for (i = 0; i < WAPI_CAM_ENTRY_NUM; i++) { + _rtw_memset(pWapiInfo->wapiCamEntry[i].PeerMacAddr, 0, ETH_ALEN); + pWapiInfo->wapiCamEntry[i].IsUsed = 0; + pWapiInfo->wapiCamEntry[i].keyidx = 2; /* invalid */ + pWapiInfo->wapiCamEntry[i].entry_idx = 4 + i * 2; + } + + WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__); + + return; +} + +u8 WapiWriteOneCamEntry( + _adapter *padapter, + u8 *pMacAddr, + u8 KeyId, + u8 EntryId, + u8 EncAlg, + u8 bGroupKey, + u8 *pKey +) +{ + u8 retVal = 0; + u16 usConfig = 0; + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + if (EntryId >= 32) { + WAPI_TRACE(WAPI_ERR, "<=== CamAddOneEntry(): ulKeyId exceed!\n"); + return retVal; + } + + usConfig = usConfig | (0x01 << 15) | ((u16)(EncAlg) << 2) | (KeyId); + + if (EncAlg == _SMS4_) { + if (bGroupKey == 1) + usConfig |= (0x01 << 6); + if ((EntryId % 2) == 1) /* ==0 sec key; == 1mic key */ + usConfig |= (0x01 << 5); + } + + write_cam(padapter, EntryId, usConfig, pMacAddr, pKey); + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + return 1; +} + +void rtw_wapi_init(_adapter *padapter) +{ + PRT_WAPI_T pWapiInfo; + int i; + + WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__); + RT_ASSERT_RET(padapter); + + if (!padapter->WapiSupport) { + WAPI_TRACE(WAPI_INIT, "<========== %s, WAPI not supported!\n", __FUNCTION__); + return; + } + + pWapiInfo = &padapter->wapiInfo; + pWapiInfo->bWapiEnable = false; + + /* Init BKID List */ + INIT_LIST_HEAD(&pWapiInfo->wapiBKIDIdleList); + INIT_LIST_HEAD(&pWapiInfo->wapiBKIDStoreList); + for (i = 0; i < WAPI_MAX_BKID_NUM; i++) + list_add_tail(&pWapiInfo->wapiBKID[i].list, &pWapiInfo->wapiBKIDIdleList); + + /* Init STA List */ + INIT_LIST_HEAD(&pWapiInfo->wapiSTAIdleList); + INIT_LIST_HEAD(&pWapiInfo->wapiSTAUsedList); + for (i = 0; i < WAPI_MAX_STAINFO_NUM; i++) + list_add_tail(&pWapiInfo->wapiSta[i].list, &pWapiInfo->wapiSTAIdleList); + + for (i = 0; i < WAPI_CAM_ENTRY_NUM; i++) { + pWapiInfo->wapiCamEntry[i].IsUsed = 0; + pWapiInfo->wapiCamEntry[i].keyidx = 2; /* invalid */ + pWapiInfo->wapiCamEntry[i].entry_idx = 4 + i * 2; + } + + WAPI_TRACE(WAPI_INIT, "<========== %s\n", __FUNCTION__); +} + +void rtw_wapi_free(_adapter *padapter) +{ + WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__); + RT_ASSERT_RET(padapter); + + if (!padapter->WapiSupport) { + WAPI_TRACE(WAPI_INIT, "<========== %s, WAPI not supported!\n", __FUNCTION__); + return; + } + + WapiFreeAllStaInfo(padapter); + + WAPI_TRACE(WAPI_INIT, "<========== %s\n", __FUNCTION__); +} + +void rtw_wapi_disable_tx(_adapter *padapter) +{ + WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__); + RT_ASSERT_RET(padapter); + + if (!padapter->WapiSupport) { + WAPI_TRACE(WAPI_INIT, "<========== %s, WAPI not supported!\n", __FUNCTION__); + return; + } + + padapter->wapiInfo.wapiTxMsk.bTxEnable = false; + padapter->wapiInfo.wapiTxMsk.bSet = false; + + WAPI_TRACE(WAPI_INIT, "<========== %s\n", __FUNCTION__); +} + +u8 rtw_wapi_is_wai_packet(_adapter *padapter, u8 *pkt_data) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + PRT_WAPI_STA_INFO pWapiSta = NULL; + u8 WaiPkt = 0, *pTaddr, bFind = false; + u8 Offset_TypeWAI = 0 ; /* (mac header len + llc length) */ + + WAPI_TRACE(WAPI_TX | WAPI_RX, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return 0; + } + + Offset_TypeWAI = 24 + 6 ; + + /* YJ,add,091103. Data frame may also have skb->data[30]=0x88 and skb->data[31]=0xb4. */ + if ((pkt_data[1] & 0x40) != 0) { + /* RTW_INFO("data is privacy\n"); */ + return 0; + } + + pTaddr = get_addr2_ptr(pkt_data); + if (list_empty(&pWapiInfo->wapiSTAUsedList)) + bFind = false; + else { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (_rtw_memcmp(pTaddr, pWapiSta->PeerMacAddr, 6) == _TRUE) { + bFind = true; + break; + } + } + } + + WAPI_TRACE(WAPI_TX | WAPI_RX, "%s: bFind=%d pTaddr="MAC_FMT"\n", __FUNCTION__, bFind, MAC_ARG(pTaddr)); + + if (pkt_data[0] == WIFI_QOS_DATA_TYPE) + Offset_TypeWAI += 2; + + /* 88b4? */ + if ((pkt_data[Offset_TypeWAI] == 0x88) && (pkt_data[Offset_TypeWAI + 1] == 0xb4)) { + WaiPkt = pkt_data[Offset_TypeWAI + 5]; + + psecuritypriv->hw_decrypted = _TRUE; + } else + WAPI_TRACE(WAPI_TX | WAPI_RX, "%s(): non wai packet\n", __FUNCTION__); + + WAPI_TRACE(WAPI_TX | WAPI_RX, "%s(): Recvd WAI frame. IsWAIPkt(%d)\n", __FUNCTION__, WaiPkt); + + return WaiPkt; +} + + +void rtw_wapi_update_info(_adapter *padapter, union recv_frame *precv_frame) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + struct recv_frame_hdr *precv_hdr; + u8 *ptr; + u8 *pTA; + u8 *pRecvPN; + + + WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + precv_hdr = &precv_frame->u.hdr; + ptr = precv_hdr->rx_data; + + if (precv_hdr->attrib.qos == 1) + precv_hdr->UserPriority = GetTid(ptr); + else + precv_hdr->UserPriority = 0; + + pTA = get_addr2_ptr(ptr); + _rtw_memcpy((u8 *)precv_hdr->WapiSrcAddr, pTA, 6); + pRecvPN = ptr + precv_hdr->attrib.hdrlen + 2; + _rtw_memcpy((u8 *)precv_hdr->WapiTempPN, pRecvPN, 16); + + WAPI_TRACE(WAPI_RX, "<========== %s\n", __FUNCTION__); +} + +/**************************************************************************** +TRUE-----------------Drop +FALSE---------------- handle +add to support WAPI to N-mode +*****************************************************************************/ +u8 rtw_wapi_check_for_drop( + _adapter *padapter, + union recv_frame *precv_frame, + u8 *ehdr_ops +) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + u8 *pLastRecvPN = NULL; + u8 bFind = false; + PRT_WAPI_STA_INFO pWapiSta = NULL; + u8 bDrop = false; + struct recv_frame_hdr *precv_hdr = &precv_frame->u.hdr; + u8 WapiAEPNInitialValueSrc[16] = {0x37, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + u8 WapiAEMultiCastPNInitialValueSrc[16] = {0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + u8 *ptr = ehdr_ops; + int i; + + WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return false; + } + + if (precv_hdr->bIsWaiPacket != 0) { + if (precv_hdr->bIsWaiPacket == 0x8) { + + RTW_INFO("rtw_wapi_check_for_drop: dump packet\n"); + for (i = 0; i < 50; i++) { + RTW_INFO("%02X ", ptr[i]); + if ((i + 1) % 8 == 0) + RTW_INFO("\n"); + } + RTW_INFO("\n rtw_wapi_check_for_drop: dump packet\n"); + + for (i = 0; i < 16; i++) { + if (ptr[i + 27] != 0) + break; + } + + if (i == 16) { + WAPI_TRACE(WAPI_RX, "rtw_wapi_check_for_drop: drop with zero BKID\n"); + return true; + } else + return false; + } else + return false; + } + + if (list_empty(&pWapiInfo->wapiSTAUsedList)) + bFind = false; + else { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (_rtw_memcmp(precv_hdr->WapiSrcAddr, pWapiSta->PeerMacAddr, ETH_ALEN) == _TRUE) { + bFind = true; + break; + } + } + } + WAPI_TRACE(WAPI_RX, "%s: bFind=%d prxb->WapiSrcAddr="MAC_FMT"\n", __FUNCTION__, bFind, MAC_ARG(precv_hdr->WapiSrcAddr)); + + if (bFind) { + if (IS_MCAST(precv_hdr->attrib.ra)) { + WAPI_TRACE(WAPI_RX, "rtw_wapi_check_for_drop: multicast case\n"); + pLastRecvPN = pWapiSta->lastRxMulticastPN; + } else { + WAPI_TRACE(WAPI_RX, "rtw_wapi_check_for_drop: unicast case\n"); + switch (precv_hdr->UserPriority) { + case 0: + case 3: + pLastRecvPN = pWapiSta->lastRxUnicastPNBEQueue; + break; + case 1: + case 2: + pLastRecvPN = pWapiSta->lastRxUnicastPNBKQueue; + break; + case 4: + case 5: + pLastRecvPN = pWapiSta->lastRxUnicastPNVIQueue; + break; + case 6: + case 7: + pLastRecvPN = pWapiSta->lastRxUnicastPNVOQueue; + break; + default: + WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__); + break; + } + } + + if (!WapiComparePN(precv_hdr->WapiTempPN, pLastRecvPN)) { + WAPI_TRACE(WAPI_RX, "%s: Equal PN!!\n", __FUNCTION__); + if (IS_MCAST(precv_hdr->attrib.ra)) + _rtw_memcpy(pLastRecvPN, WapiAEMultiCastPNInitialValueSrc, 16); + else + _rtw_memcpy(pLastRecvPN, WapiAEPNInitialValueSrc, 16); + bDrop = true; + } else + _rtw_memcpy(pLastRecvPN, precv_hdr->WapiTempPN, 16); + } + + WAPI_TRACE(WAPI_RX, "<========== %s\n", __FUNCTION__); + return bDrop; +} + +void rtw_build_probe_resp_wapi_ie(_adapter *padapter, unsigned char *pframe, struct pkt_attrib *pattrib) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + u8 WapiIELength = 0; + + WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported!\n", __FUNCTION__); + return; + } + + WapiSetIE(padapter); + WapiIELength = pWapiInfo->wapiIELength; + pframe[0] = _WAPI_IE_; + pframe[1] = WapiIELength; + _rtw_memcpy(pframe + 2, pWapiInfo->wapiIE, WapiIELength); + pframe += WapiIELength + 2; + pattrib->pktlen += WapiIELength + 2; + + WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__); +} + +void rtw_build_beacon_wapi_ie(_adapter *padapter, unsigned char *pframe, struct pkt_attrib *pattrib) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + u8 WapiIELength = 0; + WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported!\n", __FUNCTION__); + return; + } + + WapiSetIE(padapter); + WapiIELength = pWapiInfo->wapiIELength; + pframe[0] = _WAPI_IE_; + pframe[1] = WapiIELength; + _rtw_memcpy(pframe + 2, pWapiInfo->wapiIE, WapiIELength); + pframe += WapiIELength + 2; + pattrib->pktlen += WapiIELength + 2; + + WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__); +} + +void rtw_build_assoc_req_wapi_ie(_adapter *padapter, unsigned char *pframe, struct pkt_attrib *pattrib) +{ + PRT_WAPI_BKID pWapiBKID; + u16 bkidNum; + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + u8 WapiIELength = 0; + + WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported!\n", __FUNCTION__); + return; + } + + WapiSetIE(padapter); + WapiIELength = pWapiInfo->wapiIELength; + bkidNum = 0; + if (!list_empty(&(pWapiInfo->wapiBKIDStoreList))) { + list_for_each_entry(pWapiBKID, &pWapiInfo->wapiBKIDStoreList, list) { + bkidNum++; + _rtw_memcpy(pWapiInfo->wapiIE + WapiIELength + 2, pWapiBKID->bkid, 16); + WapiIELength += 16; + } + } + _rtw_memcpy(pWapiInfo->wapiIE + WapiIELength, &bkidNum, 2); + WapiIELength += 2; + + pframe[0] = _WAPI_IE_; + pframe[1] = WapiIELength; + _rtw_memcpy(pframe + 2, pWapiInfo->wapiIE, WapiIELength); + pframe += WapiIELength + 2; + pattrib->pktlen += WapiIELength + 2; + WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__); +} + +void rtw_wapi_on_assoc_ok(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo); + PRT_WAPI_STA_INFO pWapiSta; + u8 WapiAEPNInitialValueSrc[16] = {0x37, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + /* u8 WapiASUEPNInitialValueSrc[16] = {0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ; */ + u8 WapiAEMultiCastPNInitialValueSrc[16] = {0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + + WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + pWapiSta = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAIdleList.next, RT_WAPI_STA_INFO, list); + list_del_init(&pWapiSta->list); + list_add_tail(&pWapiSta->list, &pWapiInfo->wapiSTAUsedList); + _rtw_memcpy(pWapiSta->PeerMacAddr, padapter->mlmeextpriv.mlmext_info.network.MacAddress, 6); + _rtw_memcpy(pWapiSta->lastRxMulticastPN, WapiAEMultiCastPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPN, WapiAEPNInitialValueSrc, 16); + + /* For chenk PN error with Qos Data after s3: add by ylb 20111114 */ + _rtw_memcpy(pWapiSta->lastRxUnicastPNBEQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPNBKQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPNVIQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPNVOQueue, WapiAEPNInitialValueSrc, 16); + + WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__); +} + + +void rtw_wapi_return_one_sta_info(_adapter *padapter, u8 *MacAddr) +{ + PRT_WAPI_T pWapiInfo; + PRT_WAPI_STA_INFO pWapiStaInfo = NULL; + PRT_WAPI_BKID pWapiBkid = NULL; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + + pWapiInfo = &padapter->wapiInfo; + + WAPI_TRACE(WAPI_API, "==========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + while (!list_empty(&(pWapiInfo->wapiBKIDStoreList))) { + pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDStoreList.next, RT_WAPI_BKID, list); + list_del_init(&pWapiBkid->list); + _rtw_memset(pWapiBkid->bkid, 0, 16); + list_add_tail(&pWapiBkid->list, &pWapiInfo->wapiBKIDIdleList); + } + } + + + WAPI_TRACE(WAPI_API, " %s: after clear bkid\n", __FUNCTION__); + + + /* Remove STA info */ + if (list_empty(&(pWapiInfo->wapiSTAUsedList))) { + WAPI_TRACE(WAPI_API, " %s: wapiSTAUsedList is null\n", __FUNCTION__); + return; + } else { + + WAPI_TRACE(WAPI_API, " %s: wapiSTAUsedList is not null\n", __FUNCTION__); +#if 0 + pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry((pWapiInfo->wapiSTAUsedList.next), RT_WAPI_STA_INFO, list); + + list_for_each_entry(pWapiStaInfo, &(pWapiInfo->wapiSTAUsedList), list) { + + RTW_INFO("MAC Addr %02x-%02x-%02x-%02x-%02x-%02x\n", MacAddr[0], MacAddr[1], MacAddr[2], MacAddr[3], MacAddr[4], MacAddr[5]); + + + RTW_INFO("peer Addr %02x-%02x-%02x-%02x-%02x-%02x\n", pWapiStaInfo->PeerMacAddr[0], pWapiStaInfo->PeerMacAddr[1], pWapiStaInfo->PeerMacAddr[2], pWapiStaInfo->PeerMacAddr[3], + pWapiStaInfo->PeerMacAddr[4], pWapiStaInfo->PeerMacAddr[5]); + + if (pWapiStaInfo == NULL) { + WAPI_TRACE(WAPI_API, " %s: pWapiStaInfo == NULL Case\n", __FUNCTION__); + return; + } + + if (pWapiStaInfo->PeerMacAddr == NULL) { + WAPI_TRACE(WAPI_API, " %s: pWapiStaInfo->PeerMacAddr == NULL Case\n", __FUNCTION__); + return; + } + + if (MacAddr == NULL) { + WAPI_TRACE(WAPI_API, " %s: MacAddr == NULL Case\n", __FUNCTION__); + return; + } + + if (_rtw_memcmp(pWapiStaInfo->PeerMacAddr, MacAddr, ETH_ALEN) == _TRUE) { + pWapiStaInfo->bAuthenticateInProgress = false; + pWapiStaInfo->bSetkeyOk = false; + _rtw_memset(pWapiStaInfo->PeerMacAddr, 0, ETH_ALEN); + list_del_init(&pWapiStaInfo->list); + list_add_tail(&pWapiStaInfo->list, &pWapiInfo->wapiSTAIdleList); + break; + } + + } +#endif + + while (!list_empty(&(pWapiInfo->wapiSTAUsedList))) { + pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAUsedList.next, RT_WAPI_STA_INFO, list); + + RTW_INFO("peer Addr %02x-%02x-%02x-%02x-%02x-%02x\n", pWapiStaInfo->PeerMacAddr[0], pWapiStaInfo->PeerMacAddr[1], pWapiStaInfo->PeerMacAddr[2], pWapiStaInfo->PeerMacAddr[3], + pWapiStaInfo->PeerMacAddr[4], pWapiStaInfo->PeerMacAddr[5]); + + list_del_init(&pWapiStaInfo->list); + memset(pWapiStaInfo->PeerMacAddr, 0, ETH_ALEN); + pWapiStaInfo->bSetkeyOk = 0; + list_add_tail(&pWapiStaInfo->list, &pWapiInfo->wapiSTAIdleList); + } + + } + + WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__); + return; +} + +void rtw_wapi_return_all_sta_info(_adapter *padapter) +{ + PRT_WAPI_T pWapiInfo; + PRT_WAPI_STA_INFO pWapiStaInfo; + PRT_WAPI_BKID pWapiBkid; + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + pWapiInfo = &padapter->wapiInfo; + + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + /* Sta Info List */ + while (!list_empty(&(pWapiInfo->wapiSTAUsedList))) { + pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAUsedList.next, RT_WAPI_STA_INFO, list); + list_del_init(&pWapiStaInfo->list); + memset(pWapiStaInfo->PeerMacAddr, 0, ETH_ALEN); + pWapiStaInfo->bSetkeyOk = 0; + list_add_tail(&pWapiStaInfo->list, &pWapiInfo->wapiSTAIdleList); + } + + /* BKID List */ + while (!list_empty(&(pWapiInfo->wapiBKIDStoreList))) { + pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDStoreList.next, RT_WAPI_BKID, list); + list_del_init(&pWapiBkid->list); + memset(pWapiBkid->bkid, 0, 16); + list_add_tail(&pWapiBkid->list, &pWapiInfo->wapiBKIDIdleList); + } + WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__); +} + +void CAM_empty_entry( + PADAPTER Adapter, + u8 ucIndex +) +{ + rtw_hal_set_hwreg(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex)); +} + +void rtw_wapi_clear_cam_entry(_adapter *padapter, u8 *pMacAddr) +{ + u8 UcIndex = 0; + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 0, 0); + if (UcIndex != 0xff) { + /* CAM_mark_invalid(Adapter, UcIndex); */ + CAM_empty_entry(padapter, UcIndex); + } + + UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 1, 0); + if (UcIndex != 0xff) { + /* CAM_mark_invalid(Adapter, UcIndex); */ + CAM_empty_entry(padapter, UcIndex); + } + + UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 0, 1); + if (UcIndex != 0xff) { + /* CAM_mark_invalid(Adapter, UcIndex); */ + CAM_empty_entry(padapter, UcIndex); + } + + UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 1, 1); + if (UcIndex != 0xff) { + /* CAM_mark_invalid(padapter, UcIndex); */ + CAM_empty_entry(padapter, UcIndex); + } + + WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__); +} + +void rtw_wapi_clear_all_cam_entry(_adapter *padapter) +{ + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) { + WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + invalidate_cam_all(padapter); /* is this ok? */ + WapiResetAllCamEntry(padapter); + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); +} + +void rtw_wapi_set_key(_adapter *padapter, RT_WAPI_KEY *pWapiKey, RT_WAPI_STA_INFO *pWapiSta, u8 bGroupKey, u8 bUseDefaultKey) +{ + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + u8 *pMacAddr = pWapiSta->PeerMacAddr; + u32 EntryId = 0; + BOOLEAN IsPairWise = false ; + u8 EncAlgo; + + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) { + WAPI_TRACE(WAPI_API, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__); + return; + } + + EncAlgo = _SMS4_; + + /* For Tx bc/mc pkt,use defualt key entry */ + if (bUseDefaultKey) { + /* when WAPI update key, keyid will be 0 or 1 by turns. */ + if (pWapiKey->keyId == 0) + EntryId = 0; + else + EntryId = 2; + } else { + /* tx/rx unicast pkt, or rx broadcast, find the key entry by peer's MacAddr */ + EntryId = WapiGetEntryForCamWrite(padapter, pMacAddr, pWapiKey->keyId, bGroupKey); + } + + if (EntryId == 0xff) { + WAPI_TRACE(WAPI_API, "===>No entry for WAPI setkey! !!\n"); + return; + } + + /* EntryId is also used to diff Sec key and Mic key */ + /* Sec Key */ + WapiWriteOneCamEntry(padapter, + pMacAddr, + pWapiKey->keyId, /* keyid */ + EntryId, /* entry */ + EncAlgo, /* type */ + bGroupKey, /* pairwise or group key */ + pWapiKey->dataKey); + /* MIC key */ + WapiWriteOneCamEntry(padapter, + pMacAddr, + pWapiKey->keyId, /* keyid */ + EntryId + 1, /* entry */ + EncAlgo, /* type */ + bGroupKey, /* pairwise or group key */ + pWapiKey->micKey); + + WAPI_TRACE(WAPI_API, "Set Wapi Key :KeyId:%d,EntryId:%d,PairwiseKey:%d.\n", pWapiKey->keyId, EntryId, !bGroupKey); + WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__); + +} + +#if 0 +/* YJ,test,091013 */ +void wapi_test_set_key(struct _adapter *padapter, u8 *buf) +{ + /*Data: keyType(1) + bTxEnable(1) + bAuthenticator(1) + bUpdate(1) + PeerAddr(6) + DataKey(16) + MicKey(16) + KeyId(1)*/ + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + PRT_WAPI_BKID pWapiBkid; + PRT_WAPI_STA_INFO pWapiSta; + u8 data[43]; + bool bTxEnable; + bool bUpdate; + bool bAuthenticator; + u8 PeerAddr[6]; + u8 WapiAEPNInitialValueSrc[16] = {0x37, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + u8 WapiASUEPNInitialValueSrc[16] = {0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + u8 WapiAEMultiCastPNInitialValueSrc[16] = {0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + + WAPI_TRACE(WAPI_INIT, "===========>%s\n", __FUNCTION__); + + if (!padapter->WapiSupport) + return; + + copy_from_user(data, buf, 43); + bTxEnable = data[1]; + bAuthenticator = data[2]; + bUpdate = data[3]; + memcpy(PeerAddr, data + 4, 6); + + if (data[0] == 0x3) { + if (!list_empty(&(pWapiInfo->wapiBKIDIdleList))) { + pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDIdleList.next, RT_WAPI_BKID, list); + list_del_init(&pWapiBkid->list); + memcpy(pWapiBkid->bkid, data + 10, 16); + WAPI_DATA(WAPI_INIT, "SetKey - BKID", pWapiBkid->bkid, 16); + list_add_tail(&pWapiBkid->list, &pWapiInfo->wapiBKIDStoreList); + } + } else { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (!memcmp(pWapiSta->PeerMacAddr, PeerAddr, 6)) { + pWapiSta->bAuthenticatorInUpdata = false; + switch (data[0]) { + case 1: /* usk */ + if (bAuthenticator) { /* authenticator */ + memcpy(pWapiSta->lastTxUnicastPN, WapiAEPNInitialValueSrc, 16); + if (!bUpdate) { /* first */ + WAPI_TRACE(WAPI_INIT, "AE fisrt set usk\n"); + pWapiSta->wapiUsk.bSet = true; + memcpy(pWapiSta->wapiUsk.dataKey, data + 10, 16); + memcpy(pWapiSta->wapiUsk.micKey, data + 26, 16); + pWapiSta->wapiUsk.keyId = *(data + 42); + pWapiSta->wapiUsk.bTxEnable = true; + WAPI_DATA(WAPI_INIT, "SetKey - AE USK Data Key", pWapiSta->wapiUsk.dataKey, 16); + WAPI_DATA(WAPI_INIT, "SetKey - AE USK Mic Key", pWapiSta->wapiUsk.micKey, 16); + } else { /* update */ + WAPI_TRACE(WAPI_INIT, "AE update usk\n"); + pWapiSta->wapiUskUpdate.bSet = true; + pWapiSta->bAuthenticatorInUpdata = true; + memcpy(pWapiSta->wapiUskUpdate.dataKey, data + 10, 16); + memcpy(pWapiSta->wapiUskUpdate.micKey, data + 26, 16); + memcpy(pWapiSta->lastRxUnicastPNBEQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPNBKQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPNVIQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPNVOQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPN, WapiASUEPNInitialValueSrc, 16); + pWapiSta->wapiUskUpdate.keyId = *(data + 42); + pWapiSta->wapiUskUpdate.bTxEnable = true; + } + } else { + if (!bUpdate) { + WAPI_TRACE(WAPI_INIT, "ASUE fisrt set usk\n"); + if (bTxEnable) { + pWapiSta->wapiUsk.bTxEnable = true; + memcpy(pWapiSta->lastTxUnicastPN, WapiASUEPNInitialValueSrc, 16); + } else { + pWapiSta->wapiUsk.bSet = true; + memcpy(pWapiSta->wapiUsk.dataKey, data + 10, 16); + memcpy(pWapiSta->wapiUsk.micKey, data + 26, 16); + pWapiSta->wapiUsk.keyId = *(data + 42); + pWapiSta->wapiUsk.bTxEnable = false; + } + } else { + WAPI_TRACE(WAPI_INIT, "ASUE update usk\n"); + if (bTxEnable) { + pWapiSta->wapiUskUpdate.bTxEnable = true; + if (pWapiSta->wapiUskUpdate.bSet) { + memcpy(pWapiSta->wapiUsk.dataKey, pWapiSta->wapiUskUpdate.dataKey, 16); + memcpy(pWapiSta->wapiUsk.micKey, pWapiSta->wapiUskUpdate.micKey, 16); + pWapiSta->wapiUsk.keyId = pWapiSta->wapiUskUpdate.keyId; + memcpy(pWapiSta->lastRxUnicastPNBEQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPNBKQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPNVIQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPNVOQueue, WapiASUEPNInitialValueSrc, 16); + memcpy(pWapiSta->lastRxUnicastPN, WapiASUEPNInitialValueSrc, 16); + pWapiSta->wapiUskUpdate.bTxEnable = false; + pWapiSta->wapiUskUpdate.bSet = false; + } + memcpy(pWapiSta->lastTxUnicastPN, WapiASUEPNInitialValueSrc, 16); + } else { + pWapiSta->wapiUskUpdate.bSet = true; + memcpy(pWapiSta->wapiUskUpdate.dataKey, data + 10, 16); + memcpy(pWapiSta->wapiUskUpdate.micKey, data + 26, 16); + pWapiSta->wapiUskUpdate.keyId = *(data + 42); + pWapiSta->wapiUskUpdate.bTxEnable = false; + } + } + } + break; + case 2: /* msk */ + if (bAuthenticator) { /* authenticator */ + pWapiInfo->wapiTxMsk.bSet = true; + memcpy(pWapiInfo->wapiTxMsk.dataKey, data + 10, 16); + memcpy(pWapiInfo->wapiTxMsk.micKey, data + 26, 16); + pWapiInfo->wapiTxMsk.keyId = *(data + 42); + pWapiInfo->wapiTxMsk.bTxEnable = true; + memcpy(pWapiInfo->lastTxMulticastPN, WapiAEMultiCastPNInitialValueSrc, 16); + + if (!bUpdate) { /* first */ + WAPI_TRACE(WAPI_INIT, "AE fisrt set msk\n"); + if (!pWapiSta->bSetkeyOk) + pWapiSta->bSetkeyOk = true; + pWapiInfo->bFirstAuthentiateInProgress = false; + } else /* update */ + WAPI_TRACE(WAPI_INIT, "AE update msk\n"); + + WAPI_DATA(WAPI_INIT, "SetKey - AE MSK Data Key", pWapiInfo->wapiTxMsk.dataKey, 16); + WAPI_DATA(WAPI_INIT, "SetKey - AE MSK Mic Key", pWapiInfo->wapiTxMsk.micKey, 16); + } else { + if (!bUpdate) { + WAPI_TRACE(WAPI_INIT, "ASUE fisrt set msk\n"); + pWapiSta->wapiMsk.bSet = true; + memcpy(pWapiSta->wapiMsk.dataKey, data + 10, 16); + memcpy(pWapiSta->wapiMsk.micKey, data + 26, 16); + pWapiSta->wapiMsk.keyId = *(data + 42); + pWapiSta->wapiMsk.bTxEnable = false; + if (!pWapiSta->bSetkeyOk) + pWapiSta->bSetkeyOk = true; + pWapiInfo->bFirstAuthentiateInProgress = false; + WAPI_DATA(WAPI_INIT, "SetKey - ASUE MSK Data Key", pWapiSta->wapiMsk.dataKey, 16); + WAPI_DATA(WAPI_INIT, "SetKey - ASUE MSK Mic Key", pWapiSta->wapiMsk.micKey, 16); + } else { + WAPI_TRACE(WAPI_INIT, "ASUE update msk\n"); + pWapiSta->wapiMskUpdate.bSet = true; + memcpy(pWapiSta->wapiMskUpdate.dataKey, data + 10, 16); + memcpy(pWapiSta->wapiMskUpdate.micKey, data + 26, 16); + pWapiSta->wapiMskUpdate.keyId = *(data + 42); + pWapiSta->wapiMskUpdate.bTxEnable = false; + } + } + break; + default: + WAPI_TRACE(WAPI_ERR, "Unknown Flag\n"); + break; + } + } + } + } + WAPI_TRACE(WAPI_INIT, "<===========%s\n", __FUNCTION__); +} + + +void wapi_test_init(struct _adapter *padapter) +{ + u8 keybuf[100]; + u8 mac_addr[ETH_ALEN] = {0x00, 0xe0, 0x4c, 0x72, 0x04, 0x70}; + u8 UskDataKey[16] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f}; + u8 UskMicKey[16] = {0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f}; + u8 UskId = 0; + u8 MskDataKey[16] = {0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f}; + u8 MskMicKey[16] = {0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f}; + u8 MskId = 0; + + WAPI_TRACE(WAPI_INIT, "===========>%s\n", __FUNCTION__); + + /* Enable Wapi */ + WAPI_TRACE(WAPI_INIT, "%s: Enable wapi!!!!\n", __FUNCTION__); + padapter->wapiInfo.bWapiEnable = true; + padapter->pairwise_key_type = KEY_TYPE_SMS4; + ieee->group_key_type = KEY_TYPE_SMS4; + padapter->wapiInfo.extra_prefix_len = WAPI_EXT_LEN; + padapter->wapiInfo.extra_postfix_len = SMS4_MIC_LEN; + + /* set usk */ + WAPI_TRACE(WAPI_INIT, "%s: Set USK!!!!\n", __FUNCTION__); + memset(keybuf, 0, 100); + keybuf[0] = 1; /* set usk */ + keybuf[1] = 1; /* enable tx */ + keybuf[2] = 1; /* AE */ + keybuf[3] = 0; /* not update */ + + memcpy(keybuf + 4, mac_addr, ETH_ALEN); + memcpy(keybuf + 10, UskDataKey, 16); + memcpy(keybuf + 26, UskMicKey, 16); + keybuf[42] = UskId; + wapi_test_set_key(padapter, keybuf); + + memset(keybuf, 0, 100); + keybuf[0] = 1; /* set usk */ + keybuf[1] = 1; /* enable tx */ + keybuf[2] = 0; /* AE */ + keybuf[3] = 0; /* not update */ + + memcpy(keybuf + 4, mac_addr, ETH_ALEN); + memcpy(keybuf + 10, UskDataKey, 16); + memcpy(keybuf + 26, UskMicKey, 16); + keybuf[42] = UskId; + wapi_test_set_key(padapter, keybuf); + + /* set msk */ + WAPI_TRACE(WAPI_INIT, "%s: Set MSK!!!!\n", __FUNCTION__); + memset(keybuf, 0, 100); + keybuf[0] = 2; /* set msk */ + keybuf[1] = 1; /* Enable TX */ + keybuf[2] = 1; /* AE */ + keybuf[3] = 0; /* not update */ + memcpy(keybuf + 4, mac_addr, ETH_ALEN); + memcpy(keybuf + 10, MskDataKey, 16); + memcpy(keybuf + 26, MskMicKey, 16); + keybuf[42] = MskId; + wapi_test_set_key(padapter, keybuf); + + memset(keybuf, 0, 100); + keybuf[0] = 2; /* set msk */ + keybuf[1] = 1; /* Enable TX */ + keybuf[2] = 0; /* AE */ + keybuf[3] = 0; /* not update */ + memcpy(keybuf + 4, mac_addr, ETH_ALEN); + memcpy(keybuf + 10, MskDataKey, 16); + memcpy(keybuf + 26, MskMicKey, 16); + keybuf[42] = MskId; + wapi_test_set_key(padapter, keybuf); + WAPI_TRACE(WAPI_INIT, "<===========%s\n", __FUNCTION__); +} +#endif + +void rtw_wapi_get_iv(_adapter *padapter, u8 *pRA, u8 *IV) +{ + PWLAN_HEADER_WAPI_EXTENSION pWapiExt = NULL; + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + bool bPNOverflow = false; + bool bFindMatchPeer = false; + PRT_WAPI_STA_INFO pWapiSta = NULL; + + pWapiExt = (PWLAN_HEADER_WAPI_EXTENSION)IV; + + WAPI_DATA(WAPI_RX, "wapi_get_iv: pra", pRA, 6); + + if (IS_MCAST(pRA)) { + if (!pWapiInfo->wapiTxMsk.bTxEnable) { + WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__); + return; + } + + if (pWapiInfo->wapiTxMsk.keyId <= 1) { + pWapiExt->KeyIdx = pWapiInfo->wapiTxMsk.keyId; + pWapiExt->Reserved = 0; + bPNOverflow = WapiIncreasePN(pWapiInfo->lastTxMulticastPN, 1); + memcpy(pWapiExt->PN, pWapiInfo->lastTxMulticastPN, 16); + } + } else { + if (list_empty(&pWapiInfo->wapiSTAUsedList)) { + WAPI_TRACE(WAPI_RX, "rtw_wapi_get_iv: list is empty\n"); + _rtw_memset(IV, 10, 18); + return; + } else { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + WAPI_DATA(WAPI_RX, "rtw_wapi_get_iv: peermacaddr ", pWapiSta->PeerMacAddr, 6); + if (_rtw_memcmp((u8 *)pWapiSta->PeerMacAddr, pRA, 6) == _TRUE) { + bFindMatchPeer = true; + break; + } + } + + WAPI_TRACE(WAPI_RX, "bFindMatchPeer: %d\n", bFindMatchPeer); + WAPI_DATA(WAPI_RX, "Addr", pRA, 6); + + if (bFindMatchPeer) { + if ((!pWapiSta->wapiUskUpdate.bTxEnable) && (!pWapiSta->wapiUsk.bTxEnable)) + return; + + if (pWapiSta->wapiUsk.keyId <= 1) { + if (pWapiSta->wapiUskUpdate.bTxEnable) + pWapiExt->KeyIdx = pWapiSta->wapiUskUpdate.keyId; + else + pWapiExt->KeyIdx = pWapiSta->wapiUsk.keyId; + + pWapiExt->Reserved = 0; + bPNOverflow = WapiIncreasePN(pWapiSta->lastTxUnicastPN, 2); + _rtw_memcpy(pWapiExt->PN, pWapiSta->lastTxUnicastPN, 16); + + } + } + } + + } + +} + +bool rtw_wapi_drop_for_key_absent(_adapter *padapter, u8 *pRA) +{ + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + bool bFindMatchPeer = false; + bool bDrop = false; + PRT_WAPI_STA_INFO pWapiSta = NULL; + struct security_priv *psecuritypriv = &padapter->securitypriv; + + WAPI_DATA(WAPI_RX, "rtw_wapi_drop_for_key_absent: ra ", pRA, 6); + + if (psecuritypriv->dot11PrivacyAlgrthm == _SMS4_) { + if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable)) + return true; + + if (IS_MCAST(pRA)) { + if (!pWapiInfo->wapiTxMsk.bTxEnable) { + bDrop = true; + WAPI_TRACE(WAPI_RX, "rtw_wapi_drop_for_key_absent: multicast key is absent\n"); + return bDrop; + } + } else { + if (!list_empty(&pWapiInfo->wapiSTAUsedList)) { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + WAPI_DATA(WAPI_RX, "rtw_wapi_drop_for_key_absent: pWapiSta->PeerMacAddr ", pWapiSta->PeerMacAddr, 6); + if (_rtw_memcmp(pRA, pWapiSta->PeerMacAddr, 6) == _TRUE) { + bFindMatchPeer = true; + break; + } + } + if (bFindMatchPeer) { + if (!pWapiSta->wapiUsk.bTxEnable) { + bDrop = true; + WAPI_TRACE(WAPI_RX, "rtw_wapi_drop_for_key_absent: unicast key is absent\n"); + return bDrop; + } + } else { + bDrop = true; + WAPI_TRACE(WAPI_RX, "rtw_wapi_drop_for_key_absent: no peer find\n"); + return bDrop; + } + + } else { + bDrop = true; + WAPI_TRACE(WAPI_RX, "rtw_wapi_drop_for_key_absent: no sta exist\n"); + return bDrop; + } + } + } else + return bDrop; + + return bDrop; +} + +void rtw_wapi_set_set_encryption(_adapter *padapter, struct ieee_param *param) +{ + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + PRT_WAPI_STA_INFO pWapiSta; + u8 WapiASUEPNInitialValueSrc[16] = {0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + u8 WapiAEPNInitialValueSrc[16] = {0x37, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + u8 WapiAEMultiCastPNInitialValueSrc[16] = {0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C} ; + + if (param->u.crypt.set_tx == 1) { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (_rtw_memcmp(pWapiSta->PeerMacAddr, param->sta_addr, 6)) { + _rtw_memcpy(pWapiSta->lastTxUnicastPN, WapiASUEPNInitialValueSrc, 16); + + pWapiSta->wapiUsk.bSet = true; + _rtw_memcpy(pWapiSta->wapiUsk.dataKey, param->u.crypt.key, 16); + _rtw_memcpy(pWapiSta->wapiUsk.micKey, param->u.crypt.key + 16, 16); + pWapiSta->wapiUsk.keyId = param->u.crypt.idx ; + pWapiSta->wapiUsk.bTxEnable = true; + + _rtw_memcpy(pWapiSta->lastRxUnicastPNBEQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPNBKQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPNVIQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPNVOQueue, WapiAEPNInitialValueSrc, 16); + _rtw_memcpy(pWapiSta->lastRxUnicastPN, WapiAEPNInitialValueSrc, 16); + pWapiSta->wapiUskUpdate.bTxEnable = false; + pWapiSta->wapiUskUpdate.bSet = false; + + if (psecuritypriv->sw_encrypt == false || psecuritypriv->sw_decrypt == false) { + /* set unicast key for ASUE */ + rtw_wapi_set_key(padapter, &pWapiSta->wapiUsk, pWapiSta, false, false); + } + } + } + } else { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (_rtw_memcmp(pWapiSta->PeerMacAddr, get_bssid(pmlmepriv), 6)) { + pWapiSta->wapiMsk.bSet = true; + _rtw_memcpy(pWapiSta->wapiMsk.dataKey, param->u.crypt.key, 16); + _rtw_memcpy(pWapiSta->wapiMsk.micKey, param->u.crypt.key + 16, 16); + pWapiSta->wapiMsk.keyId = param->u.crypt.idx ; + pWapiSta->wapiMsk.bTxEnable = false; + if (!pWapiSta->bSetkeyOk) + pWapiSta->bSetkeyOk = true; + pWapiSta->bAuthenticateInProgress = false; + + _rtw_memcpy(pWapiSta->lastRxMulticastPN, WapiAEMultiCastPNInitialValueSrc, 16); + + if (psecuritypriv->sw_decrypt == false) { + /* set rx broadcast key for ASUE */ + rtw_wapi_set_key(padapter, &pWapiSta->wapiMsk, pWapiSta, true, false); + } + } + } + } +} +#endif diff --git a/core/rtw_wapi_sms4.c b/core/rtw_wapi_sms4.c new file mode 100644 index 0000000..4b7cf95 --- /dev/null +++ b/core/rtw_wapi_sms4.c @@ -0,0 +1,922 @@ +/****************************************************************************** + * + * Copyright(c) 2016 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifdef CONFIG_WAPI_SUPPORT + +#include +#include +#include +#include + + +#ifdef CONFIG_WAPI_SW_SMS4 + +#define WAPI_LITTLE_ENDIAN +/* #define BIG_ENDIAN */ +#define ENCRYPT 0 +#define DECRYPT 1 + + +/********************************************************** + **********************************************************/ +const u8 Sbox[256] = { + 0xd6, 0x90, 0xe9, 0xfe, 0xcc, 0xe1, 0x3d, 0xb7, 0x16, 0xb6, 0x14, 0xc2, 0x28, 0xfb, 0x2c, 0x05, + 0x2b, 0x67, 0x9a, 0x76, 0x2a, 0xbe, 0x04, 0xc3, 0xaa, 0x44, 0x13, 0x26, 0x49, 0x86, 0x06, 0x99, + 0x9c, 0x42, 0x50, 0xf4, 0x91, 0xef, 0x98, 0x7a, 0x33, 0x54, 0x0b, 0x43, 0xed, 0xcf, 0xac, 0x62, + 0xe4, 0xb3, 0x1c, 0xa9, 0xc9, 0x08, 0xe8, 0x95, 0x80, 0xdf, 0x94, 0xfa, 0x75, 0x8f, 0x3f, 0xa6, + 0x47, 0x07, 0xa7, 0xfc, 0xf3, 0x73, 0x17, 0xba, 0x83, 0x59, 0x3c, 0x19, 0xe6, 0x85, 0x4f, 0xa8, + 0x68, 0x6b, 0x81, 0xb2, 0x71, 0x64, 0xda, 0x8b, 0xf8, 0xeb, 0x0f, 0x4b, 0x70, 0x56, 0x9d, 0x35, + 0x1e, 0x24, 0x0e, 0x5e, 0x63, 0x58, 0xd1, 0xa2, 0x25, 0x22, 0x7c, 0x3b, 0x01, 0x21, 0x78, 0x87, + 0xd4, 0x00, 0x46, 0x57, 0x9f, 0xd3, 0x27, 0x52, 0x4c, 0x36, 0x02, 0xe7, 0xa0, 0xc4, 0xc8, 0x9e, + 0xea, 0xbf, 0x8a, 0xd2, 0x40, 0xc7, 0x38, 0xb5, 0xa3, 0xf7, 0xf2, 0xce, 0xf9, 0x61, 0x15, 0xa1, + 0xe0, 0xae, 0x5d, 0xa4, 0x9b, 0x34, 0x1a, 0x55, 0xad, 0x93, 0x32, 0x30, 0xf5, 0x8c, 0xb1, 0xe3, + 0x1d, 0xf6, 0xe2, 0x2e, 0x82, 0x66, 0xca, 0x60, 0xc0, 0x29, 0x23, 0xab, 0x0d, 0x53, 0x4e, 0x6f, + 0xd5, 0xdb, 0x37, 0x45, 0xde, 0xfd, 0x8e, 0x2f, 0x03, 0xff, 0x6a, 0x72, 0x6d, 0x6c, 0x5b, 0x51, + 0x8d, 0x1b, 0xaf, 0x92, 0xbb, 0xdd, 0xbc, 0x7f, 0x11, 0xd9, 0x5c, 0x41, 0x1f, 0x10, 0x5a, 0xd8, + 0x0a, 0xc1, 0x31, 0x88, 0xa5, 0xcd, 0x7b, 0xbd, 0x2d, 0x74, 0xd0, 0x12, 0xb8, 0xe5, 0xb4, 0xb0, + 0x89, 0x69, 0x97, 0x4a, 0x0c, 0x96, 0x77, 0x7e, 0x65, 0xb9, 0xf1, 0x09, 0xc5, 0x6e, 0xc6, 0x84, + 0x18, 0xf0, 0x7d, 0xec, 0x3a, 0xdc, 0x4d, 0x20, 0x79, 0xee, 0x5f, 0x3e, 0xd7, 0xcb, 0x39, 0x48 +}; + +const u32 CK[32] = { + 0x00070e15, 0x1c232a31, 0x383f464d, 0x545b6269, + 0x70777e85, 0x8c939aa1, 0xa8afb6bd, 0xc4cbd2d9, + 0xe0e7eef5, 0xfc030a11, 0x181f262d, 0x343b4249, + 0x50575e65, 0x6c737a81, 0x888f969d, 0xa4abb2b9, + 0xc0c7ced5, 0xdce3eaf1, 0xf8ff060d, 0x141b2229, + 0x30373e45, 0x4c535a61, 0x686f767d, 0x848b9299, + 0xa0a7aeb5, 0xbcc3cad1, 0xd8dfe6ed, 0xf4fb0209, + 0x10171e25, 0x2c333a41, 0x484f565d, 0x646b7279 +}; + +#define Rotl(_x, _y) (((_x) << (_y)) | ((_x) >> (32 - (_y)))) + +#define ByteSub(_A) (Sbox[(_A) >> 24 & 0xFF] << 24 | \ + Sbox[(_A) >> 16 & 0xFF] << 16 | \ + Sbox[(_A) >> 8 & 0xFF] << 8 | \ + Sbox[(_A) & 0xFF]) + +#define L1(_B) ((_B) ^ Rotl(_B, 2) ^ Rotl(_B, 10) ^ Rotl(_B, 18) ^ Rotl(_B, 24)) +#define L2(_B) ((_B) ^ Rotl(_B, 13) ^ Rotl(_B, 23)) + +static void +xor_block(void *dst, void *src1, void *src2) +/* 128-bit xor: *dst = *src1 xor *src2. Pointers must be 32-bit aligned */ +{ + ((u32 *)dst)[0] = ((u32 *)src1)[0] ^ ((u32 *)src2)[0]; + ((u32 *)dst)[1] = ((u32 *)src1)[1] ^ ((u32 *)src2)[1]; + ((u32 *)dst)[2] = ((u32 *)src1)[2] ^ ((u32 *)src2)[2]; + ((u32 *)dst)[3] = ((u32 *)src1)[3] ^ ((u32 *)src2)[3]; +} + + +void SMS4Crypt(u8 *Input, u8 *Output, u32 *rk) +{ + u32 r, mid, x0, x1, x2, x3, *p; + p = (u32 *)Input; + x0 = p[0]; + x1 = p[1]; + x2 = p[2]; + x3 = p[3]; +#ifdef WAPI_LITTLE_ENDIAN + x0 = Rotl(x0, 16); + x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8); + x1 = Rotl(x1, 16); + x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8); + x2 = Rotl(x2, 16); + x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8); + x3 = Rotl(x3, 16); + x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8); +#endif + for (r = 0; r < 32; r += 4) { + mid = x1 ^ x2 ^ x3 ^ rk[r + 0]; + mid = ByteSub(mid); + x0 ^= L1(mid); + mid = x2 ^ x3 ^ x0 ^ rk[r + 1]; + mid = ByteSub(mid); + x1 ^= L1(mid); + mid = x3 ^ x0 ^ x1 ^ rk[r + 2]; + mid = ByteSub(mid); + x2 ^= L1(mid); + mid = x0 ^ x1 ^ x2 ^ rk[r + 3]; + mid = ByteSub(mid); + x3 ^= L1(mid); + } +#ifdef WAPI_LITTLE_ENDIAN + x0 = Rotl(x0, 16); + x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8); + x1 = Rotl(x1, 16); + x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8); + x2 = Rotl(x2, 16); + x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8); + x3 = Rotl(x3, 16); + x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8); +#endif + p = (u32 *)Output; + p[0] = x3; + p[1] = x2; + p[2] = x1; + p[3] = x0; +} + + + +void SMS4KeyExt(u8 *Key, u32 *rk, u32 CryptFlag) +{ + u32 r, mid, x0, x1, x2, x3, *p; + + p = (u32 *)Key; + x0 = p[0]; + x1 = p[1]; + x2 = p[2]; + x3 = p[3]; +#ifdef WAPI_LITTLE_ENDIAN + x0 = Rotl(x0, 16); + x0 = ((x0 & 0xFF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8); + x1 = Rotl(x1, 16); + x1 = ((x1 & 0xFF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8); + x2 = Rotl(x2, 16); + x2 = ((x2 & 0xFF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8); + x3 = Rotl(x3, 16); + x3 = ((x3 & 0xFF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8); +#endif + + x0 ^= 0xa3b1bac6; + x1 ^= 0x56aa3350; + x2 ^= 0x677d9197; + x3 ^= 0xb27022dc; + for (r = 0; r < 32; r += 4) { + mid = x1 ^ x2 ^ x3 ^ CK[r + 0]; + mid = ByteSub(mid); + rk[r + 0] = x0 ^= L2(mid); + mid = x2 ^ x3 ^ x0 ^ CK[r + 1]; + mid = ByteSub(mid); + rk[r + 1] = x1 ^= L2(mid); + mid = x3 ^ x0 ^ x1 ^ CK[r + 2]; + mid = ByteSub(mid); + rk[r + 2] = x2 ^= L2(mid); + mid = x0 ^ x1 ^ x2 ^ CK[r + 3]; + mid = ByteSub(mid); + rk[r + 3] = x3 ^= L2(mid); + } + if (CryptFlag == DECRYPT) { + for (r = 0; r < 16; r++) + mid = rk[r], rk[r] = rk[31 - r], rk[31 - r] = mid; + } +} + + +void WapiSMS4Cryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength, + u8 *Output, u16 *OutputLength, u32 CryptFlag) +{ + u32 blockNum, i, j, rk[32]; + u16 remainder; + u8 blockIn[16], blockOut[16], tempIV[16], k; + + *OutputLength = 0; + remainder = InputLength & 0x0F; + blockNum = InputLength >> 4; + if (remainder != 0) + blockNum++; + else + remainder = 16; + + for (k = 0; k < 16; k++) + tempIV[k] = IV[15 - k]; + + memcpy(blockIn, tempIV, 16); + + SMS4KeyExt((u8 *)Key, rk, CryptFlag); + + for (i = 0; i < blockNum - 1; i++) { + SMS4Crypt((u8 *)blockIn, blockOut, rk); + xor_block(&Output[i * 16], &Input[i * 16], blockOut); + memcpy(blockIn, blockOut, 16); + } + + *OutputLength = i * 16; + + SMS4Crypt((u8 *)blockIn, blockOut, rk); + + for (j = 0; j < remainder; j++) + Output[i * 16 + j] = Input[i * 16 + j] ^ blockOut[j]; + *OutputLength += remainder; + +} + +void WapiSMS4Encryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength, + u8 *Output, u16 *OutputLength) +{ + + WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT); +} + +void WapiSMS4Decryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength, + u8 *Output, u16 *OutputLength) +{ + /* OFB mode: is also ENCRYPT flag */ + WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT); +} + +void WapiSMS4CalculateMic(u8 *Key, u8 *IV, u8 *Input1, u8 Input1Length, + u8 *Input2, u16 Input2Length, u8 *Output, u8 *OutputLength) +{ + u32 blockNum, i, remainder, rk[32]; + u8 BlockIn[16], BlockOut[16], TempBlock[16], tempIV[16], k; + + *OutputLength = 0; + remainder = Input1Length & 0x0F; + blockNum = Input1Length >> 4; + + for (k = 0; k < 16; k++) + tempIV[k] = IV[15 - k]; + + memcpy(BlockIn, tempIV, 16); + + SMS4KeyExt((u8 *)Key, rk, ENCRYPT); + + SMS4Crypt((u8 *)BlockIn, BlockOut, rk); + + for (i = 0; i < blockNum; i++) { + xor_block(BlockIn, (Input1 + i * 16), BlockOut); + SMS4Crypt((u8 *)BlockIn, BlockOut, rk); + } + + if (remainder != 0) { + memset(TempBlock, 0, 16); + memcpy(TempBlock, (Input1 + blockNum * 16), remainder); + + xor_block(BlockIn, TempBlock, BlockOut); + SMS4Crypt((u8 *)BlockIn, BlockOut, rk); + } + + remainder = Input2Length & 0x0F; + blockNum = Input2Length >> 4; + + for (i = 0; i < blockNum; i++) { + xor_block(BlockIn, (Input2 + i * 16), BlockOut); + SMS4Crypt((u8 *)BlockIn, BlockOut, rk); + } + + if (remainder != 0) { + memset(TempBlock, 0, 16); + memcpy(TempBlock, (Input2 + blockNum * 16), remainder); + + xor_block(BlockIn, TempBlock, BlockOut); + SMS4Crypt((u8 *)BlockIn, BlockOut, rk); + } + + memcpy(Output, BlockOut, 16); + *OutputLength = 16; +} + +void SecCalculateMicSMS4( + u8 KeyIdx, + u8 *MicKey, + u8 *pHeader, + u8 *pData, + u16 DataLen, + u8 *MicBuffer +) +{ +#if 0 + struct ieee80211_hdr_3addr_qos *header; + u8 TempBuf[34], TempLen = 32, MicLen, QosOffset, *IV; + u16 *pTemp, fc; + + WAPI_TRACE(WAPI_TX | WAPI_RX, "=========>%s\n", __FUNCTION__); + + header = (struct ieee80211_hdr_3addr_qos *)pHeader; + memset(TempBuf, 0, 34); + memcpy(TempBuf, pHeader, 2); /* FrameCtrl */ + pTemp = (u16 *)TempBuf; + *pTemp &= 0xc78f; /* bit4,5,6,11,12,13 */ + + memcpy((TempBuf + 2), (pHeader + 4), 12); /* Addr1, Addr2 */ + memcpy((TempBuf + 14), (pHeader + 22), 2); /* SeqCtrl */ + pTemp = (u16 *)(TempBuf + 14); + *pTemp &= 0x000f; + + memcpy((TempBuf + 16), (pHeader + 16), 6); /* Addr3 */ + + fc = le16_to_cpu(header->frame_ctl); + + + + if (GetFrDs((u16 *)&fc) && GetToDs((u16 *)&fc)) { + memcpy((TempBuf + 22), (pHeader + 24), 6); + QosOffset = 30; + } else { + memset((TempBuf + 22), 0, 6); + QosOffset = 24; + } + + if ((fc & 0x0088) == 0x0088) { + memcpy((TempBuf + 28), (pHeader + QosOffset), 2); + TempLen += 2; + /* IV = pHeader + QosOffset + 2 + SNAP_SIZE + sizeof(u16) + 2; */ + IV = pHeader + QosOffset + 2 + 2; + } else { + IV = pHeader + QosOffset + 2; + /* IV = pHeader + QosOffset + SNAP_SIZE + sizeof(u16) + 2; */ + } + + TempBuf[TempLen - 1] = (u8)(DataLen & 0xff); + TempBuf[TempLen - 2] = (u8)((DataLen & 0xff00) >> 8); + TempBuf[TempLen - 4] = KeyIdx; + + WAPI_DATA(WAPI_TX, "CalculateMic - KEY", MicKey, 16); + WAPI_DATA(WAPI_TX, "CalculateMic - IV", IV, 16); + WAPI_DATA(WAPI_TX, "CalculateMic - TempBuf", TempBuf, TempLen); + WAPI_DATA(WAPI_TX, "CalculateMic - pData", pData, DataLen); + + WapiSMS4CalculateMic(MicKey, IV, TempBuf, TempLen, + pData, DataLen, MicBuffer, &MicLen); + + if (MicLen != 16) + WAPI_TRACE(WAPI_ERR, "%s: MIC Length Error!!\n", __FUNCTION__); + + WAPI_TRACE(WAPI_TX | WAPI_RX, "<=========%s\n", __FUNCTION__); +#endif +} + +/* AddCount: 1 or 2. + * If overflow, return 1, + * else return 0. + */ +u8 WapiIncreasePN(u8 *PN, u8 AddCount) +{ + u8 i; + + if (NULL == PN) + return 1; + /* YJ,test,091102 */ + /* + if(AddCount == 2){ + RTW_INFO("############################%s(): PN[0]=0x%x\n", __FUNCTION__, PN[0]); + if(PN[0] == 0x48){ + PN[0] += AddCount; + return 1; + }else{ + PN[0] += AddCount; + return 0; + } + } + */ + /* YJ,test,091102,end */ + + for (i = 0; i < 16; i++) { + if (PN[i] + AddCount <= 0xff) { + PN[i] += AddCount; + return 0; + } else { + PN[i] += AddCount; + AddCount = 1; + } + } + return 1; +} + + +void WapiGetLastRxUnicastPNForQoSData( + u8 UserPriority, + PRT_WAPI_STA_INFO pWapiStaInfo, + u8 *PNOut +) +{ + WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__); + switch (UserPriority) { + case 0: + case 3: + memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNBEQueue, 16); + break; + case 1: + case 2: + memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNBKQueue, 16); + break; + case 4: + case 5: + memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNVIQueue, 16); + break; + case 6: + case 7: + memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNVOQueue, 16); + break; + default: + WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__); + break; + } + WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__); +} + + +void WapiSetLastRxUnicastPNForQoSData( + u8 UserPriority, + u8 *PNIn, + PRT_WAPI_STA_INFO pWapiStaInfo +) +{ + WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__); + switch (UserPriority) { + case 0: + case 3: + memcpy(pWapiStaInfo->lastRxUnicastPNBEQueue, PNIn, 16); + break; + case 1: + case 2: + memcpy(pWapiStaInfo->lastRxUnicastPNBKQueue, PNIn, 16); + break; + case 4: + case 5: + memcpy(pWapiStaInfo->lastRxUnicastPNVIQueue, PNIn, 16); + break; + case 6: + case 7: + memcpy(pWapiStaInfo->lastRxUnicastPNVOQueue, PNIn, 16); + break; + default: + WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__); + break; + } + WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__); +} + + +/**************************************************************************** + FALSE not RX-Reorder + TRUE do RX Reorder +add to support WAPI to N-mode +*****************************************************************************/ +u8 WapiCheckPnInSwDecrypt( + _adapter *padapter, + struct sk_buff *pskb +) +{ + u8 ret = false; + +#if 0 + struct ieee80211_hdr_3addr_qos *header; + u16 fc; + u8 *pDaddr, *pTaddr, *pRaddr; + + header = (struct ieee80211_hdr_3addr_qos *)pskb->data; + pTaddr = header->addr2; + pRaddr = header->addr1; + fc = le16_to_cpu(header->frame_ctl); + + if (GetToDs(&fc)) + pDaddr = header->addr3; + else + pDaddr = header->addr1; + + if ((_rtw_memcmp(pRaddr, padapter->pnetdev->dev_addr, ETH_ALEN) == 0) + && !(pDaddr) + && (GetFrameType(&fc) == WIFI_QOS_DATA_TYPE)) + /* && ieee->pHTInfo->bCurrentHTSupport && */ + /* ieee->pHTInfo->bCurRxReorderEnable) */ + ret = false; + else + ret = true; +#endif + WAPI_TRACE(WAPI_RX, "%s: return %d\n", __FUNCTION__, ret); + return ret; +} + +int SecSMS4HeaderFillIV(_adapter *padapter, u8 *pxmitframe) +{ + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + u8 *frame = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET; + u8 *pSecHeader = NULL, *pos = NULL, *pRA = NULL; + u8 bPNOverflow = false, bFindMatchPeer = false, hdr_len = 0; + PWLAN_HEADER_WAPI_EXTENSION pWapiExt = NULL; + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + PRT_WAPI_STA_INFO pWapiSta = NULL; + int ret = 0; + + WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__); + + return ret; +#if 0 + hdr_len = sMacHdrLng; + if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE) + hdr_len += 2; + /* hdr_len += SNAP_SIZE + sizeof(u16); */ + + pos = skb_push(pskb, padapter->wapiInfo.extra_prefix_len); + memmove(pos, pos + padapter->wapiInfo.extra_prefix_len, hdr_len); + + pSecHeader = pskb->data + hdr_len; + pWapiExt = (PWLAN_HEADER_WAPI_EXTENSION)pSecHeader; + pRA = pskb->data + 4; + + WAPI_DATA(WAPI_TX, "FillIV - Before Fill IV", pskb->data, pskb->len); + + /* Address 1 is always receiver's address */ + if (IS_MCAST(pRA)) { + if (!pWapiInfo->wapiTxMsk.bTxEnable) { + WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__); + return -2; + } + if (pWapiInfo->wapiTxMsk.keyId <= 1) { + pWapiExt->KeyIdx = pWapiInfo->wapiTxMsk.keyId; + pWapiExt->Reserved = 0; + bPNOverflow = WapiIncreasePN(pWapiInfo->lastTxMulticastPN, 1); + memcpy(pWapiExt->PN, pWapiInfo->lastTxMulticastPN, 16); + if (bPNOverflow) { + /* Update MSK Notification. */ + WAPI_TRACE(WAPI_ERR, "===============>%s():multicast PN overflow\n", __FUNCTION__); + rtw_wapi_app_event_handler(padapter, NULL, 0, pRA, false, false, true, 0, false); + } + } else { + WAPI_TRACE(WAPI_ERR, "%s: Invalid Wapi Multicast KeyIdx!!\n", __FUNCTION__); + ret = -3; + } + } else { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (!memcmp(pWapiSta->PeerMacAddr, pRA, 6)) { + bFindMatchPeer = true; + break; + } + } + if (bFindMatchPeer) { + if ((!pWapiSta->wapiUskUpdate.bTxEnable) && (!pWapiSta->wapiUsk.bTxEnable)) { + WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__); + return -4; + } + if (pWapiSta->wapiUsk.keyId <= 1) { + if (pWapiSta->wapiUskUpdate.bTxEnable) + pWapiExt->KeyIdx = pWapiSta->wapiUskUpdate.keyId; + else + pWapiExt->KeyIdx = pWapiSta->wapiUsk.keyId; + + pWapiExt->Reserved = 0; + bPNOverflow = WapiIncreasePN(pWapiSta->lastTxUnicastPN, 2); + memcpy(pWapiExt->PN, pWapiSta->lastTxUnicastPN, 16); + if (bPNOverflow) { + /* Update USK Notification. */ + WAPI_TRACE(WAPI_ERR, "===============>%s():unicast PN overflow\n", __FUNCTION__); + rtw_wapi_app_event_handler(padapter, NULL, 0, pWapiSta->PeerMacAddr, false, true, false, 0, false); + } + } else { + WAPI_TRACE(WAPI_ERR, "%s: Invalid Wapi Unicast KeyIdx!!\n", __FUNCTION__); + ret = -5; + } + } else { + WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT"!!\n", __FUNCTION__, MAC_ARG(pRA)); + ret = -6; + } + } + + WAPI_DATA(WAPI_TX, "FillIV - After Fill IV", pskb->data, pskb->len); + WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__); + return ret; +#endif +} + +/* WAPI SW Enc: must have done Coalesce! */ +void SecSWSMS4Encryption( + _adapter *padapter, + u8 *pxmitframe +) +{ + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + PRT_WAPI_STA_INFO pWapiSta = NULL; + u8 *pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_SIZE; + struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib; + + u8 *SecPtr = NULL, *pRA, *pMicKey = NULL, *pDataKey = NULL, *pIV = NULL; + u8 IVOffset, DataOffset, bFindMatchPeer = false, KeyIdx = 0, MicBuffer[16]; + u16 OutputLength; + + WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__); + + WAPI_TRACE(WAPI_TX, "hdrlen: %d\n", pattrib->hdrlen); + + return; + + DataOffset = pattrib->hdrlen + pattrib->iv_len; + + pRA = pframe + 4; + + + if (IS_MCAST(pRA)) { + KeyIdx = pWapiInfo->wapiTxMsk.keyId; + pIV = pWapiInfo->lastTxMulticastPN; + pMicKey = pWapiInfo->wapiTxMsk.micKey; + pDataKey = pWapiInfo->wapiTxMsk.dataKey; + } else { + if (!list_empty(&(pWapiInfo->wapiSTAUsedList))) { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (0 == memcmp(pWapiSta->PeerMacAddr, pRA, 6)) { + bFindMatchPeer = true; + break; + } + } + + if (bFindMatchPeer) { + if (pWapiSta->wapiUskUpdate.bTxEnable) { + KeyIdx = pWapiSta->wapiUskUpdate.keyId; + WAPI_TRACE(WAPI_TX, "%s(): Use update USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx); + pIV = pWapiSta->lastTxUnicastPN; + pMicKey = pWapiSta->wapiUskUpdate.micKey; + pDataKey = pWapiSta->wapiUskUpdate.dataKey; + } else { + KeyIdx = pWapiSta->wapiUsk.keyId; + WAPI_TRACE(WAPI_TX, "%s(): Use USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx); + pIV = pWapiSta->lastTxUnicastPN; + pMicKey = pWapiSta->wapiUsk.micKey; + pDataKey = pWapiSta->wapiUsk.dataKey; + } + } else { + WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta!!\n", __FUNCTION__); + return; + } + } else { + WAPI_TRACE(WAPI_ERR, "%s: wapiSTAUsedList is empty!!\n", __FUNCTION__); + return; + } + } + + SecPtr = pframe; + SecCalculateMicSMS4(KeyIdx, pMicKey, SecPtr, (SecPtr + DataOffset), pattrib->pktlen, MicBuffer); + + WAPI_DATA(WAPI_TX, "Encryption - MIC", MicBuffer, padapter->wapiInfo.extra_postfix_len); + + memcpy(pframe + pattrib->hdrlen + pattrib->iv_len + pattrib->pktlen - pattrib->icv_len, + (u8 *)MicBuffer, + padapter->wapiInfo.extra_postfix_len + ); + + + WapiSMS4Encryption(pDataKey, pIV, (SecPtr + DataOffset), pattrib->pktlen + pattrib->icv_len, (SecPtr + DataOffset), &OutputLength); + + WAPI_DATA(WAPI_TX, "Encryption - After SMS4 encryption", pframe, pattrib->hdrlen + pattrib->iv_len + pattrib->pktlen); + + WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__); +} + +u8 SecSWSMS4Decryption( + _adapter *padapter, + u8 *precv_frame, + struct recv_priv *precv_priv +) +{ + PRT_WAPI_T pWapiInfo = &padapter->wapiInfo; + struct recv_frame_hdr *precv_hdr; + PRT_WAPI_STA_INFO pWapiSta = NULL; + u8 IVOffset, DataOffset, bFindMatchPeer = false, bUseUpdatedKey = false; + u8 KeyIdx, MicBuffer[16], lastRxPNforQoS[16]; + u8 *pRA, *pTA, *pMicKey, *pDataKey, *pLastRxPN, *pRecvPN, *pSecData, *pRecvMic, *pos; + u8 TID = 0; + u16 OutputLength, DataLen; + u8 bQosData; + struct sk_buff *pskb; + + WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__); + + return 0; + + precv_hdr = &((union recv_frame *)precv_frame)->u.hdr; + pskb = (struct sk_buff *)(precv_hdr->rx_data); + precv_hdr->bWapiCheckPNInDecrypt = WapiCheckPnInSwDecrypt(padapter, pskb); + WAPI_TRACE(WAPI_RX, "=========>%s: check PN %d\n", __FUNCTION__, precv_hdr->bWapiCheckPNInDecrypt); + WAPI_DATA(WAPI_RX, "Decryption - Before decryption", pskb->data, pskb->len); + + IVOffset = sMacHdrLng; + bQosData = GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE; + if (bQosData) + IVOffset += 2; + + /* if(GetHTC()) */ + /* IVOffset += 4; */ + + /* IVOffset += SNAP_SIZE + sizeof(u16); */ + + DataOffset = IVOffset + padapter->wapiInfo.extra_prefix_len; + + pRA = pskb->data + 4; + pTA = pskb->data + 10; + KeyIdx = *(pskb->data + IVOffset); + pRecvPN = pskb->data + IVOffset + 2; + pSecData = pskb->data + DataOffset; + DataLen = pskb->len - DataOffset; + pRecvMic = pskb->data + pskb->len - padapter->wapiInfo.extra_postfix_len; + TID = GetTid(pskb->data); + + if (!list_empty(&(pWapiInfo->wapiSTAUsedList))) { + list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) { + if (0 == memcmp(pWapiSta->PeerMacAddr, pTA, 6)) { + bFindMatchPeer = true; + break; + } + } + } + + if (!bFindMatchPeer) { + WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT" for Key Info!!!\n", __FUNCTION__, MAC_ARG(pTA)); + return false; + } + + if (IS_MCAST(pRA)) { + WAPI_TRACE(WAPI_RX, "%s: Multicast decryption !!!\n", __FUNCTION__); + if (pWapiSta->wapiMsk.keyId == KeyIdx && pWapiSta->wapiMsk.bSet) { + pLastRxPN = pWapiSta->lastRxMulticastPN; + if (!WapiComparePN(pRecvPN, pLastRxPN)) { + WAPI_TRACE(WAPI_ERR, "%s: MSK PN is not larger than last, Dropped!!!\n", __FUNCTION__); + WAPI_DATA(WAPI_ERR, "pRecvPN:", pRecvPN, 16); + WAPI_DATA(WAPI_ERR, "pLastRxPN:", pLastRxPN, 16); + return false; + } + + memcpy(pLastRxPN, pRecvPN, 16); + pMicKey = pWapiSta->wapiMsk.micKey; + pDataKey = pWapiSta->wapiMsk.dataKey; + } else if (pWapiSta->wapiMskUpdate.keyId == KeyIdx && pWapiSta->wapiMskUpdate.bSet) { + WAPI_TRACE(WAPI_RX, "%s: Use Updated MSK for Decryption !!!\n", __FUNCTION__); + bUseUpdatedKey = true; + memcpy(pWapiSta->lastRxMulticastPN, pRecvPN, 16); + pMicKey = pWapiSta->wapiMskUpdate.micKey; + pDataKey = pWapiSta->wapiMskUpdate.dataKey; + } else { + WAPI_TRACE(WAPI_ERR, "%s: Can not find MSK with matched KeyIdx(%d), Dropped !!!\n", __FUNCTION__, KeyIdx); + return false; + } + } else { + WAPI_TRACE(WAPI_RX, "%s: Unicast decryption !!!\n", __FUNCTION__); + if (pWapiSta->wapiUsk.keyId == KeyIdx && pWapiSta->wapiUsk.bSet) { + WAPI_TRACE(WAPI_RX, "%s: Use USK for Decryption!!!\n", __FUNCTION__); + if (precv_hdr->bWapiCheckPNInDecrypt) { + if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE) { + WapiGetLastRxUnicastPNForQoSData(TID, pWapiSta, lastRxPNforQoS); + pLastRxPN = lastRxPNforQoS; + } else + pLastRxPN = pWapiSta->lastRxUnicastPN; + if (!WapiComparePN(pRecvPN, pLastRxPN)) + return false; + if (bQosData) + WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta); + else + memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16); + } else + memcpy(precv_hdr->WapiTempPN, pRecvPN, 16); + + if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) { + if ((pRecvPN[0] & 0x1) == 0) { + WAPI_TRACE(WAPI_ERR, "%s: Rx USK PN is not odd when Infra STA mode, Dropped !!!\n", __FUNCTION__); + return false; + } + } + + pMicKey = pWapiSta->wapiUsk.micKey; + pDataKey = pWapiSta->wapiUsk.dataKey; + } else if (pWapiSta->wapiUskUpdate.keyId == KeyIdx && pWapiSta->wapiUskUpdate.bSet) { + WAPI_TRACE(WAPI_RX, "%s: Use Updated USK for Decryption!!!\n", __FUNCTION__); + if (pWapiSta->bAuthenticatorInUpdata) + bUseUpdatedKey = true; + else + bUseUpdatedKey = false; + + if (bQosData) + WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta); + else + memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16); + pMicKey = pWapiSta->wapiUskUpdate.micKey; + pDataKey = pWapiSta->wapiUskUpdate.dataKey; + } else { + WAPI_TRACE(WAPI_ERR, "%s: No valid USK!!!KeyIdx=%d pWapiSta->wapiUsk.keyId=%d pWapiSta->wapiUskUpdate.keyId=%d\n", __FUNCTION__, KeyIdx, pWapiSta->wapiUsk.keyId, + pWapiSta->wapiUskUpdate.keyId); + /* dump_buf(pskb->data,pskb->len); */ + return false; + } + } + + WAPI_DATA(WAPI_RX, "Decryption - DataKey", pDataKey, 16); + WAPI_DATA(WAPI_RX, "Decryption - IV", pRecvPN, 16); + WapiSMS4Decryption(pDataKey, pRecvPN, pSecData, DataLen, pSecData, &OutputLength); + + if (OutputLength != DataLen) + WAPI_TRACE(WAPI_ERR, "%s: Output Length Error!!!!\n", __FUNCTION__); + + WAPI_DATA(WAPI_RX, "Decryption - After decryption", pskb->data, pskb->len); + + DataLen -= padapter->wapiInfo.extra_postfix_len; + + SecCalculateMicSMS4(KeyIdx, pMicKey, pskb->data, pSecData, DataLen, MicBuffer); + + WAPI_DATA(WAPI_RX, "Decryption - MIC received", pRecvMic, SMS4_MIC_LEN); + WAPI_DATA(WAPI_RX, "Decryption - MIC calculated", MicBuffer, SMS4_MIC_LEN); + + if (0 == memcmp(MicBuffer, pRecvMic, padapter->wapiInfo.extra_postfix_len)) { + WAPI_TRACE(WAPI_RX, "%s: Check MIC OK!!\n", __FUNCTION__); + if (bUseUpdatedKey) { + /* delete the old key */ + if (IS_MCAST(pRA)) { + WAPI_TRACE(WAPI_API, "%s(): AE use new update MSK!!\n", __FUNCTION__); + pWapiSta->wapiMsk.keyId = pWapiSta->wapiMskUpdate.keyId; + memcpy(pWapiSta->wapiMsk.dataKey, pWapiSta->wapiMskUpdate.dataKey, 16); + memcpy(pWapiSta->wapiMsk.micKey, pWapiSta->wapiMskUpdate.micKey, 16); + pWapiSta->wapiMskUpdate.bTxEnable = pWapiSta->wapiMskUpdate.bSet = false; + } else { + WAPI_TRACE(WAPI_API, "%s(): AE use new update USK!!\n", __FUNCTION__); + pWapiSta->wapiUsk.keyId = pWapiSta->wapiUskUpdate.keyId; + memcpy(pWapiSta->wapiUsk.dataKey, pWapiSta->wapiUskUpdate.dataKey, 16); + memcpy(pWapiSta->wapiUsk.micKey, pWapiSta->wapiUskUpdate.micKey, 16); + pWapiSta->wapiUskUpdate.bTxEnable = pWapiSta->wapiUskUpdate.bSet = false; + } + } + } else { + WAPI_TRACE(WAPI_ERR, "%s: Check MIC Error, Dropped !!!!\n", __FUNCTION__); + return false; + } + + pos = pskb->data; + memmove(pos + padapter->wapiInfo.extra_prefix_len, pos, IVOffset); + skb_pull(pskb, padapter->wapiInfo.extra_prefix_len); + + WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__); + + return true; +} + +u32 rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe) +{ + + u8 *pframe; + u32 res = _SUCCESS; + + WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) { + WAPI_TRACE(WAPI_TX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__); + return _FAIL; + } + + if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL) + return _FAIL; + + pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET; + + SecSWSMS4Encryption(padapter, pxmitframe); + + WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__); + return res; +} + +u32 rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe) +{ + u8 *pframe; + u32 res = _SUCCESS; + + WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__); + + if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) { + WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__); + return _FAIL; + } + + + /* drop packet when hw decrypt fail + * return tempraily */ + return _FAIL; + + /* pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data; */ + + if (false == SecSWSMS4Decryption(padapter, precvframe, &padapter->recvpriv)) { + WAPI_TRACE(WAPI_ERR, "%s():SMS4 decrypt frame error\n", __FUNCTION__); + return _FAIL; + } + + WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__); + return res; +} + +#else + +u32 rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe) +{ + WAPI_TRACE(WAPI_TX, "=========>Dummy %s\n", __FUNCTION__); + WAPI_TRACE(WAPI_TX, "<=========Dummy %s\n", __FUNCTION__); + return _SUCCESS; +} + +u32 rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe) +{ + WAPI_TRACE(WAPI_RX, "=========>Dummy %s\n", __FUNCTION__); + WAPI_TRACE(WAPI_RX, "<=========Dummy %s\n", __FUNCTION__); + return _SUCCESS; +} + +#endif + +#endif diff --git a/core/rtw_wlan_util.c b/core/rtw_wlan_util.c new file mode 100644 index 0000000..1a0dd02 --- /dev/null +++ b/core/rtw_wlan_util.c @@ -0,0 +1,5664 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_WLAN_UTIL_C_ + +#include +#include + +#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) + #include + #define ETH_TYPE_OFFSET 12 + #define PROTOCOL_OFFSET 23 + #define IP_OFFSET 30 + #define IPv6_OFFSET 38 + #define IPv6_PROTOCOL_OFFSET 20 +#endif + +unsigned char ARTHEROS_OUI1[] = {0x00, 0x03, 0x7f}; +unsigned char ARTHEROS_OUI2[] = {0x00, 0x13, 0x74}; + +unsigned char BROADCOM_OUI1[] = {0x00, 0x10, 0x18}; +unsigned char BROADCOM_OUI2[] = {0x00, 0x0a, 0xf7}; +unsigned char BROADCOM_OUI3[] = {0x00, 0x05, 0xb5}; + + +unsigned char CISCO_OUI[] = {0x00, 0x40, 0x96}; +unsigned char MARVELL_OUI[] = {0x00, 0x50, 0x43}; +unsigned char RALINK_OUI[] = {0x00, 0x0c, 0x43}; +unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c}; +unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5}; + +unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20}; + +extern unsigned char RTW_WPA_OUI[]; +extern unsigned char WPA_TKIP_CIPHER[4]; +extern unsigned char RSN_TKIP_CIPHER[4]; + +#define R2T_PHY_DELAY (0) + +/* #define WAIT_FOR_BCN_TO_MIN (3000) */ +#define WAIT_FOR_BCN_TO_MIN (6000) +#define WAIT_FOR_BCN_TO_MAX (20000) + +static u8 rtw_basic_rate_cck[4] = { + IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK, + IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK +}; + +static u8 rtw_basic_rate_ofdm[3] = { + IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK, + IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK +}; + +static u8 rtw_basic_rate_mix[7] = { + IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK, + IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK, + IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK, + IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK +}; + +extern u8 WIFI_CCKRATES[]; +bool rtw_is_cck_rate(u8 rate) +{ + int i; + + for (i = 0; i < 4; i++) + if ((WIFI_CCKRATES[i] & 0x7F) == (rate & 0x7F)) + return 1; + return 0; +} + +extern u8 WIFI_OFDMRATES[]; +bool rtw_is_ofdm_rate(u8 rate) +{ + int i; + + for (i = 0; i < 8; i++) + if ((WIFI_OFDMRATES[i] & 0x7F) == (rate & 0x7F)) + return 1; + return 0; +} + +/* test if rate is defined in rtw_basic_rate_cck */ +bool rtw_is_basic_rate_cck(u8 rate) +{ + int i; + + for (i = 0; i < 4; i++) + if ((rtw_basic_rate_cck[i] & 0x7F) == (rate & 0x7F)) + return 1; + return 0; +} + +/* test if rate is defined in rtw_basic_rate_ofdm */ +bool rtw_is_basic_rate_ofdm(u8 rate) +{ + int i; + + for (i = 0; i < 3; i++) + if ((rtw_basic_rate_ofdm[i] & 0x7F) == (rate & 0x7F)) + return 1; + return 0; +} + +/* test if rate is defined in rtw_basic_rate_mix */ +bool rtw_is_basic_rate_mix(u8 rate) +{ + int i; + + for (i = 0; i < 7; i++) + if ((rtw_basic_rate_mix[i] & 0x7F) == (rate & 0x7F)) + return 1; + return 0; +} +#ifdef CONFIG_BCN_CNT_CONFIRM_HDL +int new_bcn_max = 3; +#endif +int cckrates_included(unsigned char *rate, int ratelen) +{ + int i; + + for (i = 0; i < ratelen; i++) { + if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) || + (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22)) + return _TRUE; + } + + return _FALSE; + +} + +int cckratesonly_included(unsigned char *rate, int ratelen) +{ + int i; + + for (i = 0; i < ratelen; i++) { + if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) && + (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22)) + return _FALSE; + } + + return _TRUE; +} + +s8 rtw_get_sta_rx_nss(_adapter *adapter, struct sta_info *psta) +{ + s8 nss = 1; + + if (!psta) + return nss; + + nss = GET_HAL_RX_NSS(adapter); + +#ifdef CONFIG_80211N_HT + #ifdef CONFIG_80211AC_VHT + if (psta->vhtpriv.vht_option) + nss = rtw_min(nss, rtw_vht_mcsmap_to_nss(psta->vhtpriv.vht_mcs_map)); + else + #endif /* CONFIG_80211AC_VHT */ + if (psta->htpriv.ht_option) + nss = rtw_min(nss, rtw_ht_mcsset_to_nss(psta->htpriv.ht_cap.supp_mcs_set)); +#endif /*CONFIG_80211N_HT*/ + RTW_INFO("%s: %d ss\n", __func__, nss); + return nss; +} + +s8 rtw_get_sta_tx_nss(_adapter *adapter, struct sta_info *psta) +{ + s8 nss = 1; + + if (!psta) + return nss; + + nss = GET_HAL_TX_NSS(adapter); + +#ifdef CONFIG_80211N_HT + #ifdef CONFIG_80211AC_VHT + if (psta->vhtpriv.vht_option) + nss = rtw_min(nss, rtw_vht_mcsmap_to_nss(psta->vhtpriv.vht_mcs_map)); + else + #endif /* CONFIG_80211AC_VHT */ + if (psta->htpriv.ht_option) + nss = rtw_min(nss, rtw_ht_mcsset_to_nss(psta->htpriv.ht_cap.supp_mcs_set)); +#endif /*CONFIG_80211N_HT*/ + RTW_INFO("%s: %d SS\n", __func__, nss); + return nss; +} + +u8 judge_network_type(_adapter *padapter, unsigned char *rate, int ratelen) +{ + u8 network_type = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + + if (pmlmeext->cur_channel > 14) { + if (pmlmeinfo->VHT_enable) + network_type = WIRELESS_11AC; + else if (pmlmeinfo->HT_enable) + network_type = WIRELESS_11_5N; + + network_type |= WIRELESS_11A; + } else { + if (pmlmeinfo->HT_enable) + network_type = WIRELESS_11_24N; + + if ((cckratesonly_included(rate, ratelen)) == _TRUE) + network_type |= WIRELESS_11B; + else if ((cckrates_included(rate, ratelen)) == _TRUE) + network_type |= WIRELESS_11BG; + else + network_type |= WIRELESS_11G; + } + + return network_type; +} + +unsigned char ratetbl_val_2wifirate(unsigned char rate); +unsigned char ratetbl_val_2wifirate(unsigned char rate) +{ + unsigned char val = 0; + + switch (rate & 0x7f) { + case 0: + val = IEEE80211_CCK_RATE_1MB; + break; + + case 1: + val = IEEE80211_CCK_RATE_2MB; + break; + + case 2: + val = IEEE80211_CCK_RATE_5MB; + break; + + case 3: + val = IEEE80211_CCK_RATE_11MB; + break; + + case 4: + val = IEEE80211_OFDM_RATE_6MB; + break; + + case 5: + val = IEEE80211_OFDM_RATE_9MB; + break; + + case 6: + val = IEEE80211_OFDM_RATE_12MB; + break; + + case 7: + val = IEEE80211_OFDM_RATE_18MB; + break; + + case 8: + val = IEEE80211_OFDM_RATE_24MB; + break; + + case 9: + val = IEEE80211_OFDM_RATE_36MB; + break; + + case 10: + val = IEEE80211_OFDM_RATE_48MB; + break; + + case 11: + val = IEEE80211_OFDM_RATE_54MB; + break; + + } + + return val; + +} + +int is_basicrate(_adapter *padapter, unsigned char rate); +int is_basicrate(_adapter *padapter, unsigned char rate) +{ + int i; + unsigned char val; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + for (i = 0; i < NumRates; i++) { + val = pmlmeext->basicrate[i]; + + if ((val != 0xff) && (val != 0xfe)) { + if (rate == ratetbl_val_2wifirate(val)) + return _TRUE; + } + } + + return _FALSE; +} + +unsigned int ratetbl2rateset(_adapter *padapter, unsigned char *rateset); +unsigned int ratetbl2rateset(_adapter *padapter, unsigned char *rateset) +{ + int i; + unsigned char rate; + unsigned int len = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + for (i = 0; i < NumRates; i++) { + rate = pmlmeext->datarate[i]; + + if (rtw_get_oper_ch(padapter) > 14 && rate < _6M_RATE_) /*5G no support CCK rate*/ + continue; + + switch (rate) { + case 0xff: + return len; + + case 0xfe: + continue; + + default: + rate = ratetbl_val_2wifirate(rate); + + if (is_basicrate(padapter, rate) == _TRUE) + rate |= IEEE80211_BASIC_RATE_MASK; + + rateset[len] = rate; + len++; + break; + } + } + return len; +} + +void get_rate_set(_adapter *padapter, unsigned char *pbssrate, int *bssrate_len) +{ + unsigned char supportedrates[NumRates]; + + _rtw_memset(supportedrates, 0, NumRates); + *bssrate_len = ratetbl2rateset(padapter, supportedrates); + _rtw_memcpy(pbssrate, supportedrates, *bssrate_len); +} + +void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask) +{ + u8 mcs_rate_1r = (u8)(mask & 0xff); + u8 mcs_rate_2r = (u8)((mask >> 8) & 0xff); + u8 mcs_rate_3r = (u8)((mask >> 16) & 0xff); + u8 mcs_rate_4r = (u8)((mask >> 24) & 0xff); + + mcs_set[0] &= mcs_rate_1r; + mcs_set[1] &= mcs_rate_2r; + mcs_set[2] &= mcs_rate_3r; + mcs_set[3] &= mcs_rate_4r; +} + +void UpdateBrateTbl( + PADAPTER Adapter, + u8 *mBratesOS +) +{ + u8 i; + u8 rate; + + /* 1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */ + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + rate = mBratesOS[i] & 0x7f; + switch (rate) { + case IEEE80211_CCK_RATE_1MB: + case IEEE80211_CCK_RATE_2MB: + case IEEE80211_CCK_RATE_5MB: + case IEEE80211_CCK_RATE_11MB: + case IEEE80211_OFDM_RATE_6MB: + case IEEE80211_OFDM_RATE_12MB: + case IEEE80211_OFDM_RATE_24MB: + mBratesOS[i] |= IEEE80211_BASIC_RATE_MASK; + break; + } + } + +} + +void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen) +{ + u8 i; + u8 rate; + + for (i = 0; i < bssratelen; i++) { + rate = bssrateset[i] & 0x7f; + switch (rate) { + case IEEE80211_CCK_RATE_1MB: + case IEEE80211_CCK_RATE_2MB: + case IEEE80211_CCK_RATE_5MB: + case IEEE80211_CCK_RATE_11MB: + bssrateset[i] |= IEEE80211_BASIC_RATE_MASK; + break; + } + } + +} +void Set_MSR(_adapter *padapter, u8 type) +{ + rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type)); +} + +inline u8 rtw_get_oper_ch(_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_channel; +} + +inline void rtw_set_oper_ch(_adapter *adapter, u8 ch) +{ +#ifdef DBG_CH_SWITCH + const int len = 128; + char msg[128] = {0}; + int cnt = 0; + int i = 0; +#endif /* DBG_CH_SWITCH */ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + + if (dvobj->oper_channel != ch) { + dvobj->on_oper_ch_time = rtw_get_current_time(); + +#ifdef DBG_CH_SWITCH + cnt += snprintf(msg + cnt, len - cnt, "switch to ch %3u", ch); + + for (i = 0; i < dvobj->iface_nums; i++) { + _adapter *iface = dvobj->padapters[i]; + cnt += snprintf(msg + cnt, len - cnt, " ["ADPT_FMT":", ADPT_ARG(iface)); + if (iface->mlmeextpriv.cur_channel == ch) + cnt += snprintf(msg + cnt, len - cnt, "C"); + else + cnt += snprintf(msg + cnt, len - cnt, "_"); + if (iface->wdinfo.listen_channel == ch && !rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_NONE)) + cnt += snprintf(msg + cnt, len - cnt, "L"); + else + cnt += snprintf(msg + cnt, len - cnt, "_"); + cnt += snprintf(msg + cnt, len - cnt, "]"); + } + + RTW_INFO(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(adapter), msg); +#endif /* DBG_CH_SWITCH */ + } + + dvobj->oper_channel = ch; +} + +inline u8 rtw_get_oper_bw(_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_bwmode; +} + +inline void rtw_set_oper_bw(_adapter *adapter, u8 bw) +{ + adapter_to_dvobj(adapter)->oper_bwmode = bw; +} + +inline u8 rtw_get_oper_choffset(_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->oper_ch_offset; +} + +inline void rtw_set_oper_choffset(_adapter *adapter, u8 offset) +{ + adapter_to_dvobj(adapter)->oper_ch_offset = offset; +} + +inline systime rtw_get_on_oper_ch_time(_adapter *adapter) +{ + return adapter_to_dvobj(adapter)->on_oper_ch_time; +} + +inline systime rtw_get_on_cur_ch_time(_adapter *adapter) +{ + if (adapter->mlmeextpriv.cur_channel == adapter_to_dvobj(adapter)->oper_channel) + return adapter_to_dvobj(adapter)->on_oper_ch_time; + else + return 0; +} + +void set_channel_bwmode(_adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode) +{ + u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE; +#if (defined(CONFIG_TDLS) && defined(CONFIG_TDLS_CH_SW)) || defined(CONFIG_MCC_MODE) + u8 iqk_info_backup = _FALSE; +#endif + + if (padapter->bNotifyChannelChange) + RTW_INFO("[%s] ch = %d, offset = %d, bwmode = %d\n", __FUNCTION__, channel, channel_offset, bwmode); + + center_ch = rtw_get_center_ch(channel, bwmode, channel_offset); + + if (bwmode == CHANNEL_WIDTH_80) { + if (center_ch > channel) + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_LOWER; + else if (center_ch < channel) + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_UPPER; + else + chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + _enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL); + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) { + /* driver doesn't set channel setting reg under MCC */ + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) + RTW_INFO("Warning: Do not set channel setting reg MCC mode\n"); + } +#endif + +#ifdef CONFIG_DFS_MASTER + { + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + bool ori_overlap_radar_detect_ch = rtw_rfctl_overlap_radar_detect_ch(rfctl); + bool new_overlap_radar_detect_ch = _rtw_rfctl_overlap_radar_detect_ch(rfctl, channel, bwmode, channel_offset); + + if (new_overlap_radar_detect_ch && IS_CH_WAITING(rfctl)) { + u8 pause = 0xFF; + + rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &pause); + } +#endif /* CONFIG_DFS_MASTER */ + + /* set Channel */ + /* saved channel/bw info */ + rtw_set_oper_ch(padapter, channel); + rtw_set_oper_bw(padapter, bwmode); + rtw_set_oper_choffset(padapter, channel_offset); + +#if (defined(CONFIG_TDLS) && defined(CONFIG_TDLS_CH_SW)) || defined(CONFIG_MCC_MODE) + /* To check if we need to backup iqk info after switch chnl & bw */ + { + u8 take_care_iqk, do_iqk; + + rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk); + rtw_hal_get_hwreg(padapter, HW_VAR_DO_IQK, &do_iqk); + if ((take_care_iqk == _TRUE) && (do_iqk == _TRUE)) + iqk_info_backup = _TRUE; + } +#endif + + rtw_hal_set_chnl_bw(padapter, center_ch, bwmode, channel_offset, chnl_offset80); /* set center channel */ + +#if (defined(CONFIG_TDLS) && defined(CONFIG_TDLS_CH_SW)) || defined(CONFIG_MCC_MODE) + if (iqk_info_backup == _TRUE) + rtw_hal_ch_sw_iqk_info_backup(padapter); +#endif + +#ifdef CONFIG_DFS_MASTER + if (new_overlap_radar_detect_ch) + rtw_odm_radar_detect_enable(padapter); + else if (ori_overlap_radar_detect_ch) { + u8 pause = 0x00; + + rtw_odm_radar_detect_disable(padapter); + rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &pause); + } + } +#endif /* CONFIG_DFS_MASTER */ + + _exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL); +} + +__inline u8 *get_my_bssid(WLAN_BSSID_EX *pnetwork) +{ + return pnetwork->MacAddress; +} + +u16 get_beacon_interval(WLAN_BSSID_EX *bss) +{ + unsigned short val; + _rtw_memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->IEs), 2); + + return le16_to_cpu(val); + +} + +int is_client_associated_to_ap(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext; + struct mlme_ext_info *pmlmeinfo; + + if (!padapter) + return _FAIL; + + pmlmeext = &padapter->mlmeextpriv; + pmlmeinfo = &(pmlmeext->mlmext_info); + + if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)) + return _TRUE; + else + return _FAIL; +} + +int is_client_associated_to_ibss(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) + return _TRUE; + else + return _FAIL; +} + +int is_IBSS_empty(_adapter *padapter) +{ + int i; + struct macid_ctl_t *macid_ctl = &padapter->dvobj->macid_ctl; + + for (i = 0; i < macid_ctl->num; i++) { + if (!rtw_macid_is_used(macid_ctl, i)) + continue; + if (!rtw_macid_is_iface_specific(macid_ctl, i, padapter)) + continue; + if (!GET_H2CCMD_MSRRPT_PARM_OPMODE(&macid_ctl->h2c_msr[i])) + continue; + if (GET_H2CCMD_MSRRPT_PARM_ROLE(&macid_ctl->h2c_msr[i]) == H2C_MSR_ROLE_ADHOC) + return _FAIL; + } + + return _TRUE; +} + +unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval) +{ + if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN) + return WAIT_FOR_BCN_TO_MIN; + else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX) + return WAIT_FOR_BCN_TO_MAX; + else + return bcn_interval << 2; +} + +void invalidate_cam_all(_adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + u8 bmc_id = rtw_iface_bcmc_id_get(padapter); + _irqL irqL; + u8 val8 = 0; + + rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, &val8); + + _enter_critical_bh(&cam_ctl->lock, &irqL); + + rtw_sec_cam_map_clr_all(&cam_ctl->used); + +#ifndef SEC_DEFAULT_KEY_SEARCH + /* for BMC data TX with force camid */ + if (bmc_id != INVALID_SEC_MAC_CAM_ID) { + rtw_sec_cam_map_set(&cam_ctl->used, bmc_id); + if (_rtw_camctl_chk_cap(padapter, SEC_CAP_CHK_EXTRA_SEC)) + rtw_sec_cam_map_set(&cam_ctl->used, bmc_id + 1); + } +#endif + + _rtw_memset(dvobj->cam_cache, 0, sizeof(struct sec_cam_ent) * SEC_CAM_ENT_NUM_SW_LIMIT); + _exit_critical_bh(&cam_ctl->lock, &irqL); + +#ifdef SEC_DEFAULT_KEY_SEARCH//!BMC TX force camid + /* clear default key related key search setting */ + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)_FALSE); +#endif +} + +void _clear_cam_entry(_adapter *padapter, u8 entry) +{ + unsigned char null_sta[6] = {0}; + unsigned char null_key[32] = {0}; + + rtw_sec_write_cam_ent(padapter, entry, 0, null_sta, null_key); +} + +inline void _write_cam(_adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key) +{ +#ifdef CONFIG_WRITE_CACHE_ONLY + write_cam_cache(adapter, id , ctrl, mac, key); +#else + rtw_sec_write_cam_ent(adapter, id, ctrl, mac, key); + write_cam_cache(adapter, id , ctrl, mac, key); +#endif +} + +inline void write_cam(_adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key) +{ + if (ctrl & BIT(9)) { + _write_cam(adapter, id, ctrl, mac, key); + _write_cam(adapter, (id + 1), ctrl | BIT(5), mac, (key + 16)); + RTW_INFO_DUMP("key-0: ", key, 16); + RTW_INFO_DUMP("key-1: ", (key + 16), 16); + } else + _write_cam(adapter, id, ctrl, mac, key); +} + +inline void clear_cam_entry(_adapter *adapter, u8 id) +{ + _clear_cam_entry(adapter, id); + clear_cam_cache(adapter, id); +} + +inline void write_cam_from_cache(_adapter *adapter, u8 id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + struct sec_cam_ent cache; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + _rtw_memcpy(&cache, &dvobj->cam_cache[id], sizeof(struct sec_cam_ent)); + _exit_critical_bh(&cam_ctl->lock, &irqL); + + rtw_sec_write_cam_ent(adapter, id, cache.ctrl, cache.mac, cache.key); +} +void write_cam_cache(_adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + + dvobj->cam_cache[id].ctrl = ctrl; + _rtw_memcpy(dvobj->cam_cache[id].mac, mac, ETH_ALEN); + _rtw_memcpy(dvobj->cam_cache[id].key, key, 16); + + _exit_critical_bh(&cam_ctl->lock, &irqL); +} + +void clear_cam_cache(_adapter *adapter, u8 id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + + _rtw_memset(&(dvobj->cam_cache[id]), 0, sizeof(struct sec_cam_ent)); + + _exit_critical_bh(&cam_ctl->lock, &irqL); +} + +inline bool _rtw_camctl_chk_cap(_adapter *adapter, u8 cap) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + if (cam_ctl->sec_cap & cap) + return _TRUE; + return _FALSE; +} + +inline void _rtw_camctl_set_flags(_adapter *adapter, u32 flags) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + cam_ctl->flags |= flags; +} + +inline void rtw_camctl_set_flags(_adapter *adapter, u32 flags) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + _rtw_camctl_set_flags(adapter, flags); + _exit_critical_bh(&cam_ctl->lock, &irqL); +} + +inline void _rtw_camctl_clr_flags(_adapter *adapter, u32 flags) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + cam_ctl->flags &= ~flags; +} + +inline void rtw_camctl_clr_flags(_adapter *adapter, u32 flags) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + _rtw_camctl_clr_flags(adapter, flags); + _exit_critical_bh(&cam_ctl->lock, &irqL); +} + +inline bool _rtw_camctl_chk_flags(_adapter *adapter, u32 flags) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + + if (cam_ctl->flags & flags) + return _TRUE; + return _FALSE; +} + +void dump_sec_cam_map(void *sel, struct sec_cam_bmp *map, u8 max_num) +{ + RTW_PRINT_SEL(sel, "0x%08x\n", map->m0); +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + if (max_num && max_num > 32) + RTW_PRINT_SEL(sel, "0x%08x\n", map->m1); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + if (max_num && max_num > 64) + RTW_PRINT_SEL(sel, "0x%08x\n", map->m2); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + if (max_num && max_num > 96) + RTW_PRINT_SEL(sel, "0x%08x\n", map->m3); +#endif +} + +inline bool rtw_sec_camid_is_set(struct sec_cam_bmp *map, u8 id) +{ + if (id < 32) + return map->m0 & BIT(id); +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + else if (id < 64) + return map->m1 & BIT(id - 32); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + else if (id < 96) + return map->m2 & BIT(id - 64); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + else if (id < 128) + return map->m3 & BIT(id - 96); +#endif + else + rtw_warn_on(1); + + return 0; +} + +inline void rtw_sec_cam_map_set(struct sec_cam_bmp *map, u8 id) +{ + if (id < 32) + map->m0 |= BIT(id); +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + else if (id < 64) + map->m1 |= BIT(id - 32); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + else if (id < 96) + map->m2 |= BIT(id - 64); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + else if (id < 128) + map->m3 |= BIT(id - 96); +#endif + else + rtw_warn_on(1); +} + +inline void rtw_sec_cam_map_clr(struct sec_cam_bmp *map, u8 id) +{ + if (id < 32) + map->m0 &= ~BIT(id); +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + else if (id < 64) + map->m1 &= ~BIT(id - 32); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + else if (id < 96) + map->m2 &= ~BIT(id - 64); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + else if (id < 128) + map->m3 &= ~BIT(id - 96); +#endif + else + rtw_warn_on(1); +} + +inline void rtw_sec_cam_map_clr_all(struct sec_cam_bmp *map) +{ + map->m0 = 0; +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + map->m1 = 0; +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + map->m2 = 0; +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + map->m3 = 0; +#endif +} + +inline bool rtw_sec_camid_is_drv_forbid(struct cam_ctl_t *cam_ctl, u8 id) +{ + struct sec_cam_bmp forbid_map; + + forbid_map.m0 = 0x00000ff0; +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + forbid_map.m1 = 0x00000000; +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + forbid_map.m2 = 0x00000000; +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + forbid_map.m3 = 0x00000000; +#endif + + if (id < 32) + return forbid_map.m0 & BIT(id); +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32) + else if (id < 64) + return forbid_map.m1 & BIT(id - 32); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64) + else if (id < 96) + return forbid_map.m2 & BIT(id - 64); +#endif +#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96) + else if (id < 128) + return forbid_map.m3 & BIT(id - 96); +#endif + else + rtw_warn_on(1); + + return 1; +} + +bool _rtw_sec_camid_is_used(struct cam_ctl_t *cam_ctl, u8 id) +{ + bool ret = _FALSE; + + if (id >= cam_ctl->num) { + rtw_warn_on(1); + goto exit; + } + +#if 0 /* for testing */ + if (rtw_sec_camid_is_drv_forbid(cam_ctl, id)) { + ret = _TRUE; + goto exit; + } +#endif + + ret = rtw_sec_camid_is_set(&cam_ctl->used, id); + +exit: + return ret; +} + +inline bool rtw_sec_camid_is_used(struct cam_ctl_t *cam_ctl, u8 id) +{ + _irqL irqL; + bool ret; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + ret = _rtw_sec_camid_is_used(cam_ctl, id); + _exit_critical_bh(&cam_ctl->lock, &irqL); + + return ret; +} +u8 rtw_get_sec_camid(_adapter *adapter, u8 max_bk_key_num, u8 *sec_key_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + int i; + _irqL irqL; + u8 sec_cam_num = 0; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + for (i = 0; i < cam_ctl->num; i++) { + if (_rtw_sec_camid_is_used(cam_ctl, i)) { + sec_key_id[sec_cam_num++] = i; + if (sec_cam_num == max_bk_key_num) + break; + } + } + _exit_critical_bh(&cam_ctl->lock, &irqL); + + return sec_cam_num; +} + +inline bool _rtw_camid_is_gk(_adapter *adapter, u8 cam_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + bool ret = _FALSE; + + if (cam_id >= cam_ctl->num) { + rtw_warn_on(1); + goto exit; + } + + if (_rtw_sec_camid_is_used(cam_ctl, cam_id) == _FALSE) + goto exit; + + ret = (dvobj->cam_cache[cam_id].ctrl & BIT6) ? _TRUE : _FALSE; + +exit: + return ret; +} + +inline bool rtw_camid_is_gk(_adapter *adapter, u8 cam_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + bool ret; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + ret = _rtw_camid_is_gk(adapter, cam_id); + _exit_critical_bh(&cam_ctl->lock, &irqL); + + return ret; +} + +bool cam_cache_chk(_adapter *adapter, u8 id, u8 *addr, s16 kid, s8 gk) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + bool ret = _FALSE; + + if (addr && _rtw_memcmp(dvobj->cam_cache[id].mac, addr, ETH_ALEN) == _FALSE) + goto exit; + if (kid >= 0 && kid != (dvobj->cam_cache[id].ctrl & 0x03)) + goto exit; + if (gk != -1 && (gk ? _TRUE : _FALSE) != _rtw_camid_is_gk(adapter, id)) + goto exit; + + ret = _TRUE; + +exit: + return ret; +} + +s16 _rtw_camid_search(_adapter *adapter, u8 *addr, s16 kid, s8 gk) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + int i; + s16 cam_id = -1; + + for (i = 0; i < cam_ctl->num; i++) { + if (cam_cache_chk(adapter, i, addr, kid, gk)) { + cam_id = i; + break; + } + } + + if (0) { + if (addr) + RTW_INFO(FUNC_ADPT_FMT" addr:"MAC_FMT" kid:%d, gk:%d, return cam_id:%d\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid, gk, cam_id); + else + RTW_INFO(FUNC_ADPT_FMT" addr:%p kid:%d, gk:%d, return cam_id:%d\n" + , FUNC_ADPT_ARG(adapter), addr, kid, gk, cam_id); + } + + return cam_id; +} + +s16 rtw_camid_search(_adapter *adapter, u8 *addr, s16 kid, s8 gk) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + s16 cam_id = -1; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + cam_id = _rtw_camid_search(adapter, addr, kid, gk); + _exit_critical_bh(&cam_ctl->lock, &irqL); + + return cam_id; +} + +s16 rtw_get_camid(_adapter *adapter, u8 *addr, s16 kid, u8 gk, bool ext_sec) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + int i; +#if 0 /* for testing */ + static u8 start_id = 0; +#else + u8 start_id = 0; +#endif + s16 cam_id = -1; + + if (addr == NULL) { + RTW_PRINT(FUNC_ADPT_FMT" mac_address is NULL\n" + , FUNC_ADPT_ARG(adapter)); + rtw_warn_on(1); + goto _exit; + } + + /* find cam entry which has the same addr, kid (, gk bit) */ + if (_rtw_camctl_chk_cap(adapter, SEC_CAP_CHK_BMC) == _TRUE) + i = _rtw_camid_search(adapter, addr, kid, gk); + else + i = _rtw_camid_search(adapter, addr, kid, -1); + + if (i >= 0) { + cam_id = i; + goto _exit; + } + + for (i = 0; i < cam_ctl->num; i++) { + /* bypass default key which is allocated statically */ +#ifdef SEC_DEFAULT_KEY_SEARCH + if (((i + start_id) % cam_ctl->num) < 4) + continue; +#endif + if (_rtw_sec_camid_is_used(cam_ctl, ((i + start_id) % cam_ctl->num)) == _FALSE) { + if (ext_sec) { + /* look out continue slot */ + if (((i + 1) < cam_ctl->num) && + (_rtw_sec_camid_is_used(cam_ctl, (((i + 1) + start_id) % cam_ctl->num)) == _FALSE)) + break; + else + continue; + } else + break; + } + } + + if (i == cam_ctl->num) { + RTW_PRINT(FUNC_ADPT_FMT" %s key with "MAC_FMT" id:%u no room\n" + , FUNC_ADPT_ARG(adapter), gk ? "group" : "pairwise", MAC_ARG(addr), kid); + rtw_warn_on(1); + goto _exit; + } + + cam_id = ((i + start_id) % cam_ctl->num); + start_id = ((i + start_id + 1) % cam_ctl->num); + +_exit: + return cam_id; +} + +s16 rtw_camid_alloc(_adapter *adapter, struct sta_info *sta, u8 kid, u8 gk, bool ext_sec, bool *used) +{ + struct mlme_ext_info *mlmeinfo = &adapter->mlmeextpriv.mlmext_info; + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + s16 cam_id = -1; + + *used = _FALSE; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + + if ((((mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((mlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) + && !sta) { + /* + * 1. non-STA mode WEP key + * 2. group TX key + */ +#ifdef SEC_DEFAULT_KEY_SEARCH + /* static alloction to default key by key ID when concurrent is not defined */ + if (kid > 3) { + RTW_PRINT(FUNC_ADPT_FMT" group key with invalid key id:%u\n" + , FUNC_ADPT_ARG(adapter), kid); + rtw_warn_on(1); + goto bitmap_handle; + } + cam_id = kid; +#else + u8 *addr = adapter_mac_addr(adapter); + + cam_id = rtw_get_camid(adapter, addr, kid, gk, ext_sec); + if (1) + RTW_PRINT(FUNC_ADPT_FMT" group key with "MAC_FMT" assigned cam_id:%u\n" + , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), cam_id); +#endif + } else { + /* + * 1. STA mode WEP key + * 2. STA mode group RX key + * 3. sta key (pairwise, group RX) + */ + u8 *addr = sta ? sta->cmn.mac_addr : NULL; + + if (!sta) { + if (!(mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) { + /* bypass STA mode group key setting before connected(ex:WEP) because bssid is not ready */ + goto bitmap_handle; + } + addr = get_bssid(&adapter->mlmepriv);/*A2*/ + } + cam_id = rtw_get_camid(adapter, addr, kid, gk, ext_sec); + } + + +bitmap_handle: + if (cam_id >= 0) { + *used = _rtw_sec_camid_is_used(cam_ctl, cam_id); + rtw_sec_cam_map_set(&cam_ctl->used, cam_id); + if (ext_sec) + rtw_sec_cam_map_set(&cam_ctl->used, cam_id + 1); + } + + _exit_critical_bh(&cam_ctl->lock, &irqL); + + return cam_id; +} + +void rtw_camid_set(_adapter *adapter, u8 cam_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + + if (cam_id < cam_ctl->num) + rtw_sec_cam_map_set(&cam_ctl->used, cam_id); + + _exit_critical_bh(&cam_ctl->lock, &irqL); +} + +void rtw_camid_free(_adapter *adapter, u8 cam_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + + if (cam_id < cam_ctl->num) + rtw_sec_cam_map_clr(&cam_ctl->used, cam_id); + + _exit_critical_bh(&cam_ctl->lock, &irqL); +} + +/*Must pause TX/RX before use this API*/ +inline void rtw_sec_cam_swap(_adapter *adapter, u8 cam_id_a, u8 cam_id_b) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + struct sec_cam_ent cache_a, cache_b; + _irqL irqL; + bool cam_a_used, cam_b_used; + + if (1) + RTW_INFO(ADPT_FMT" - sec_cam %d,%d swap\n", ADPT_ARG(adapter), cam_id_a, cam_id_b); + + if (cam_id_a == cam_id_b) + return; + + rtw_mi_update_ap_bmc_camid(adapter, cam_id_a, cam_id_b); + + /*setp-1. backup org cam_info*/ + _enter_critical_bh(&cam_ctl->lock, &irqL); + + cam_a_used = _rtw_sec_camid_is_used(cam_ctl, cam_id_a); + cam_b_used = _rtw_sec_camid_is_used(cam_ctl, cam_id_b); + + if (cam_a_used) + _rtw_memcpy(&cache_a, &dvobj->cam_cache[cam_id_a], sizeof(struct sec_cam_ent)); + + if (cam_b_used) + _rtw_memcpy(&cache_b, &dvobj->cam_cache[cam_id_b], sizeof(struct sec_cam_ent)); + + _exit_critical_bh(&cam_ctl->lock, &irqL); + + /*setp-2. clean cam_info*/ + if (cam_a_used) { + rtw_camid_free(adapter, cam_id_a); + clear_cam_entry(adapter, cam_id_a); + } + if (cam_b_used) { + rtw_camid_free(adapter, cam_id_b); + clear_cam_entry(adapter, cam_id_b); + } + + /*setp-3. set cam_info*/ + if (cam_a_used) { + write_cam(adapter, cam_id_b, cache_a.ctrl, cache_a.mac, cache_a.key); + rtw_camid_set(adapter, cam_id_b); + } + + if (cam_b_used) { + write_cam(adapter, cam_id_a, cache_b.ctrl, cache_b.mac, cache_b.key); + rtw_camid_set(adapter, cam_id_a); + } +} + +s16 rtw_get_empty_cam_entry(_adapter *adapter, u8 start_camid) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl; + _irqL irqL; + int i; + s16 cam_id = -1; + + _enter_critical_bh(&cam_ctl->lock, &irqL); + for (i = start_camid; i < cam_ctl->num; i++) { + if (_FALSE == _rtw_sec_camid_is_used(cam_ctl, i)) { + cam_id = i; + break; + } + } + _exit_critical_bh(&cam_ctl->lock, &irqL); + + return cam_id; +} +void rtw_clean_dk_section(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj); + s16 ept_cam_id; + int i; + + for (i = 0; i < 4; i++) { + if (rtw_sec_camid_is_used(cam_ctl, i)) { + ept_cam_id = rtw_get_empty_cam_entry(adapter, 4); + if (ept_cam_id > 0) + rtw_sec_cam_swap(adapter, i, ept_cam_id); + } + } +} +void rtw_clean_hw_dk_cam(_adapter *adapter) +{ + int i; + + for (i = 0; i < 4; i++) { + if (_rtw_camctl_chk_cap(adapter, SEC_CAP_CHK_WRITE_CAM_NEW_RULE)) + _clear_cam_entry(adapter, i); + else + rtw_sec_clr_cam_ent(adapter, i); + } +} + +void flush_all_cam_entry(_adapter *padapter) +{ +#ifdef CONFIG_CONCURRENT_MODE + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta; + + psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress); + if (psta) { + if (psta->state & WIFI_AP_STATE) { + /*clear cam when ap free per sta_info*/ + } else + rtw_clearstakey_cmd(padapter, psta, _FALSE); + } + } else if (MLME_IS_AP(padapter) || MLME_IS_MESH(padapter)) { +#ifdef CONFIG_AP_MODE +#ifndef SEC_DEFAULT_KEY_SEARCH + int cam_id = -1; + u8 *addr = adapter_mac_addr(padapter); + u8 bmc_id = rtw_iface_bcmc_id_get(padapter); + + while ((cam_id = rtw_camid_search(padapter, addr, -1, -1)) >= 0) { + RTW_PRINT("clear wep or group key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(addr), cam_id); + clear_cam_entry(padapter, cam_id); + /* clear cam_ctl.used bit for data BMC TX force camid in rtw_release_macid() */ + if (bmc_id == INVALID_SEC_MAC_CAM_ID || cam_id != bmc_id) + rtw_camid_free(padapter, cam_id); + } +#else + /* clear default key */ + int i, cam_id; + u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0}; + + for (i = 0; i < 4; i++) { + cam_id = rtw_camid_search(padapter, null_addr, i, -1); + if (cam_id >= 0) { + clear_cam_entry(padapter, cam_id); + rtw_camid_free(padapter, cam_id); + } + } + /* clear default key related key search setting */ + rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)_FALSE); +#endif +#endif /* CONFIG_AP_MODE */ + } + +#else /*NON CONFIG_CONCURRENT_MODE*/ + + invalidate_cam_all(padapter); +#endif +} + +#if defined(CONFIG_P2P) && defined(CONFIG_WFD) +void rtw_process_wfd_ie(_adapter *adapter, u8 *wfd_ie, u8 wfd_ielen, const char *tag) +{ + struct wifidirect_info *wdinfo = &adapter->wdinfo; + + u8 *attr_content; + u32 attr_contentlen = 0; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + return; + + RTW_INFO("[%s] Found WFD IE\n", tag); + attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen); + if (attr_content && attr_contentlen) { + wdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2); + RTW_INFO("[%s] Peer PORT NUM = %d\n", tag, wdinfo->wfd_info->peer_rtsp_ctrlport); + } +} + +void rtw_process_wfd_ies(_adapter *adapter, u8 *ies, u8 ies_len, const char *tag) +{ + u8 *wfd_ie; + u32 wfd_ielen; + + if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST)) + return; + + wfd_ie = rtw_get_wfd_ie(ies, ies_len, NULL, &wfd_ielen); + while (wfd_ie) { + rtw_process_wfd_ie(adapter, wfd_ie, wfd_ielen, tag); + wfd_ie = rtw_get_wfd_ie(wfd_ie + wfd_ielen, (ies + ies_len) - (wfd_ie + wfd_ielen), NULL, &wfd_ielen); + } +} +#endif /* defined(CONFIG_P2P) && defined(CONFIG_WFD) */ + +int WMM_param_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + /* struct registry_priv *pregpriv = &padapter->registrypriv; */ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pmlmepriv->qospriv.qos_option == 0) { + pmlmeinfo->WMM_enable = 0; + return _FALSE; + } + + if (_rtw_memcmp(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element))) + return _FALSE; + else + _rtw_memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element)); + pmlmeinfo->WMM_enable = 1; + return _TRUE; + +#if 0 + if (pregpriv->wifi_spec == 1) { + if (pmlmeinfo->WMM_enable == 1) { + /* todo: compare the parameter set count & decide wheher to update or not */ + return _FAIL; + } else { + pmlmeinfo->WMM_enable = 1; + _rtw_rtw_memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element)); + return _TRUE; + } + } else { + pmlmeinfo->WMM_enable = 0; + return _FAIL; + } +#endif + +} + +#ifdef CONFIG_RTW_TOKEN_BASED_XMIT +u8 rtw_is_tbtx_capabilty(u8 *p, u8 len){ + int i; + u8 tbtx_cap_ie[8] = {0x00, 0xe0, 0x4c, 0x01, 0x00, 0x00, 0x00, 0x00}; + + for (i = 0; i < len; i++) { + if (*(p + i) != tbtx_cap_ie[i]) + return _FALSE; + else + continue; + } + return _TRUE; +} +#endif + +void WMMOnAssocRsp(_adapter *padapter) +{ + u8 ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime; + u8 acm_mask; + u16 TXOP; + u32 acParm, i; + u32 edca[4], inx[4]; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct registry_priv *pregpriv = &padapter->registrypriv; +#ifdef CONFIG_WMMPS_STA + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct qos_priv *pqospriv = &pmlmepriv->qospriv; +#endif /* CONFIG_WMMPS_STA */ + + acm_mask = 0; + + if (is_supported_5g(pmlmeext->cur_wireless_mode) || + (pmlmeext->cur_wireless_mode & WIRELESS_11_24N)) + aSifsTime = 16; + else + aSifsTime = 10; + + if (pmlmeinfo->WMM_enable == 0) { + padapter->mlmepriv.acm_mask = 0; + + AIFS = aSifsTime + (2 * pmlmeinfo->slotTime); + + if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11A)) { + ECWMin = 4; + ECWMax = 10; + } else if (pmlmeext->cur_wireless_mode & WIRELESS_11B) { + ECWMin = 5; + ECWMax = 10; + } else { + ECWMin = 4; + ECWMax = 10; + } + + TXOP = 0; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm)); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm)); + + ECWMin = 2; + ECWMax = 3; + TXOP = 0x2f; + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + } else { + edca[0] = edca[1] = edca[2] = edca[3] = 0; + + for (i = 0; i < 4; i++) { + ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03; + ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01; + + /* AIFS = AIFSN * slot time + SIFS - r2t phy delay */ + AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime; + + ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f); + ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4; + TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit); + + acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16); + + switch (ACI) { + case 0x0: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm)); + acm_mask |= (ACM ? BIT(1) : 0); + edca[XMIT_BE_QUEUE] = acParm; + break; + + case 0x1: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm)); + /* acm_mask |= (ACM? BIT(0):0); */ + edca[XMIT_BK_QUEUE] = acParm; + break; + + case 0x2: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm)); + acm_mask |= (ACM ? BIT(2) : 0); + edca[XMIT_VI_QUEUE] = acParm; + break; + + case 0x3: + rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm)); + acm_mask |= (ACM ? BIT(3) : 0); + edca[XMIT_VO_QUEUE] = acParm; + break; + } + + RTW_INFO("WMM(%x): %x, %x\n", ACI, ACM, acParm); + } + + if (padapter->registrypriv.acm_method == 1) + rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask)); + else + padapter->mlmepriv.acm_mask = acm_mask; + + inx[0] = 0; + inx[1] = 1; + inx[2] = 2; + inx[3] = 3; + + if (pregpriv->wifi_spec == 1) { + u32 j, tmp, change_inx = _FALSE; + + /* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */ + for (i = 0; i < 4; i++) { + for (j = i + 1; j < 4; j++) { + /* compare CW and AIFS */ + if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) + change_inx = _TRUE; + else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) { + /* compare TXOP */ + if ((edca[j] >> 16) > (edca[i] >> 16)) + change_inx = _TRUE; + } + + if (change_inx) { + tmp = edca[i]; + edca[i] = edca[j]; + edca[j] = tmp; + + tmp = inx[i]; + inx[i] = inx[j]; + inx[j] = tmp; + + change_inx = _FALSE; + } + } + } + } + + for (i = 0; i < 4; i++) { + pxmitpriv->wmm_para_seq[i] = inx[i]; + RTW_INFO("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]); + } + +#ifdef CONFIG_WMMPS_STA + /* if AP supports UAPSD function, driver must set each uapsd TID to coresponding mac register 0x693 */ + if (pmlmeinfo->WMM_param.QoS_info & AP_SUPPORTED_UAPSD) { + pqospriv->uapsd_ap_supported = 1; + rtw_hal_set_hwreg(padapter, HW_VAR_UAPSD_TID, NULL); + } +#endif /* CONFIG_WMMPS_STA */ + } +} + +static void bwmode_update_check(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ +#ifdef CONFIG_80211N_HT + unsigned char new_bwmode; + unsigned char new_ch_offset; + struct HT_info_element *pHT_info; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct registry_priv *pregistrypriv = &padapter->registrypriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + u8 cbw40_enable = 0; + + if (!pIE) + return; + + if (phtpriv->ht_option == _FALSE) + return; + + if (pmlmeext->cur_bwmode >= CHANNEL_WIDTH_80) + return; + + if (pIE->Length > sizeof(struct HT_info_element)) + return; + + pHT_info = (struct HT_info_element *)pIE->data; + + if (hal_chk_bw_cap(padapter, BW_CAP_40M)) { + if (pmlmeext->cur_channel > 14) { + if (REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40)) + cbw40_enable = 1; + } else { + if (REGSTY_IS_BW_2G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40)) + cbw40_enable = 1; + } + } + + if ((pHT_info->infos[0] & BIT(2)) && cbw40_enable) { + new_bwmode = CHANNEL_WIDTH_40; + + switch (pHT_info->infos[0] & 0x3) { + case 1: + new_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + + case 3: + new_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + + default: + new_bwmode = CHANNEL_WIDTH_20; + new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + } else { + new_bwmode = CHANNEL_WIDTH_20; + new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + + if ((new_bwmode != pmlmeext->cur_bwmode || new_ch_offset != pmlmeext->cur_ch_offset) + && new_bwmode < pmlmeext->cur_bwmode + ) { + pmlmeinfo->bwmode_updated = _TRUE; + + pmlmeext->cur_bwmode = new_bwmode; + pmlmeext->cur_ch_offset = new_ch_offset; + + /* update HT info also */ + HT_info_handler(padapter, pIE); + } else + pmlmeinfo->bwmode_updated = _FALSE; + + + if (_TRUE == pmlmeinfo->bwmode_updated) { + struct sta_info *psta; + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + struct sta_priv *pstapriv = &padapter->stapriv; + + /* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */ + + + /* update ap's stainfo */ + psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress); + if (psta) { + struct ht_priv *phtpriv_sta = &psta->htpriv; + + if (phtpriv_sta->ht_option) { + /* bwmode */ + psta->cmn.bw_mode = pmlmeext->cur_bwmode; + phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset; + } else { + psta->cmn.bw_mode = CHANNEL_WIDTH_20; + phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + } + + rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta); + } + + /* pmlmeinfo->bwmode_updated = _FALSE; */ /* bwmode_updated done, reset it! */ + } +#endif /* CONFIG_80211N_HT */ +} + +#ifdef ROKU_PRIVATE +void Supported_rate_infra_ap(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + unsigned int i; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pIE == NULL) + return; + + for (i = 0 ; i < pIE->Length; i++) + pmlmeinfo->SupportedRates_infra_ap[i] = (pIE->data[i]); + +} + +void Extended_Supported_rate_infra_ap(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + unsigned int i, j; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pIE == NULL) + return; + + if (pIE->Length > 0) { + for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) { + if (pmlmeinfo->SupportedRates_infra_ap[i] == 0) + break; + } + for (j = 0; j < pIE->Length; j++) + pmlmeinfo->SupportedRates_infra_ap[i+j] = (pIE->data[j]); + } + +} + +void HT_get_ss_from_mcs_set(u8 *mcs_set, u8 *Rx_ss) +{ + u8 i, j; + u8 r_ss = 0, t_ss = 0; + + for (i = 0; i < 4; i++) { + if ((mcs_set[3-i] & 0xff) != 0x00) { + r_ss = 4-i; + break; + } + } + + *Rx_ss = r_ss; +} + +void HT_caps_handler_infra_ap(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + unsigned int i; + u8 cur_stbc_cap_infra_ap = 0; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv_infra_ap *phtpriv = &pmlmepriv->htpriv_infra_ap; + + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pIE == NULL) + return; + + pmlmeinfo->ht_vht_received |= BIT(0); + + /*copy MCS_SET*/ + for (i = 3; i < 19; i++) + phtpriv->MCS_set_infra_ap[i-3] = (pIE->data[i]); + + /*get number of stream from mcs set*/ + HT_get_ss_from_mcs_set(phtpriv->MCS_set_infra_ap, &phtpriv->Rx_ss_infra_ap); + + phtpriv->rx_highest_data_rate_infra_ap = le16_to_cpu(GET_HT_CAP_ELE_RX_HIGHEST_DATA_RATE(pIE->data)); + + phtpriv->ldpc_cap_infra_ap = GET_HT_CAP_ELE_LDPC_CAP(pIE->data); + + if (GET_HT_CAP_ELE_RX_STBC(pIE->data)) + SET_FLAG(cur_stbc_cap_infra_ap, STBC_HT_ENABLE_RX); + if (GET_HT_CAP_ELE_TX_STBC(pIE->data)) + SET_FLAG(cur_stbc_cap_infra_ap, STBC_HT_ENABLE_TX); + phtpriv->stbc_cap_infra_ap = cur_stbc_cap_infra_ap; + + /*store ap info SGI 20m 40m*/ + phtpriv->sgi_20m_infra_ap = GET_HT_CAP_ELE_SHORT_GI20M(pIE->data); + phtpriv->sgi_40m_infra_ap = GET_HT_CAP_ELE_SHORT_GI40M(pIE->data); + + /*store ap info for supported channel bandwidth*/ + phtpriv->channel_width_infra_ap = GET_HT_CAP_ELE_CHL_WIDTH(pIE->data); +} +#endif /* ROKU_PRIVATE */ + +void HT_caps_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ +#ifdef CONFIG_80211N_HT + unsigned int i; + u8 max_AMPDU_len, min_MPDU_spacing; + u8 cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0, rx_nss = 0; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; +#ifdef CONFIG_DISABLE_MCS13TO15 + struct registry_priv *pregistrypriv = &padapter->registrypriv; +#endif + + if (pIE == NULL) + return; + + if (phtpriv->ht_option == _FALSE) + return; + + pmlmeinfo->HT_caps_enable = 1; + + for (i = 0; i < (pIE->Length); i++) { + if (i != 2) { + /* Commented by Albert 2010/07/12 */ + /* Got the endian issue here. */ + pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]); + } else { + /* AMPDU Parameters field */ + + /* Get MIN of MAX AMPDU Length Exp */ + if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3)) + max_AMPDU_len = (pIE->data[i] & 0x3); + else + max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3); + + /* Get MAX of MIN MPDU Start Spacing */ + if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c)) + min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c); + else + min_MPDU_spacing = (pIE->data[i] & 0x1c); + + pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing; + } + } + + /* Commented by Albert 2010/07/12 */ + /* Have to handle the endian issue after copying. */ + /* HT_ext_caps didn't be used yet. */ + pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info); + pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps); + + /* update the MCS set */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i]; + + rx_nss = GET_HAL_RX_NSS(padapter); + + switch (rx_nss) { + case 1: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R); + break; + case 2: + #ifdef CONFIG_DISABLE_MCS13TO15 + if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1) + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF); + else + #endif + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R); + break; + case 3: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_3R); + break; + case 4: + set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_4R); + break; + default: + RTW_WARN("rf_type:%d or rx_nss:%u is not expected\n", GET_HAL_RFPATH(padapter), rx_nss); + } + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) { + /* Config STBC setting */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(pIE->data)) { + SET_FLAG(cur_stbc_cap, STBC_HT_ENABLE_TX); + RTW_INFO("Enable HT Tx STBC !\n"); + } + phtpriv->stbc_cap = cur_stbc_cap; + +#ifdef CONFIG_BEAMFORMING + /* Config Tx beamforming setting */ + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + /* Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pIE->data) << 6); + } + + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + /* Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pIE->data) << 4); + } + phtpriv->beamform_cap = cur_beamform_cap; + if (cur_beamform_cap) + RTW_INFO("AP HT Beamforming Cap = 0x%02X\n", cur_beamform_cap); +#endif /*CONFIG_BEAMFORMING*/ + } else { + /*WIFI_STATION_STATEorI_ADHOC_STATE or WIFI_ADHOC_MASTER_STATE*/ + /* Config LDPC Coding Capability */ + if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAP_ELE_LDPC_CAP(pIE->data)) { + SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX)); + RTW_INFO("Enable HT Tx LDPC!\n"); + } + phtpriv->ldpc_cap = cur_ldpc_cap; + + /* Config STBC setting */ + if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(pIE->data)) { + SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX)); + RTW_INFO("Enable HT Tx STBC!\n"); + } + phtpriv->stbc_cap = cur_stbc_cap; + +#ifdef CONFIG_BEAMFORMING +#ifdef RTW_BEAMFORMING_VERSION_2 + /* Config beamforming setting */ + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + /* Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pIE->data) << 6); + } + + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + /* Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pIE->data) << 4); + } +#else /* !RTW_BEAMFORMING_VERSION_2 */ + /* Config Tx beamforming setting */ + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE); + /* Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pIE->data) << 6); + } + + if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) && + GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pIE->data)) { + SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE); + /* Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/ + SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pIE->data) << 4); + } +#endif /* !RTW_BEAMFORMING_VERSION_2 */ + phtpriv->beamform_cap = cur_beamform_cap; + if (cur_beamform_cap) + RTW_INFO("Client HT Beamforming Cap = 0x%02X\n", cur_beamform_cap); +#endif /*CONFIG_BEAMFORMING*/ + } + +#endif /* CONFIG_80211N_HT */ +} + +void HT_info_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ +#ifdef CONFIG_80211N_HT + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct ht_priv *phtpriv = &pmlmepriv->htpriv; + + if (pIE == NULL) + return; + + if (phtpriv->ht_option == _FALSE) + return; + + + if (pIE->Length > sizeof(struct HT_info_element)) + return; + + pmlmeinfo->HT_info_enable = 1; + _rtw_memcpy(&(pmlmeinfo->HT_info), pIE->data, pIE->Length); +#endif /* CONFIG_80211N_HT */ + return; +} + +void HTOnAssocRsp(_adapter *padapter) +{ + unsigned char max_AMPDU_len; + unsigned char min_MPDU_spacing; + /* struct registry_priv *pregpriv = &padapter->registrypriv; */ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + RTW_INFO("%s\n", __FUNCTION__); + + if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) + pmlmeinfo->HT_enable = 1; + else { + pmlmeinfo->HT_enable = 0; + /* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */ + return; + } + + /* handle A-MPDU parameter field */ + /* + AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k + AMPDU_para [4:2]:Min MPDU Start Spacing + */ + max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03; + + min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2; + + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing)); +#ifdef CONFIG_80211N_HT + rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len)); +#endif /* CONFIG_80211N_HT */ +#if 0 /* move to rtw_update_ht_cap() */ + if ((pregpriv->bw_mode > 0) && + (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1)) && + (pmlmeinfo->HT_info.infos[0] & BIT(2))) { + /* switch to the 40M Hz mode accoring to the AP */ + pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; + switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) { + case EXTCHNL_OFFSET_UPPER: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; + break; + + case EXTCHNL_OFFSET_LOWER: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; + break; + + default: + pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; + break; + } + } +#endif + + /* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */ + +#if 0 /* move to rtw_update_ht_cap() */ + /* */ + /* Config SM Power Save setting */ + /* */ + pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2; + if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC) { +#if 0 + u8 i; + /* update the MCS rates */ + for (i = 0; i < 16; i++) + pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i]; +#endif + RTW_INFO("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __FUNCTION__); + } + + /* */ + /* Config current HT Protection mode. */ + /* */ + pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; +#endif + +} + +void ERP_IE_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE) +{ + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (pIE->Length > 1) + return; + + pmlmeinfo->ERP_enable = 1; + _rtw_memcpy(&(pmlmeinfo->ERP_IE), pIE->data, pIE->Length); +} + +void VCS_update(_adapter *padapter, struct sta_info *psta) +{ + struct registry_priv *pregpriv = &padapter->registrypriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + switch (pregpriv->vrtl_carrier_sense) { /* 0:off 1:on 2:auto */ + case 0: /* off */ + psta->rtsen = 0; + psta->cts2self = 0; + break; + + case 1: /* on */ + if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */ + psta->rtsen = 1; + psta->cts2self = 0; + } else { + psta->rtsen = 0; + psta->cts2self = 1; + } + break; + + case 2: /* auto */ + default: + if (((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) + /*||(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)*/ + ) { + if (pregpriv->vcs_type == 1) { + psta->rtsen = 1; + psta->cts2self = 0; + } else { + psta->rtsen = 0; + psta->cts2self = 1; + } + } else { + psta->rtsen = 0; + psta->cts2self = 0; + } + break; + } +} + +void update_ldpc_stbc_cap(struct sta_info *psta) +{ +#ifdef CONFIG_80211N_HT + +#ifdef CONFIG_80211AC_VHT + if (psta->vhtpriv.vht_option) { + if (TEST_FLAG(psta->vhtpriv.ldpc_cap, LDPC_VHT_ENABLE_TX)) + psta->cmn.ldpc_en = VHT_LDPC_EN; + else + psta->cmn.ldpc_en = 0; + + if (TEST_FLAG(psta->vhtpriv.stbc_cap, STBC_VHT_ENABLE_TX)) + psta->cmn.stbc_en = VHT_STBC_EN; + else + psta->cmn.stbc_en = 0; + } else +#endif /* CONFIG_80211AC_VHT */ + if (psta->htpriv.ht_option) { + if (TEST_FLAG(psta->htpriv.ldpc_cap, LDPC_HT_ENABLE_TX)) + psta->cmn.ldpc_en = HT_LDPC_EN; + else + psta->cmn.ldpc_en = 0; + + if (TEST_FLAG(psta->htpriv.stbc_cap, STBC_HT_ENABLE_TX)) + psta->cmn.stbc_en = HT_STBC_EN; + else + psta->cmn.stbc_en = 0; + } else { + psta->cmn.ldpc_en = 0; + psta->cmn.stbc_en = 0; + } + +#endif /* CONFIG_80211N_HT */ +} + +int check_ielen(u8 *start, uint len) +{ + int left = len; + u8 *pos = start; + u8 id, elen; + + while (left >= 2) { + id = *pos++; + elen = *pos++; + left -= 2; + + if (elen > left) { + RTW_INFO("IEEE 802.11 element parse failed (id=%d elen=%d left=%lu)\n", + id, elen, (unsigned long) left); + return _FALSE; + } + if ((id == WLAN_EID_VENDOR_SPECIFIC) && (elen < 3)) + return _FALSE; + + left -= elen; + pos += elen; + } + if (left) + return _FALSE; + + return _TRUE; +} + +int validate_beacon_len(u8 *pframe, u32 len) +{ + u8 ie_offset = _BEACON_IE_OFFSET_ + sizeof(struct rtw_ieee80211_hdr_3addr); + + if (len < ie_offset) { + RTW_INFO("%s: incorrect beacon length(%d)\n", __func__, len); + return _FALSE; + } + + if (check_ielen(pframe + ie_offset, len - ie_offset) == _FALSE) + return _FALSE; + + return _TRUE; +} + +#ifdef CONFIG_CHECK_SPECIFIC_IE_CONTENT +u8 support_rate_ranges[] = { + IEEE80211_CCK_RATE_1MB, + IEEE80211_CCK_RATE_2MB, + IEEE80211_CCK_RATE_5MB, + IEEE80211_CCK_RATE_11MB, + IEEE80211_OFDM_RATE_6MB, + IEEE80211_OFDM_RATE_9MB, + IEEE80211_OFDM_RATE_12MB, + IEEE80211_OFDM_RATE_18MB, + IEEE80211_PBCC_RATE_22MB, + IEEE80211_FREAK_RATE_22_5MB, + IEEE80211_OFDM_RATE_24MB, + IEEE80211_OFDM_RATE_36MB, + IEEE80211_OFDM_RATE_48MB, + IEEE80211_OFDM_RATE_54MB, +}; + +inline bool match_ranges(u16 EID, u32 value) +{ + int i; + int nr_range; + + switch (EID) { + case _EXT_SUPPORTEDRATES_IE_: + case _SUPPORTEDRATES_IE_: + nr_range = sizeof(support_rate_ranges)/sizeof(u8); + for (i = 0; i < nr_range; i++) { + /* clear bit7 before searching. */ + value &= ~BIT(7); + if (value == support_rate_ranges[i]) + return _TRUE; + } + break; + default: + break; + }; + return _FALSE; +} + +/* + * rtw_validate_value: validate the IE contain. + * + * Input : + * EID : Element ID + * p : IE buffer (without EID & length) + * len : IE length + * return: + * _TRUE : All Values are validated. + * _FALSE : At least one value is NOT validated. + */ +bool rtw_validate_value(u16 EID, u8 *p, u16 len) +{ + u8 rate; + u32 i, nr_val; + + switch (EID) { + case _EXT_SUPPORTEDRATES_IE_: + case _SUPPORTEDRATES_IE_: + nr_val = len; + for (i=0; iSsid.Ssid, snetwork->Ssid.SsidLength); +} + +/* + Get SSID if this ilegal frame(probe resp) comes from a hidden SSID AP. + Update the SSID to the corresponding pnetwork in scan queue. +*/ +void rtw_absorb_ssid_ifneed(_adapter *padapter, WLAN_BSSID_EX *bssid, u8 *pframe) +{ + struct wlan_network *scanned = NULL; + WLAN_BSSID_EX *snetwork; + u8 ie_offset, *p=NULL, *next_ie=NULL, *mac = get_addr2_ptr(pframe); + sint ssid_len_ori; + u32 remain_len = 0; + u8 backupIE[MAX_IE_SZ]; + u16 subtype = get_frame_sub_type(pframe); + _irqL irqL; + + if (subtype == WIFI_BEACON) { + bssid->Reserved[0] = BSS_TYPE_BCN; + ie_offset = _BEACON_IE_OFFSET_; + } else { + /* FIXME : more type */ + if (subtype == WIFI_PROBERSP) { + ie_offset = _PROBERSP_IE_OFFSET_; + bssid->Reserved[0] = BSS_TYPE_PROB_RSP; + } else if (subtype == WIFI_PROBEREQ) { + ie_offset = _PROBEREQ_IE_OFFSET_; + bssid->Reserved[0] = BSS_TYPE_PROB_REQ; + } else { + bssid->Reserved[0] = BSS_TYPE_UNDEF; + ie_offset = _FIXED_IE_LENGTH_; + } + } + + _enter_critical_bh(&padapter->mlmepriv.scanned_queue.lock, &irqL); + scanned = _rtw_find_network(&padapter->mlmepriv.scanned_queue, mac); + if (!scanned) { + _exit_critical_bh(&padapter->mlmepriv.scanned_queue.lock, &irqL); + return; + } + + snetwork = &(scanned->network); + /* scan queue records as Hidden SSID && Input frame is NOT Hidden SSID */ + if (hidden_ssid_ap(snetwork) && !hidden_ssid_ap(bssid)) { + p = rtw_get_ie(snetwork->IEs+ie_offset, _SSID_IE_, &ssid_len_ori, snetwork->IELength-ie_offset); + if (!p) { + _exit_critical_bh(&padapter->mlmepriv.scanned_queue.lock, &irqL); + return; + } + next_ie = p + 2 + ssid_len_ori; + remain_len = snetwork->IELength - (next_ie - snetwork->IEs); + scanned->network.Ssid.SsidLength = bssid->Ssid.SsidLength; + _rtw_memcpy(scanned->network.Ssid.Ssid, bssid->Ssid.Ssid, bssid->Ssid.SsidLength); + + //update pnetwork->ssid, pnetwork->ssidlen + _rtw_memcpy(backupIE, next_ie, remain_len); + *(p+1) = bssid->Ssid.SsidLength; + _rtw_memcpy(p+2, bssid->Ssid.Ssid, bssid->Ssid.SsidLength); + _rtw_memcpy(p+2+bssid->Ssid.SsidLength, backupIE, remain_len); + snetwork->IELength += bssid->Ssid.SsidLength; + } + _exit_critical_bh(&padapter->mlmepriv.scanned_queue.lock, &irqL); +} + +#ifdef DBG_RX_BCN +void rtw_debug_rx_bcn(_adapter *adapter, u8 *pframe, u32 packet_len) +{ + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *mlmeinfo = &(pmlmeext->mlmext_info); + u16 sn = ((struct rtw_ieee80211_hdr_3addr *)pframe)->seq_ctl >> 4; + u64 tsf, tsf_offset; + u8 dtim_cnt, dtim_period, tim_bmap, tim_pvbit; + + update_TSF(pmlmeext, pframe, packet_len); + tsf = pmlmeext->TSFValue; + tsf_offset = rtw_modular64(pmlmeext->TSFValue, (mlmeinfo->bcn_interval * 1024)); + + /*get TIM IE*/ + /*DTIM Count*/ + dtim_cnt = pmlmeext->tim[0]; + /*DTIM Period*/ + dtim_period = pmlmeext->tim[1]; + /*Bitmap*/ + tim_bmap = pmlmeext->tim[2]; + /*Partial VBitmap AID 0 ~ 7*/ + tim_pvbit = pmlmeext->tim[3]; + + RTW_INFO("[BCN] SN-%d, TSF-%lld(us), offset-%lld, bcn_interval-%d DTIM-%d[%d] bitmap-0x%02x-0x%02x\n", + sn, tsf, tsf_offset, mlmeinfo->bcn_interval, dtim_period, dtim_cnt, tim_bmap, tim_pvbit); +} +#endif + +/* + * rtw_get_bcn_keys: get beacon keys from recv frame + * + * TODO: + * WLAN_EID_COUNTRY + * WLAN_EID_ERP_INFO + * WLAN_EID_CHANNEL_SWITCH + * WLAN_EID_PWR_CONSTRAINT + */ +int _rtw_get_bcn_keys(u8 *cap_info, u32 buf_len, u8 def_ch, ADAPTER *adapter + , struct beacon_keys *recv_beacon) +{ + int left; + u16 capability; + unsigned char *pos; + struct rtw_ieee802_11_elems elems; + + _rtw_memset(recv_beacon, 0, sizeof(*recv_beacon)); + + /* checking capabilities */ + capability = le16_to_cpu(*(unsigned short *)(cap_info)); + + /* checking IEs */ + left = buf_len - 2; + pos = cap_info + 2; + if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed) + return _FALSE; + + if (elems.ht_capabilities) { + if (elems.ht_capabilities_len != 26) + return _FALSE; + } + + if (elems.ht_operation) { + if (elems.ht_operation_len != 22) + return _FALSE; + } + + if (elems.vht_capabilities) { + if (elems.vht_capabilities_len != 12) + return _FALSE; + } + + if (elems.vht_operation) { + if (elems.vht_operation_len != 5) + return _FALSE; + } + + if (rtw_ies_get_supported_rate(pos, left, recv_beacon->rate_set, &recv_beacon->rate_num) == _FAIL) + return _FALSE; + + if (cckratesonly_included(recv_beacon->rate_set, recv_beacon->rate_num) == _TRUE) + recv_beacon->proto_cap |= PROTO_CAP_11B; + else if (cckrates_included(recv_beacon->rate_set, recv_beacon->rate_num) == _TRUE) + recv_beacon->proto_cap |= PROTO_CAP_11B | PROTO_CAP_11G; + else + recv_beacon->proto_cap |= PROTO_CAP_11G; + + if (elems.ht_capabilities && elems.ht_operation) + recv_beacon->proto_cap |= PROTO_CAP_11N; + + if (elems.vht_capabilities && elems.vht_operation) + recv_beacon->proto_cap |= PROTO_CAP_11AC; + + /* check bw and channel offset */ + rtw_ies_get_chbw(pos, left, &recv_beacon->ch, &recv_beacon->bw, &recv_beacon->offset, 1, 1); + if (!recv_beacon->ch) + recv_beacon->ch = def_ch; + + /* checking SSID */ + if (elems.ssid) { + if (elems.ssid_len > sizeof(recv_beacon->ssid)) + return _FALSE; + + _rtw_memcpy(recv_beacon->ssid, elems.ssid, elems.ssid_len); + recv_beacon->ssid_len = elems.ssid_len; + } + + /* checking RSN first */ + if (elems.rsn_ie && elems.rsn_ie_len) { + recv_beacon->encryp_protocol = ENCRYP_PROTOCOL_WPA2; + rtw_parse_wpa2_ie(elems.rsn_ie - 2, elems.rsn_ie_len + 2, + &recv_beacon->group_cipher, &recv_beacon->pairwise_cipher, + NULL, &recv_beacon->akm, NULL, NULL); + } + /* checking WPA secon */ + else if (elems.wpa_ie && elems.wpa_ie_len) { + recv_beacon->encryp_protocol = ENCRYP_PROTOCOL_WPA; + rtw_parse_wpa_ie(elems.wpa_ie - 2, elems.wpa_ie_len + 2, + &recv_beacon->group_cipher, &recv_beacon->pairwise_cipher, + &recv_beacon->akm); + } else if (capability & BIT(4)) + recv_beacon->encryp_protocol = ENCRYP_PROTOCOL_WEP; + + if (adapter) { + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + + if (elems.tim && elems.tim_len) { + #ifdef DBG_RX_BCN + _rtw_memcpy(pmlmeext->tim, elems.tim, 4); + #endif + pmlmeext->dtim = elems.tim[1]; + } + + /* checking RTW TBTX */ + #ifdef CONFIG_RTW_TOKEN_BASED_XMIT + if (elems.tbtx_cap && elems.tbtx_cap_len) { + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (rtw_is_tbtx_capabilty(elems.tbtx_cap, elems.tbtx_cap_len)) + RTW_DBG("AP support TBTX\n"); + } + #endif + } + + return _TRUE; +} + +int rtw_get_bcn_keys(_adapter *adapter, u8 *whdr, u32 flen, struct beacon_keys *bcn_keys) +{ + return _rtw_get_bcn_keys( + whdr + WLAN_HDR_A3_LEN + 10 + , flen - WLAN_HDR_A3_LEN - 10 + , adapter->mlmeextpriv.cur_channel, adapter + , bcn_keys); +} + +int rtw_get_bcn_keys_from_bss(WLAN_BSSID_EX *bss, struct beacon_keys *bcn_keys) +{ + return _rtw_get_bcn_keys( + bss->IEs + 10 + , bss->IELength - 10 + , bss->Configuration.DSConfig, NULL + , bcn_keys); +} + +int rtw_update_bcn_keys_of_network(struct wlan_network *network) +{ + network->bcn_keys_valid = rtw_get_bcn_keys_from_bss(&network->network, &network->bcn_keys); + return network->bcn_keys_valid; +} + +void rtw_dump_bcn_keys(void *sel, struct beacon_keys *recv_beacon) +{ +#if defined(CONFIG_RTW_DEBUG) || defined(CONFIG_PROC_DEBUG) + u8 ssid[IW_ESSID_MAX_SIZE + 1]; + + _rtw_memcpy(ssid, recv_beacon->ssid, recv_beacon->ssid_len); + ssid[recv_beacon->ssid_len] = '\0'; + + RTW_PRINT_SEL(sel, "ssid = %s (len = %u)\n", ssid, recv_beacon->ssid_len); + RTW_PRINT_SEL(sel, "ch = %u,%u,%u\n" + , recv_beacon->ch, recv_beacon->bw, recv_beacon->offset); + RTW_PRINT_SEL(sel, "proto_cap = 0x%02x\n", recv_beacon->proto_cap); + RTW_MAP_DUMP_SEL(sel, "rate_set = " + , recv_beacon->rate_set, recv_beacon->rate_num); + RTW_PRINT_SEL(sel, "sec = %d, group = 0x%x, pair = 0x%x, akm = 0x%08x\n" + , recv_beacon->encryp_protocol, recv_beacon->group_cipher + , recv_beacon->pairwise_cipher, recv_beacon->akm); +#endif +} + +void rtw_bcn_key_err_fix(struct beacon_keys *cur, struct beacon_keys *recv) +{ + if ((recv->ch == cur->ch) && (recv->bw == cur->bw) && (recv->bw > CHANNEL_WIDTH_20)) { + if ((recv->offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) + && (cur->offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE)) { + RTW_DBG("recv_bcn offset = %d is invalid, try to use cur_bcn offset = %d to replace it !\n", recv->offset, cur->offset); + recv->offset = cur->offset; + } + } +} + +bool rtw_bcn_key_compare(struct beacon_keys *cur, struct beacon_keys *recv) +{ +#define BCNKEY_VERIFY_PROTO_CAP 0 +#define BCNKEY_VERIFY_WHOLE_RATE_SET 0 + + struct beacon_keys tmp; + bool ret = _FALSE; + + if (!rtw_is_chbw_grouped(cur->ch, cur->bw, cur->offset + , recv->ch, recv->bw, recv->offset)) + goto exit; + + _rtw_memcpy(&tmp, cur, sizeof(tmp)); + + /* check fields excluding below */ + tmp.ch = recv->ch; + tmp.bw = recv->bw; + tmp.offset = recv->offset; + if (!BCNKEY_VERIFY_PROTO_CAP) + tmp.proto_cap = recv->proto_cap; + if (!BCNKEY_VERIFY_WHOLE_RATE_SET) { + tmp.rate_num = recv->rate_num; + _rtw_memcpy(tmp.rate_set, recv->rate_set, 12); + } + + if (_rtw_memcmp(&tmp, recv, sizeof(*recv)) == _FALSE) + goto exit; + + ret = _TRUE; + +exit: + return ret; +} + +int rtw_check_bcn_info(ADAPTER *Adapter, u8 *pframe, u32 packet_len) +{ + u8 *pbssid = GetAddr3Ptr(pframe); + struct mlme_priv *pmlmepriv = &Adapter->mlmepriv; + struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network); + struct beacon_keys *cur_beacon = &pmlmepriv->cur_beacon_keys; + struct beacon_keys recv_beacon; + int ret = 0; + u8 ifbmp_m = rtw_mi_get_ap_mesh_ifbmp(Adapter); + u8 ifbmp_s = rtw_mi_get_ld_sta_ifbmp(Adapter); + struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter); + _adapter *pri_adapter = dvobj_get_primary_adapter(dvobj); + struct mlme_ext_priv *pmlmeext = &pri_adapter->mlmeextpriv; + + if (is_client_associated_to_ap(Adapter) == _FALSE) + goto exit_success; + + if (rtw_get_bcn_keys(Adapter, pframe, packet_len, &recv_beacon) == _FALSE) + goto exit_success; /* parsing failed => broken IE */ + +#ifdef DBG_RX_BCN + rtw_debug_rx_bcn(Adapter, pframe, packet_len); +#endif + + /* hidden ssid, replace with current beacon ssid directly */ + if (is_hidden_ssid(recv_beacon.ssid, recv_beacon.ssid_len)) { + _rtw_memcpy(recv_beacon.ssid, cur_beacon->ssid, cur_beacon->ssid_len); + recv_beacon.ssid_len = cur_beacon->ssid_len; + } + + if (check_fwstate(pmlmepriv, WIFI_CSA_UPDATE_BEACON)) { + u8 u_ch, u_offset, u_bw; + struct sta_info *psta = NULL; + _rtw_memcpy(cur_beacon, &recv_beacon, sizeof(recv_beacon)); + clr_fwstate(pmlmepriv, WIFI_CSA_UPDATE_BEACON); + rtw_mi_get_ch_setting_union(Adapter, &u_ch, &u_bw, &u_offset); + + /* RTW_INFO("u_ch=%d, u_bw=%d, u_offset=%d \n", u_ch, u_bw, u_offset); + RTW_INFO("recv_beacon.ch=%d, recv_beacon.bw=%d, recv_beacon.offset=%d \n", recv_beacon.ch, recv_beacon.bw, recv_beacon.offset); */ + /* rtw_dump_bcn_keys(RTW_DBGDUMP, &recv_beacon); */ + + /* RTW_INFO("_cancel_timer_async csa_timer\n"); */ + _cancel_timer_async(&pmlmeext->csa_timer); + + /* beacon bw/offset is different from CSA IE */ + if((recv_beacon.bw > u_bw) || + ((recv_beacon.offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE) && ((u_offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE)) + && (recv_beacon.offset != u_offset))) { + + /* update ch, bw, offset for all asoc STA ifaces */ + if (ifbmp_s) { + _adapter *iface; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if (!iface || !(ifbmp_s & BIT(iface->iface_id))) + continue; + + iface->mlmeextpriv.cur_channel = recv_beacon.ch; + iface->mlmeextpriv.cur_bwmode = recv_beacon.bw; + iface->mlmeextpriv.cur_ch_offset = recv_beacon.offset; + iface->mlmepriv.cur_network.network.Configuration.DSConfig = recv_beacon.ch; + } + } + +#ifdef CONFIG_AP_MODE + if (ifbmp_m) { + rtw_change_bss_chbw_cmd(dvobj_get_primary_adapter(dvobj), 0 + , ifbmp_m, 0, recv_beacon.ch, REQ_BW_ORI, REQ_OFFSET_NONE); + } else +#endif + { + #ifdef CONFIG_DFS_MASTER + rtw_dfs_rd_en_decision(dvobj_get_primary_adapter(dvobj), MLME_OPCH_SWITCH, 0); + #endif + rtw_set_chbw_cmd(Adapter, recv_beacon.ch, recv_beacon.bw, recv_beacon.offset, 0); + } + rtw_mi_get_ch_setting_union(Adapter, &u_ch, &u_bw, &u_offset); + + /* RTW_INFO("u_ch=%d, u_bw=%d, u_offset=%d \n", u_ch, u_bw, u_offset); */ + } else { + RTW_INFO("u_ch=%d, u_bw=%d, u_offset=%d, recv_beacon.ch=%d, recv_beacon.bw=%d, recv_beacon.offset=%d\n" + , u_ch, u_bw, u_offset, recv_beacon.ch, recv_beacon.bw, recv_beacon.offset); + } + + rtw_iqk_cmd(Adapter, 0); + psta = rtw_get_stainfo(&Adapter->stapriv, get_bssid(&Adapter->mlmepriv)); + if (psta) + rtw_dm_ra_mask_wk_cmd(Adapter, (u8 *)psta); + + } + +#ifdef CONFIG_BCN_CNT_CONFIRM_HDL + if (_rtw_memcmp(&recv_beacon, cur_beacon, sizeof(recv_beacon)) == _TRUE) + pmlmepriv->new_beacon_cnts = 0; + else if ((pmlmepriv->new_beacon_cnts == 0) || + _rtw_memcmp(&recv_beacon, &pmlmepriv->new_beacon_keys, sizeof(recv_beacon)) == _FALSE) { + RTW_DBG("%s: start new beacon (seq=%d)\n", __func__, GetSequence(pframe)); + + if (pmlmepriv->new_beacon_cnts == 0) { + RTW_ERR("%s: cur beacon key\n", __func__); + RTW_DBG_EXPR(rtw_dump_bcn_keys(RTW_DBGDUMP, cur_beacon)); + } + + RTW_DBG("%s: new beacon key\n", __func__); + RTW_DBG_EXPR(rtw_dump_bcn_keys(RTW_DBGDUMP, &recv_beacon)); + + _rtw_memcpy(&pmlmepriv->new_beacon_keys, &recv_beacon, sizeof(recv_beacon)); + pmlmepriv->new_beacon_cnts = 1; + } else { + RTW_DBG("%s: new beacon again (seq=%d)\n", __func__, GetSequence(pframe)); + pmlmepriv->new_beacon_cnts++; + } + + /* if counter >= max, it means beacon is changed really */ + if (pmlmepriv->new_beacon_cnts >= new_bcn_max) +#else + if (_rtw_memcmp(&recv_beacon, cur_beacon, sizeof(recv_beacon)) == _FALSE) +#endif + { + RTW_INFO(FUNC_ADPT_FMT" new beacon occur!!\n", FUNC_ADPT_ARG(Adapter)); + RTW_INFO(FUNC_ADPT_FMT" cur beacon key:\n", FUNC_ADPT_ARG(Adapter)); + rtw_dump_bcn_keys(RTW_DBGDUMP, cur_beacon); + RTW_INFO(FUNC_ADPT_FMT" new beacon key:\n", FUNC_ADPT_ARG(Adapter)); + rtw_dump_bcn_keys(RTW_DBGDUMP, &recv_beacon); + + rtw_bcn_key_err_fix(cur_beacon, &recv_beacon); + + if (rtw_bcn_key_compare(cur_beacon, &recv_beacon) == _FALSE) + goto exit; + + _rtw_memcpy(cur_beacon, &recv_beacon, sizeof(recv_beacon)); + #ifdef CONFIG_BCN_CNT_CONFIRM_HDL + pmlmepriv->new_beacon_cnts = 0; + #endif + } + +exit_success: + ret = 1; + +exit: + return ret; +} + +void update_beacon_info(_adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta) +{ + unsigned int i; + unsigned int len; + PNDIS_802_11_VARIABLE_IEs pIE; + +#ifdef CONFIG_TDLS + struct tdls_info *ptdlsinfo = &padapter->tdlsinfo; + u8 tdls_prohibited[] = { 0x00, 0x00, 0x00, 0x00, 0x10 }; /* bit(38): TDLS_prohibited */ +#endif /* CONFIG_TDLS */ + + len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN); + + for (i = 0; i < len;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + /* to update WMM paramter set while receiving beacon */ + if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6) && pIE->Length == WLAN_WMM_LEN) /* WMM */ + (WMM_param_handler(padapter, pIE)) ? report_wmm_edca_update(padapter) : 0; + + break; + + case _HT_EXTRA_INFO_IE_: /* HT info */ + /* HT_info_handler(padapter, pIE); */ + bwmode_update_check(padapter, pIE); + break; +#ifdef CONFIG_80211AC_VHT + case EID_OpModeNotification: + rtw_process_vht_op_mode_notify(padapter, pIE->data, psta); + break; +#endif /* CONFIG_80211AC_VHT */ + case _ERPINFO_IE_: + ERP_IE_handler(padapter, pIE); + VCS_update(padapter, psta); + break; + +#ifdef CONFIG_TDLS + case _EXT_CAP_IE_: + if (check_ap_tdls_prohibited(pIE->data, pIE->Length) == _TRUE) + ptdlsinfo->ap_prohibited = _TRUE; + if (check_ap_tdls_ch_switching_prohibited(pIE->data, pIE->Length) == _TRUE) + ptdlsinfo->ch_switch_prohibited = _TRUE; + break; +#endif /* CONFIG_TDLS */ + default: + break; + } + + i += (pIE->Length + 2); + } +} + +#if CONFIG_DFS +void process_csa_ie(_adapter *padapter, u8 *ies, uint ies_len) +{ + struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + unsigned int i, j, countdown; + PNDIS_802_11_VARIABLE_IEs pIE, sub_pie; + u8 ch = 0, csa_ch_offset = 0, csa_ch_width = 0, csa_ch_freq_seg0 = 0, csa_ch_freq_seg1 = 0, csa_switch_cnt = 0; + + /* TODO: compare with scheduling CSA */ + if (rfctl->csa_ch) + return; + + for (i = 0; i + 1 < ies_len;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(ies + i); + + switch (pIE->ElementID) { + case _CH_SWTICH_ANNOUNCE_: + ch = *(pIE->data + 1); + csa_switch_cnt = *(pIE->data + 2); + break; + case WLAN_EID_SECONDARY_CHANNEL_OFFSET: + csa_ch_offset = *(pIE->data); + break; + case WLAN_EID_WIDE_BANDWIDTH_CHANNEL_SWITCH: + csa_ch_width = *(pIE->data); + csa_ch_freq_seg0 = *(pIE->data+1); + csa_ch_freq_seg1 = *(pIE->data+2); + /* RTW_INFO("bw:%02x center_freq_0:%d center_freq_1:%d, ch=%d\n" + , csa_ch_width, csa_ch_freq_seg0, csa_ch_freq_seg1, ch); */ + break; + case WLAN_EID_CHANNEL_SWITCH_WRAPPER: + for(j=0; j + 1 < pIE->Length;) { + sub_pie = (PNDIS_802_11_VARIABLE_IEs)(ies + i + j + 2); + if(sub_pie->ElementID == WLAN_EID_WIDE_BANDWIDTH_CHANNEL_SWITCH) { + csa_ch_width = *(sub_pie->data); + csa_ch_freq_seg0 = *(sub_pie->data+1); + csa_ch_freq_seg1 = *(sub_pie->data+2); + /* RTW_INFO("2. sub_IE:%02x IE_length:%02x bw:%02x center_freq_0:%d center_freq_1:%d, ch=%d\n" + , sub_pie->ElementID, sub_pie->Length, csa_ch_width, csa_ch_freq_seg0, csa_ch_freq_seg1, ch); */ + } + j += (sub_pie->Length + 2); + } + + break; + default: + break; + } + + i += (pIE->Length + 2); + } + + if (ch != 0) { + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + _adapter *pri_adapter = dvobj_get_primary_adapter(dvobj); + + rfctl->csa_ch = ch; + rfctl->csa_switch_cnt = csa_switch_cnt; + rfctl->csa_ch_offset = csa_ch_offset; + rfctl->csa_ch_width = csa_ch_width; + rfctl->csa_ch_freq_seg0 = csa_ch_freq_seg0; + rfctl->csa_ch_freq_seg1 = csa_ch_freq_seg1; + + countdown = pmlmeinfo->network.Configuration.BeaconPeriod * (csa_switch_cnt+1); /* ms */ + RTW_INFO("csa: set countdown timer to %d ms\n", countdown); + _set_timer(&pri_adapter->mlmeextpriv.csa_timer, countdown); + + } +} +#endif /* CONFIG_DFS */ + +enum eap_type parsing_eapol_packet(_adapter *padapter, u8 *key_payload, struct sta_info *psta, u8 trx_type) +{ + struct security_priv *psecuritypriv = &(padapter->securitypriv); + struct ieee802_1x_hdr *hdr; + struct wpa_eapol_key *key; + u16 key_info, key_data_length; + char *trx_msg = trx_type ? "send" : "recv"; + enum eap_type eapol_type; + + hdr = (struct ieee802_1x_hdr *) key_payload; + + /* WPS - eapol start packet */ + if (hdr->type == 1 && hdr->length == 0) { + RTW_INFO("%s eapol start packet\n", trx_msg); + return EAPOL_START; + } + + if (hdr->type == 0) { /* WPS - eapol packet */ + RTW_INFO("%s eapol packet\n", trx_msg); + return EAPOL_PACKET; + } + + key = (struct wpa_eapol_key *) (hdr + 1); + key_info = be16_to_cpu(*((u16 *)(key->key_info))); + key_data_length = be16_to_cpu(*((u16 *)(key->key_data_length))); + + if (!(key_info & WPA_KEY_INFO_KEY_TYPE)) { /* WPA group key handshake */ + if (key_info & WPA_KEY_INFO_ACK) { + RTW_PRINT("%s eapol packet - WPA Group Key 1/2\n", trx_msg); + eapol_type = EAPOL_WPA_GROUP_KEY_1_2; + } else { + RTW_PRINT("%s eapol packet - WPA Group Key 2/2\n", trx_msg); + eapol_type = EAPOL_WPA_GROUP_KEY_2_2; + + /* WPA key-handshake has completed */ + if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK) + psta->state &= (~WIFI_UNDER_KEY_HANDSHAKE); + } + } else if (key_info & WPA_KEY_INFO_MIC) { + if (key_data_length == 0) { + RTW_PRINT("%s eapol packet 4/4\n", trx_msg); + eapol_type = EAPOL_4_4; + } else if (key_info & WPA_KEY_INFO_ACK) { + RTW_PRINT("%s eapol packet 3/4\n", trx_msg); + eapol_type = EAPOL_3_4; + } else { + RTW_PRINT("%s eapol packet 2/4\n", trx_msg); + eapol_type = EAPOL_2_4; + } + } else { + RTW_PRINT("%s eapol packet 1/4\n", trx_msg); + eapol_type = EAPOL_1_4; + } + + return eapol_type; +} + +unsigned int is_ap_in_tkip(_adapter *padapter) +{ + u32 i; + PNDIS_802_11_VARIABLE_IEs pIE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + + if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) && (_rtw_memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4))) + return _TRUE; + break; + + case _RSN_IE_2_: + if (_rtw_memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4)) + return _TRUE; + + default: + break; + } + + i += (pIE->Length + 2); + } + + return _FALSE; + } else + return _FALSE; + +} + +unsigned int should_forbid_n_rate(_adapter *padapter) +{ + u32 i; + PNDIS_802_11_VARIABLE_IEs pIE; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + WLAN_BSSID_EX *cur_network = &pmlmepriv->cur_network.network; + + if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < cur_network->IELength;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(cur_network->IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if (_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4) && + ((_rtw_memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP, 4)) || + (_rtw_memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP, 4)))) + return _FALSE; + break; + + case _RSN_IE_2_: + if ((_rtw_memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP, 4)) || + (_rtw_memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP, 4))) + return _FALSE; + + default: + break; + } + + i += (pIE->Length + 2); + } + + return _TRUE; + } else + return _FALSE; + +} + + +unsigned int is_ap_in_wep(_adapter *padapter) +{ + u32 i; + PNDIS_802_11_VARIABLE_IEs pIE; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + + if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY) { + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if (_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) + return _FALSE; + break; + + case _RSN_IE_2_: + return _FALSE; + + default: + break; + } + + i += (pIE->Length + 2); + } + + return _TRUE; + } else + return _FALSE; + +} + +int wifirate2_ratetbl_inx(unsigned char rate); +int wifirate2_ratetbl_inx(unsigned char rate) +{ + int inx = 0; + rate = rate & 0x7f; + + switch (rate) { + case 54*2: + inx = 11; + break; + + case 48*2: + inx = 10; + break; + + case 36*2: + inx = 9; + break; + + case 24*2: + inx = 8; + break; + + case 18*2: + inx = 7; + break; + + case 12*2: + inx = 6; + break; + + case 9*2: + inx = 5; + break; + + case 6*2: + inx = 4; + break; + + case 11*2: + inx = 3; + break; + case 11: + inx = 2; + break; + + case 2*2: + inx = 1; + break; + + case 1*2: + inx = 0; + break; + + } + return inx; +} + +unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz) +{ + unsigned int i, num_of_rate; + unsigned int mask = 0; + + num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz; + + for (i = 0; i < num_of_rate; i++) { + if ((*(ptn + i)) & 0x80) + mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i)); + } + return mask; +} + +unsigned int update_supported_rate(unsigned char *ptn, unsigned int ptn_sz) +{ + unsigned int i, num_of_rate; + unsigned int mask = 0; + + num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz; + + for (i = 0; i < num_of_rate; i++) + mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i)); + + return mask; +} + +int support_short_GI(_adapter *padapter, struct HT_caps_element *pHT_caps, u8 bwmode) +{ + unsigned char bit_offset; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + + if (!(pmlmeinfo->HT_enable)) + return _FAIL; + + bit_offset = (bwmode & CHANNEL_WIDTH_40) ? 6 : 5; + + if (pHT_caps->u.HT_cap_element.HT_caps_info & (0x1 << bit_offset)) + return _SUCCESS; + else + return _FAIL; +} + +unsigned char get_highest_rate_idx(u64 mask) +{ + int i; + unsigned char rate_idx = 0; + + for (i = 63; i >= 0; i--) { + if ((mask >> i) & 0x01) { + rate_idx = i; + break; + } + } + + return rate_idx; +} +unsigned char get_lowest_rate_idx_ex(u64 mask, int start_bit) +{ + int i; + unsigned char rate_idx = 0; + + for (i = start_bit; i < 64; i++) { + if ((mask >> i) & 0x01) { + rate_idx = i; + break; + } + } + + return rate_idx; +} + +void Update_RA_Entry(_adapter *padapter, struct sta_info *psta) +{ + rtw_hal_update_ra_mask(psta); +} + +void set_sta_rate(_adapter *padapter, struct sta_info *psta) +{ + /* rate adaptive */ + rtw_hal_update_ra_mask(psta); +} + +/* Update RRSR and Rate for USERATE */ +void update_tx_basic_rate(_adapter *padapter, u8 wirelessmode) +{ + NDIS_802_11_RATES_EX supported_rates; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + /* Added by Albert 2011/03/22 */ + /* In the P2P mode, the driver should not support the b mode. */ + /* So, the Tx packet shouldn't use the CCK rate */ + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; +#endif /* CONFIG_P2P */ + + _rtw_memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX); + + /* clear B mod if current channel is in 5G band, avoid tx cck rate in 5G band. */ + if (pmlmeext->cur_channel > 14) + wirelessmode &= ~(WIRELESS_11B); + + if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B)) + _rtw_memcpy(supported_rates, rtw_basic_rate_cck, 4); + else if (wirelessmode & WIRELESS_11B) + _rtw_memcpy(supported_rates, rtw_basic_rate_mix, 7); + else + _rtw_memcpy(supported_rates, rtw_basic_rate_ofdm, 3); + + if (wirelessmode & WIRELESS_11B) + update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB); + else + update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB); + + rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, supported_rates); +} + +unsigned char check_assoc_AP(u8 *pframe, uint len) +{ + unsigned int i; + PNDIS_802_11_VARIABLE_IEs pIE; + + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < len;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i); + + switch (pIE->ElementID) { + case _VENDOR_SPECIFIC_IE_: + if ((_rtw_memcmp(pIE->data, ARTHEROS_OUI1, 3)) || (_rtw_memcmp(pIE->data, ARTHEROS_OUI2, 3))) { + RTW_INFO("link to Artheros AP\n"); + return HT_IOT_PEER_ATHEROS; + } else if ((_rtw_memcmp(pIE->data, BROADCOM_OUI1, 3)) + || (_rtw_memcmp(pIE->data, BROADCOM_OUI2, 3)) + || (_rtw_memcmp(pIE->data, BROADCOM_OUI3, 3))) { + RTW_INFO("link to Broadcom AP\n"); + return HT_IOT_PEER_BROADCOM; + } else if (_rtw_memcmp(pIE->data, MARVELL_OUI, 3)) { + RTW_INFO("link to Marvell AP\n"); + return HT_IOT_PEER_MARVELL; + } else if (_rtw_memcmp(pIE->data, RALINK_OUI, 3)) { + RTW_INFO("link to Ralink AP\n"); + return HT_IOT_PEER_RALINK; + } else if (_rtw_memcmp(pIE->data, CISCO_OUI, 3)) { + RTW_INFO("link to Cisco AP\n"); + return HT_IOT_PEER_CISCO; + } else if (_rtw_memcmp(pIE->data, REALTEK_OUI, 3)) { + u32 Vender = HT_IOT_PEER_REALTEK; + + if (pIE->Length >= 5) { + if (pIE->data[4] == 1) { + /* if(pIE->data[5] & RT_HT_CAP_USE_LONG_PREAMBLE) */ + /* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_LONG_PREAMBLE; */ + + if (pIE->data[5] & RT_HT_CAP_USE_92SE) { + /* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_92SE; */ + Vender = HT_IOT_PEER_REALTEK_92SE; + } + } + + if (pIE->data[5] & RT_HT_CAP_USE_SOFTAP) + Vender = HT_IOT_PEER_REALTEK_SOFTAP; + + if (pIE->data[4] == 2) { + if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_BCUT) { + Vender = HT_IOT_PEER_REALTEK_JAGUAR_BCUTAP; + RTW_INFO("link to Realtek JAGUAR_BCUTAP\n"); + } + if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_CCUT) { + Vender = HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP; + RTW_INFO("link to Realtek JAGUAR_CCUTAP\n"); + } + } + } + + RTW_INFO("link to Realtek AP\n"); + return Vender; + } else if (_rtw_memcmp(pIE->data, AIRGOCAP_OUI, 3)) { + RTW_INFO("link to Airgo Cap\n"); + return HT_IOT_PEER_AIRGO; + } else + break; + + default: + break; + } + + i += (pIE->Length + 2); + } + + RTW_INFO("link to new AP\n"); + return HT_IOT_PEER_UNKNOWN; +} + +void get_assoc_AP_Vendor(char *vendor, u8 assoc_AP_vendor) +{ + switch (assoc_AP_vendor) { + + case HT_IOT_PEER_UNKNOWN: + sprintf(vendor, "%s", "unknown"); + break; + + case HT_IOT_PEER_REALTEK: + case HT_IOT_PEER_REALTEK_92SE: + case HT_IOT_PEER_REALTEK_SOFTAP: + case HT_IOT_PEER_REALTEK_JAGUAR_BCUTAP: + case HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP: + + sprintf(vendor, "%s", "Realtek"); + break; + + case HT_IOT_PEER_BROADCOM: + sprintf(vendor, "%s", "Broadcom"); + break; + + case HT_IOT_PEER_MARVELL: + sprintf(vendor, "%s", "Marvell"); + break; + + case HT_IOT_PEER_RALINK: + sprintf(vendor, "%s", "Ralink"); + break; + + case HT_IOT_PEER_CISCO: + sprintf(vendor, "%s", "Cisco"); + break; + + case HT_IOT_PEER_AIRGO: + sprintf(vendor, "%s", "Airgo"); + break; + + case HT_IOT_PEER_ATHEROS: + sprintf(vendor, "%s", "Atheros"); + break; + + default: + sprintf(vendor, "%s", "unkown"); + break; + } + +} +#ifdef CONFIG_RTS_FULL_BW +void rtw_parse_sta_vendor_ie_8812(_adapter *adapter, struct sta_info *sta, u8 *tlv_ies, u16 tlv_ies_len) +{ + unsigned char REALTEK_OUI[] = {0x00,0xe0, 0x4c}; + u8 *p; + + p = rtw_get_ie_ex(tlv_ies, tlv_ies_len, WLAN_EID_VENDOR_SPECIFIC, REALTEK_OUI, 3, NULL, NULL); + if (!p) + goto exit; + else { + if(*(p+1) > 6 ) { + + if(*(p+6) != 2) + goto exit; + + if(*(p+8) == RT_HT_CAP_USE_JAGUAR_BCUT) + sta->vendor_8812 = TRUE; + else if (*(p+8) == RT_HT_CAP_USE_JAGUAR_CCUT) + sta->vendor_8812 = TRUE; + } + } +exit: + return; +} +#endif/*CONFIG_RTS_FULL_BW*/ + +#ifdef CONFIG_80211AC_VHT +void get_vht_bf_cap(u8 *pframe, uint len, struct vht_bf_cap *bf_cap) +{ + unsigned int i; + PNDIS_802_11_VARIABLE_IEs pIE; + + for (i = sizeof(NDIS_802_11_FIXED_IEs); i < len;) { + pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i); + + switch (pIE->ElementID) { + + case EID_VHTCapability: + bf_cap->is_mu_bfer = GET_VHT_CAPABILITY_ELE_MU_BFER(pIE->data); + bf_cap->su_sound_dim = GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pIE->data); + break; + default: + break; + } + i += (pIE->Length + 2); + } +} +#endif + +void update_capinfo(PADAPTER Adapter, u16 updateCap) +{ + struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + BOOLEAN ShortPreamble; + + /* Check preamble mode, 2005.01.06, by rcnjko. */ + /* Mark to update preamble value forever, 2008.03.18 by lanhsin */ + /* if( pMgntInfo->RegPreambleMode == PREAMBLE_AUTO ) */ + { + + if (updateCap & cShortPreamble) { + /* Short Preamble */ + if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) { /* PREAMBLE_LONG or PREAMBLE_AUTO */ + ShortPreamble = _TRUE; + pmlmeinfo->preamble_mode = PREAMBLE_SHORT; + rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble); + } + } else { + /* Long Preamble */ + if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) { /* PREAMBLE_SHORT or PREAMBLE_AUTO */ + ShortPreamble = _FALSE; + pmlmeinfo->preamble_mode = PREAMBLE_LONG; + rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble); + } + } + } + + if (updateCap & cIBSS) { + /* Filen: See 802.11-2007 p.91 */ + pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME; + } else { + /* Filen: See 802.11-2007 p.90 */ + if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N | WIRELESS_11A | WIRELESS_11_5N | WIRELESS_11AC)) + pmlmeinfo->slotTime = SHORT_SLOT_TIME; + else if (pmlmeext->cur_wireless_mode & (WIRELESS_11G)) { + if ((updateCap & cShortSlotTime) /* && (!(pMgntInfo->pHTInfo->RT2RT_HT_Mode & RT_HT_CAP_USE_LONG_PREAMBLE)) */) { + /* Short Slot Time */ + pmlmeinfo->slotTime = SHORT_SLOT_TIME; + } else { + /* Long Slot Time */ + pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME; + } + } else { + /* B Mode */ + pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME; + } + } + + rtw_hal_set_hwreg(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime); + +} + +/* +* set adapter.mlmeextpriv.mlmext_info.HT_enable +* set adapter.mlmeextpriv.cur_wireless_mode +* set SIFS register +* set mgmt tx rate +*/ +void update_wireless_mode(_adapter *padapter) +{ + int ratelen, network_type = 0; + u32 SIFS_Timer; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + unsigned char *rate = cur_network->SupportedRates; +#ifdef CONFIG_P2P + struct wifidirect_info *pwdinfo = &(padapter->wdinfo); +#endif /* CONFIG_P2P */ + + ratelen = rtw_get_rateset_len(cur_network->SupportedRates); + + if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) + pmlmeinfo->HT_enable = 1; + + if (pmlmeext->cur_channel > 14) { + if (pmlmeinfo->VHT_enable) + network_type = WIRELESS_11AC; + else if (pmlmeinfo->HT_enable) + network_type = WIRELESS_11_5N; + + network_type |= WIRELESS_11A; + } else { + if (pmlmeinfo->VHT_enable) + network_type = WIRELESS_11AC; + else if (pmlmeinfo->HT_enable) + network_type = WIRELESS_11_24N; + + if ((cckratesonly_included(rate, ratelen)) == _TRUE) + network_type |= WIRELESS_11B; + else if ((cckrates_included(rate, ratelen)) == _TRUE) + network_type |= WIRELESS_11BG; + else + network_type |= WIRELESS_11G; + } + + pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode; + /* RTW_INFO("network_type=%02x, padapter->registrypriv.wireless_mode=%02x\n", network_type, padapter->registrypriv.wireless_mode); */ + +#ifndef RTW_HALMAC + /* HALMAC IC do not set HW_VAR_RESP_SIFS here */ +#if 0 + if ((pmlmeext->cur_wireless_mode == WIRELESS_11G) || + (pmlmeext->cur_wireless_mode == WIRELESS_11BG)) /* WIRELESS_MODE_G) */ + SIFS_Timer = 0x0a0a;/* CCK */ + else + SIFS_Timer = 0x0e0e;/* pHalData->SifsTime; //OFDM */ +#endif + + SIFS_Timer = 0x0a0a0808; /* 0x0808->for CCK, 0x0a0a->for OFDM + * change this value if having IOT issues. */ + + rtw_hal_set_hwreg(padapter, HW_VAR_RESP_SIFS, (u8 *)&SIFS_Timer); +#endif + + rtw_hal_set_hwreg(padapter, HW_VAR_WIRELESS_MODE, (u8 *)&(pmlmeext->cur_wireless_mode)); + + if ((pmlmeext->cur_wireless_mode & WIRELESS_11B) + #ifdef CONFIG_P2P + && (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) + #ifdef CONFIG_IOCTL_CFG80211 + || !rtw_cfg80211_iface_has_p2p_group_cap(padapter) + #endif + ) + #endif + ) + update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB); + else + update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB); +} + +void fire_write_MAC_cmd(_adapter *padapter, unsigned int addr, unsigned int value); +void fire_write_MAC_cmd(_adapter *padapter, unsigned int addr, unsigned int value) +{ +#if 0 + struct cmd_obj *ph2c; + struct reg_rw_parm *pwriteMacPara; + struct cmd_priv *pcmdpriv = &(padapter->cmdpriv); + + ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj)); + if (ph2c == NULL) + return; + + pwriteMacPara = (struct reg_rw_parm *)rtw_malloc(sizeof(struct reg_rw_parm)); + if (pwriteMacPara == NULL) { + rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj)); + return; + } + + pwriteMacPara->rw = 1; + pwriteMacPara->addr = addr; + pwriteMacPara->value = value; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pwriteMacPara, GEN_CMD_CODE(_Write_MACREG)); + rtw_enqueue_cmd(pcmdpriv, ph2c); +#endif +} + +void update_sta_basic_rate(struct sta_info *psta, u8 wireless_mode) +{ + if (IsSupportedTxCCK(wireless_mode)) { + /* Only B, B/G, and B/G/N AP could use CCK rate */ + _rtw_memcpy(psta->bssrateset, rtw_basic_rate_cck, 4); + psta->bssratelen = 4; + } else { + _rtw_memcpy(psta->bssrateset, rtw_basic_rate_ofdm, 3); + psta->bssratelen = 3; + } +} + +int rtw_ies_get_supported_rate(u8 *ies, uint ies_len, u8 *rate_set, u8 *rate_num) +{ + u8 *ie, *p; + unsigned int ie_len; + int i, j; + + struct support_rate_handler support_rate_tbl[] = { + {IEEE80211_CCK_RATE_1MB, _FALSE, _FALSE}, + {IEEE80211_CCK_RATE_2MB, _FALSE, _FALSE}, + {IEEE80211_CCK_RATE_5MB, _FALSE, _FALSE}, + {IEEE80211_CCK_RATE_11MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_6MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_9MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_12MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_18MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_24MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_36MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_48MB, _FALSE, _FALSE}, + {IEEE80211_OFDM_RATE_54MB, _FALSE, _FALSE}, + }; + + if (!rate_set || !rate_num) + return _FALSE; + + *rate_num = 0; + ie = rtw_get_ie(ies, _SUPPORTEDRATES_IE_, &ie_len, ies_len); + if (ie == NULL) + goto ext_rate; + + /* get valid supported rates */ + for (i = 0; i < 12; i++) { + p = ie + 2; + for (j = 0; j < ie_len; j++) { + if ((*p & ~BIT(7)) == support_rate_tbl[i].rate){ + support_rate_tbl[i].existence = _TRUE; + if ((*p) & BIT(7)) + support_rate_tbl[i].basic = _TRUE; + } + p++; + } + } + +ext_rate: + ie = rtw_get_ie(ies, _EXT_SUPPORTEDRATES_IE_, &ie_len, ies_len); + if (ie) { + /* get valid extended supported rates */ + for (i = 0; i < 12; i++) { + p = ie + 2; + for (j = 0; j < ie_len; j++) { + if ((*p & ~BIT(7)) == support_rate_tbl[i].rate){ + support_rate_tbl[i].existence = _TRUE; + if ((*p) & BIT(7)) + support_rate_tbl[i].basic = _TRUE; + } + p++; + } + } + } + + for (i = 0; i < 12; i++){ + if (support_rate_tbl[i].existence){ + if (support_rate_tbl[i].basic) + rate_set[*rate_num] = support_rate_tbl[i].rate | IEEE80211_BASIC_RATE_MASK; + else + rate_set[*rate_num] = support_rate_tbl[i].rate; + *rate_num += 1; + } + } + + if (*rate_num == 0) + return _FAIL; + + if (0) { + int i; + + for (i = 0; i < *rate_num; i++) + RTW_INFO("rate:0x%02x\n", *(rate_set + i)); + } + + return _SUCCESS; +} + +void process_addba_req(_adapter *padapter, u8 *paddba_req, u8 *addr) +{ + struct sta_info *psta; + u16 tid, start_seq, param; + struct sta_priv *pstapriv = &padapter->stapriv; + struct ADDBA_request *preq = (struct ADDBA_request *)paddba_req; + u8 size, accept = _FALSE; + + psta = rtw_get_stainfo(pstapriv, addr); + if (!psta) + goto exit; + + start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4; + + param = le16_to_cpu(preq->BA_para_set); + tid = (param >> 2) & 0x0f; + + + accept = rtw_rx_ampdu_is_accept(padapter); + if (padapter->fix_rx_ampdu_size != RX_AMPDU_SIZE_INVALID) + size = padapter->fix_rx_ampdu_size; + else { + size = rtw_rx_ampdu_size(padapter); + size = rtw_min(size, rx_ampdu_size_sta_limit(padapter, psta)); + } + + if (accept == _TRUE) + rtw_addbarsp_cmd(padapter, addr, tid, 0, size, start_seq); + else + rtw_addbarsp_cmd(padapter, addr, tid, 37, size, start_seq); /* reject ADDBA Req */ + +exit: + return; +} + +void rtw_process_bar_frame(_adapter *padapter, union recv_frame *precv_frame) +{ + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *pframe = precv_frame->u.hdr.rx_data; + struct sta_info *psta = NULL; + struct recv_reorder_ctrl *preorder_ctrl = NULL; + u8 tid = 0; + u16 start_seq=0; + + psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe)); + if (psta == NULL) + goto exit; + + tid = ((cpu_to_le16((*(u16 *)(pframe + 16))) & 0xf000) >> 12); + preorder_ctrl = &psta->recvreorder_ctrl[tid]; + start_seq = ((cpu_to_le16(*(u16 *)(pframe + 18))) >> 4); + preorder_ctrl->indicate_seq = start_seq; + + /* for Debug use */ + if (0) + RTW_INFO(FUNC_ADPT_FMT" tid=%d, start_seq=%d\n", FUNC_ADPT_ARG(padapter), tid, start_seq); + +exit: + return; +} + +void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len) +{ + u8 *pIE; + u32 *pbuf; + + pIE = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + pbuf = (u32 *)pIE; + + pmlmeext->TSFValue = le32_to_cpu(*(pbuf + 1)); + + pmlmeext->TSFValue = pmlmeext->TSFValue << 32; + + pmlmeext->TSFValue |= le32_to_cpu(*pbuf); +} + +void correct_TSF(_adapter *padapter, u8 mlme_state) +{ + u8 m_state = mlme_state; + + rtw_hal_set_hwreg(padapter, HW_VAR_CORRECT_TSF, (u8 *)&m_state); +} + +#ifdef CONFIG_BCN_RECV_TIME +/* calculate beacon receiving time + 1.RxBCNTime(CCK_1M) = [192us(preamble)] + [length of beacon(byte)*8us] + [10us] + 2.RxBCNTime(OFDM_6M) = [8us(S) + 8us(L) + 4us(L-SIG)] + [(length of beacon(byte)/3 + 1] *4us] + [10us] +*/ +inline u16 _rx_bcn_time_calculate(uint bcn_len, u8 data_rate) +{ + u16 rx_bcn_time = 0;/*us*/ + + if (data_rate == DESC_RATE1M) + rx_bcn_time = 192 + bcn_len * 8 + 10; + else if(data_rate == DESC_RATE6M) + rx_bcn_time = 8 + 8 + 4 + (bcn_len /3 + 1) * 4 + 10; +/* + else + RTW_ERR("%s invalid data rate(0x%02x)\n", __func__, data_rate); +*/ + return rx_bcn_time; +} +void rtw_rx_bcn_time_update(_adapter *adapter, uint bcn_len, u8 data_rate) +{ + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + + pmlmeext->bcn_rx_time = _rx_bcn_time_calculate(bcn_len, data_rate); +} +#endif + +void beacon_timing_control(_adapter *padapter) +{ + rtw_hal_bcn_related_reg_setting(padapter); +} + +inline bool _rtw_macid_ctl_chk_cap(_adapter *adapter, u8 cap) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct macid_ctl_t *macid_ctl = &dvobj->macid_ctl; + + if (macid_ctl->macid_cap & cap) + return _TRUE; + return _FALSE; +} + +void dump_macid_map(void *sel, struct macid_bmp *map, u8 max_num) +{ + RTW_PRINT_SEL(sel, "0x%08x\n", map->m0); +#if (MACID_NUM_SW_LIMIT > 32) + if (max_num && max_num > 32) + RTW_PRINT_SEL(sel, "0x%08x\n", map->m1); +#endif +#if (MACID_NUM_SW_LIMIT > 64) + if (max_num && max_num > 64) + RTW_PRINT_SEL(sel, "0x%08x\n", map->m2); +#endif +#if (MACID_NUM_SW_LIMIT > 96) + if (max_num && max_num > 96) + RTW_PRINT_SEL(sel, "0x%08x\n", map->m3); +#endif +} + +inline bool rtw_macid_is_set(struct macid_bmp *map, u8 id) +{ + if (id < 32) + return map->m0 & BIT(id); +#if (MACID_NUM_SW_LIMIT > 32) + else if (id < 64) + return map->m1 & BIT(id - 32); +#endif +#if (MACID_NUM_SW_LIMIT > 64) + else if (id < 96) + return map->m2 & BIT(id - 64); +#endif +#if (MACID_NUM_SW_LIMIT > 96) + else if (id < 128) + return map->m3 & BIT(id - 96); +#endif + else + rtw_warn_on(1); + + return 0; +} + +inline void rtw_macid_map_set(struct macid_bmp *map, u8 id) +{ + if (id < 32) + map->m0 |= BIT(id); +#if (MACID_NUM_SW_LIMIT > 32) + else if (id < 64) + map->m1 |= BIT(id - 32); +#endif +#if (MACID_NUM_SW_LIMIT > 64) + else if (id < 96) + map->m2 |= BIT(id - 64); +#endif +#if (MACID_NUM_SW_LIMIT > 96) + else if (id < 128) + map->m3 |= BIT(id - 96); +#endif + else + rtw_warn_on(1); +} + +inline void rtw_macid_map_clr(struct macid_bmp *map, u8 id) +{ + if (id < 32) + map->m0 &= ~BIT(id); +#if (MACID_NUM_SW_LIMIT > 32) + else if (id < 64) + map->m1 &= ~BIT(id - 32); +#endif +#if (MACID_NUM_SW_LIMIT > 64) + else if (id < 96) + map->m2 &= ~BIT(id - 64); +#endif +#if (MACID_NUM_SW_LIMIT > 96) + else if (id < 128) + map->m3 &= ~BIT(id - 96); +#endif + else + rtw_warn_on(1); +} + +inline bool rtw_macid_is_used(struct macid_ctl_t *macid_ctl, u8 id) +{ + return rtw_macid_is_set(&macid_ctl->used, id); +} + +inline bool rtw_macid_is_bmc(struct macid_ctl_t *macid_ctl, u8 id) +{ + return rtw_macid_is_set(&macid_ctl->bmc, id); +} + +inline u8 rtw_macid_get_iface_bmp(struct macid_ctl_t *macid_ctl, u8 id) +{ + int i; + u8 iface_bmp = 0; + + for (i = 0; i < CONFIG_IFACE_NUMBER; i++) { + if (rtw_macid_is_set(&macid_ctl->if_g[i], id)) + iface_bmp |= BIT(i); + } + return iface_bmp; +} + +inline bool rtw_macid_is_iface_shared(struct macid_ctl_t *macid_ctl, u8 id) +{ +#if CONFIG_IFACE_NUMBER >= 2 + int i; + u8 iface_bmp = 0; + + for (i = 0; i < CONFIG_IFACE_NUMBER; i++) { + if (rtw_macid_is_set(&macid_ctl->if_g[i], id)) { + if (iface_bmp) + return 1; + iface_bmp |= BIT(i); + } + } +#endif + return 0; +} + +inline bool rtw_macid_is_iface_specific(struct macid_ctl_t *macid_ctl, u8 id, _adapter *adapter) +{ + int i; + u8 iface_bmp = 0; + + for (i = 0; i < CONFIG_IFACE_NUMBER; i++) { + if (rtw_macid_is_set(&macid_ctl->if_g[i], id)) { + if (iface_bmp || i != adapter->iface_id) + return 0; + iface_bmp |= BIT(i); + } + } + + return iface_bmp ? 1 : 0; +} + +inline s8 rtw_macid_get_ch_g(struct macid_ctl_t *macid_ctl, u8 id) +{ + int i; + + for (i = 0; i < 2; i++) { + if (rtw_macid_is_set(&macid_ctl->ch_g[i], id)) + return i; + } + return -1; +} + +/*Record bc's mac-id and sec-cam-id*/ +inline void rtw_iface_bcmc_id_set(_adapter *padapter, u8 mac_id) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + + macid_ctl->iface_bmc[padapter->iface_id] = mac_id; +} +inline u8 rtw_iface_bcmc_id_get(_adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + + return macid_ctl->iface_bmc[padapter->iface_id]; +} +#if defined(DBG_CONFIG_ERROR_RESET) +void rtw_iface_bcmc_sec_cam_map_restore(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj); + int cam_id = -1; + + cam_id = rtw_iface_bcmc_id_get(adapter); + if (cam_id != INVALID_SEC_MAC_CAM_ID) + rtw_sec_cam_map_set(&cam_ctl->used, cam_id); +} +#endif +void rtw_alloc_macid(_adapter *padapter, struct sta_info *psta) +{ + int i; + _irqL irqL; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + struct macid_bmp *used_map = &macid_ctl->used; + /* static u8 last_id = 0; for testing */ + u8 last_id = 0; + u8 is_bc_sta = _FALSE; + + if (_rtw_memcmp(psta->cmn.mac_addr, adapter_mac_addr(padapter), ETH_ALEN)) { + psta->cmn.mac_id = macid_ctl->num; + return; + } + + if (_rtw_memcmp(psta->cmn.mac_addr, bc_addr, ETH_ALEN)) { + is_bc_sta = _TRUE; + rtw_iface_bcmc_id_set(padapter, INVALID_SEC_MAC_CAM_ID); /*init default value*/ + } + + if (is_bc_sta + #ifndef SEC_DEFAULT_KEY_SEARCH + && (MLME_IS_STA(padapter) || MLME_IS_NULL(padapter)) + #endif + ) { + /* STA mode have no BMC data TX, shared with this macid */ + /* When non-concurrent, only one BMC data TX is used, shared with this macid */ + /* TODO: When concurrent, non-security BMC data TX may use this, but will not control by specific macid sleep */ + i = RTW_DEFAULT_MGMT_MACID; + goto assigned; + } + + _enter_critical_bh(&macid_ctl->lock, &irqL); + + for (i = last_id; i < macid_ctl->num; i++) { +#ifdef CONFIG_MCC_MODE + /* macid 0/1 reserve for mcc for mgnt queue macid */ + if (MCC_EN(padapter)) { + if (i == MCC_ROLE_STA_GC_MGMT_QUEUE_MACID) + continue; + if (i == MCC_ROLE_SOFTAP_GO_MGMT_QUEUE_MACID) + continue; + } +#endif /* CONFIG_MCC_MODE */ + + #ifndef SEC_DEFAULT_KEY_SEARCH + /* for BMC data TX with force camid */ + if (is_bc_sta && rtw_sec_camid_is_used(dvobj_to_sec_camctl(dvobj), i)) + continue; + #endif + + if (!rtw_macid_is_used(macid_ctl, i)) + break; + } + + if (i < macid_ctl->num) { + + rtw_macid_map_set(used_map, i); + + #ifndef SEC_DEFAULT_KEY_SEARCH + /* for BMC data TX with force camid */ + if (is_bc_sta) { + struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj); + + rtw_macid_map_set(&macid_ctl->bmc, i); + rtw_iface_bcmc_id_set(padapter, i); + rtw_sec_cam_map_set(&cam_ctl->used, i); + if (_rtw_camctl_chk_cap(padapter, SEC_CAP_CHK_EXTRA_SEC)) + rtw_sec_cam_map_set(&cam_ctl->used, i + 1); + } + #endif + + rtw_macid_map_set(&macid_ctl->if_g[padapter->iface_id], i); + macid_ctl->sta[i] = psta; + + /* TODO ch_g? */ + + last_id++; + last_id %= macid_ctl->num; + } + + _exit_critical_bh(&macid_ctl->lock, &irqL); + + if (i >= macid_ctl->num) { + psta->cmn.mac_id = macid_ctl->num; + RTW_ERR(FUNC_ADPT_FMT" if%u, mac_addr:"MAC_FMT" no available macid\n" + , FUNC_ADPT_ARG(padapter), padapter->iface_id + 1, MAC_ARG(psta->cmn.mac_addr)); + rtw_warn_on(1); + goto exit; + } else + goto assigned; + +assigned: + psta->cmn.mac_id = i; + RTW_INFO(FUNC_ADPT_FMT" if%u, mac_addr:"MAC_FMT" macid:%u\n" + , FUNC_ADPT_ARG(padapter), padapter->iface_id + 1, MAC_ARG(psta->cmn.mac_addr), psta->cmn.mac_id); + +exit: + return; +} + +void rtw_release_macid(_adapter *padapter, struct sta_info *psta) +{ + _irqL irqL; + u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + u8 ifbmp; + int i; + + if (_rtw_memcmp(psta->cmn.mac_addr, adapter_mac_addr(padapter), ETH_ALEN)) + goto exit; + + if (psta->cmn.mac_id >= macid_ctl->num) { + RTW_WARN(FUNC_ADPT_FMT" if%u, mac_addr:"MAC_FMT" macid:%u not valid\n" + , FUNC_ADPT_ARG(padapter), padapter->iface_id + 1 + , MAC_ARG(psta->cmn.mac_addr), psta->cmn.mac_id); + rtw_warn_on(1); + goto exit; + } + + if (psta->cmn.mac_id == RTW_DEFAULT_MGMT_MACID) + goto msg; + + _enter_critical_bh(&macid_ctl->lock, &irqL); + + if (!rtw_macid_is_used(macid_ctl, psta->cmn.mac_id)) { + RTW_WARN(FUNC_ADPT_FMT" if%u, mac_addr:"MAC_FMT" macid:%u not used\n" + , FUNC_ADPT_ARG(padapter), padapter->iface_id + 1 + , MAC_ARG(psta->cmn.mac_addr), psta->cmn.mac_id); + _exit_critical_bh(&macid_ctl->lock, &irqL); + rtw_warn_on(1); + goto exit; + } + + ifbmp = rtw_macid_get_iface_bmp(macid_ctl, psta->cmn.mac_id); + if (!(ifbmp & BIT(padapter->iface_id))) { + RTW_WARN(FUNC_ADPT_FMT" if%u, mac_addr:"MAC_FMT" macid:%u not used by self\n" + , FUNC_ADPT_ARG(padapter), padapter->iface_id + 1 + , MAC_ARG(psta->cmn.mac_addr), psta->cmn.mac_id); + _exit_critical_bh(&macid_ctl->lock, &irqL); + rtw_warn_on(1); + goto exit; + } + + if (_rtw_memcmp(psta->cmn.mac_addr, bc_addr, ETH_ALEN)) { + struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj); + u8 id = rtw_iface_bcmc_id_get(padapter); + + if ((id != INVALID_SEC_MAC_CAM_ID) && (id < cam_ctl->num)) { + rtw_sec_cam_map_clr(&cam_ctl->used, id); + if (_rtw_camctl_chk_cap(padapter, SEC_CAP_CHK_EXTRA_SEC)) + rtw_sec_cam_map_clr(&cam_ctl->used, id + 1); + } + + rtw_iface_bcmc_id_set(padapter, INVALID_SEC_MAC_CAM_ID); + } + + rtw_macid_map_clr(&macid_ctl->if_g[padapter->iface_id], psta->cmn.mac_id); + + ifbmp &= ~BIT(padapter->iface_id); + if (!ifbmp) { /* only used by self */ + rtw_macid_map_clr(&macid_ctl->used, psta->cmn.mac_id); + rtw_macid_map_clr(&macid_ctl->bmc, psta->cmn.mac_id); + for (i = 0; i < 2; i++) + rtw_macid_map_clr(&macid_ctl->ch_g[i], psta->cmn.mac_id); + macid_ctl->sta[psta->cmn.mac_id] = NULL; + } + + _exit_critical_bh(&macid_ctl->lock, &irqL); + +msg: + RTW_INFO(FUNC_ADPT_FMT" if%u, mac_addr:"MAC_FMT" macid:%u\n" + , FUNC_ADPT_ARG(padapter), padapter->iface_id + 1 + , MAC_ARG(psta->cmn.mac_addr), psta->cmn.mac_id + ); + +exit: + psta->cmn.mac_id = macid_ctl->num; +} + +/* For 8188E RA */ +u8 rtw_search_max_mac_id(_adapter *padapter) +{ + u8 max_mac_id = 0; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + int i; + _irqL irqL; + + /* TODO: Only search for connected macid? */ + + _enter_critical_bh(&macid_ctl->lock, &irqL); + for (i = (macid_ctl->num - 1); i > 0 ; i--) { + if (rtw_macid_is_used(macid_ctl, i)) + break; + } + _exit_critical_bh(&macid_ctl->lock, &irqL); + max_mac_id = i; + + return max_mac_id; +} + +inline u8 rtw_macid_ctl_set_h2c_msr(struct macid_ctl_t *macid_ctl, u8 id, u8 h2c_msr) +{ + u8 op_num_change_bmp = 0; + + if (id >= macid_ctl->num) { + rtw_warn_on(1); + goto exit; + } + + if (GET_H2CCMD_MSRRPT_PARM_OPMODE(&macid_ctl->h2c_msr[id]) + && !GET_H2CCMD_MSRRPT_PARM_OPMODE(&h2c_msr) + ) { + u8 role = GET_H2CCMD_MSRRPT_PARM_ROLE(&macid_ctl->h2c_msr[id]); + + if (role < H2C_MSR_ROLE_MAX) { + macid_ctl->op_num[role]--; + op_num_change_bmp |= BIT(role); + } + } else if (!GET_H2CCMD_MSRRPT_PARM_OPMODE(&macid_ctl->h2c_msr[id]) + && GET_H2CCMD_MSRRPT_PARM_OPMODE(&h2c_msr) + ) { + u8 role = GET_H2CCMD_MSRRPT_PARM_ROLE(&h2c_msr); + + if (role < H2C_MSR_ROLE_MAX) { + macid_ctl->op_num[role]++; + op_num_change_bmp |= BIT(role); + } + } + + macid_ctl->h2c_msr[id] = h2c_msr; + if (0) + RTW_INFO("macid:%u, h2c_msr:"H2C_MSR_FMT"\n", id, H2C_MSR_ARG(&macid_ctl->h2c_msr[id])); + +exit: + return op_num_change_bmp; +} + +inline void rtw_macid_ctl_set_bw(struct macid_ctl_t *macid_ctl, u8 id, u8 bw) +{ + if (id >= macid_ctl->num) { + rtw_warn_on(1); + return; + } + + macid_ctl->bw[id] = bw; + if (0) + RTW_INFO("macid:%u, bw:%s\n", id, ch_width_str(macid_ctl->bw[id])); +} + +inline void rtw_macid_ctl_set_vht_en(struct macid_ctl_t *macid_ctl, u8 id, u8 en) +{ + if (id >= macid_ctl->num) { + rtw_warn_on(1); + return; + } + + macid_ctl->vht_en[id] = en; + if (0) + RTW_INFO("macid:%u, vht_en:%u\n", id, macid_ctl->vht_en[id]); +} + +inline void rtw_macid_ctl_set_rate_bmp0(struct macid_ctl_t *macid_ctl, u8 id, u32 bmp) +{ + if (id >= macid_ctl->num) { + rtw_warn_on(1); + return; + } + + macid_ctl->rate_bmp0[id] = bmp; + if (0) + RTW_INFO("macid:%u, rate_bmp0:0x%08X\n", id, macid_ctl->rate_bmp0[id]); +} + +inline void rtw_macid_ctl_set_rate_bmp1(struct macid_ctl_t *macid_ctl, u8 id, u32 bmp) +{ + if (id >= macid_ctl->num) { + rtw_warn_on(1); + return; + } + + macid_ctl->rate_bmp1[id] = bmp; + if (0) + RTW_INFO("macid:%u, rate_bmp1:0x%08X\n", id, macid_ctl->rate_bmp1[id]); +} + +#ifdef CONFIG_PROTSEL_MACSLEEP +inline void rtw_macid_ctl_init_sleep_reg(struct macid_ctl_t *macid_ctl, u16 reg_ctrl, u16 reg_info) +{ + macid_ctl->reg_sleep_ctrl = reg_ctrl; + macid_ctl->reg_sleep_info = reg_info; +} +inline void rtw_macid_ctl_init_drop_reg(struct macid_ctl_t *macid_ctl, u16 reg_ctrl, u16 reg_info) +{ + macid_ctl->reg_drop_ctrl = reg_ctrl; + macid_ctl->reg_drop_info = reg_info; +} + +#else +inline void rtw_macid_ctl_init_sleep_reg(struct macid_ctl_t *macid_ctl, u16 m0, u16 m1, u16 m2, u16 m3) +{ + macid_ctl->reg_sleep_m0 = m0; +#if (MACID_NUM_SW_LIMIT > 32) + macid_ctl->reg_sleep_m1 = m1; +#endif +#if (MACID_NUM_SW_LIMIT > 64) + macid_ctl->reg_sleep_m2 = m2; +#endif +#if (MACID_NUM_SW_LIMIT > 96) + macid_ctl->reg_sleep_m3 = m3; +#endif +} + +inline void rtw_macid_ctl_init_drop_reg(struct macid_ctl_t *macid_ctl, u16 m0, u16 m1, u16 m2, u16 m3) +{ + macid_ctl->reg_drop_m0 = m0; +#if (MACID_NUM_SW_LIMIT > 32) + macid_ctl->reg_drop_m1 = m1; +#endif +#if (MACID_NUM_SW_LIMIT > 64) + macid_ctl->reg_drop_m2 = m2; +#endif +#if (MACID_NUM_SW_LIMIT > 96) + macid_ctl->reg_drop_m3 = m3; +#endif +} +#endif + +inline void rtw_macid_ctl_init(struct macid_ctl_t *macid_ctl) +{ + int i; + u8 id = RTW_DEFAULT_MGMT_MACID; + + rtw_macid_map_set(&macid_ctl->used, id); + rtw_macid_map_set(&macid_ctl->bmc, id); + for (i = 0; i < CONFIG_IFACE_NUMBER; i++) + rtw_macid_map_set(&macid_ctl->if_g[i], id); + macid_ctl->sta[id] = NULL; + + _rtw_spinlock_init(&macid_ctl->lock); +} + +inline void rtw_macid_ctl_deinit(struct macid_ctl_t *macid_ctl) +{ + _rtw_spinlock_free(&macid_ctl->lock); +} + +inline bool rtw_bmp_is_set(const u8 *bmp, u8 bmp_len, u8 id) +{ + if (id / 8 >= bmp_len) + return 0; + + return bmp[id / 8] & BIT(id % 8); +} + +inline void rtw_bmp_set(u8 *bmp, u8 bmp_len, u8 id) +{ + if (id / 8 < bmp_len) + bmp[id / 8] |= BIT(id % 8); +} + +inline void rtw_bmp_clear(u8 *bmp, u8 bmp_len, u8 id) +{ + if (id / 8 < bmp_len) + bmp[id / 8] &= ~BIT(id % 8); +} + +inline bool rtw_bmp_not_empty(const u8 *bmp, u8 bmp_len) +{ + int i; + + for (i = 0; i < bmp_len; i++) { + if (bmp[i]) + return 1; + } + + return 0; +} + +inline bool rtw_bmp_not_empty_exclude_bit0(const u8 *bmp, u8 bmp_len) +{ + int i; + + for (i = 0; i < bmp_len; i++) { + if (i == 0) { + if (bmp[i] & 0xFE) + return 1; + } else { + if (bmp[i]) + return 1; + } + } + + return 0; +} + +#ifdef CONFIG_AP_MODE +/* Check the id be set or not in map , if yes , return a none zero value*/ +bool rtw_tim_map_is_set(_adapter *padapter, const u8 *map, u8 id) +{ + return rtw_bmp_is_set(map, padapter->stapriv.aid_bmp_len, id); +} + +/* Set the id into map array*/ +void rtw_tim_map_set(_adapter *padapter, u8 *map, u8 id) +{ + rtw_bmp_set(map, padapter->stapriv.aid_bmp_len, id); +} + +/* Clear the id from map array*/ +void rtw_tim_map_clear(_adapter *padapter, u8 *map, u8 id) +{ + rtw_bmp_clear(map, padapter->stapriv.aid_bmp_len, id); +} + +/* Check have anyone bit be set , if yes return true*/ +bool rtw_tim_map_anyone_be_set(_adapter *padapter, const u8 *map) +{ + return rtw_bmp_not_empty(map, padapter->stapriv.aid_bmp_len); +} + +/* Check have anyone bit be set exclude bit0 , if yes return true*/ +bool rtw_tim_map_anyone_be_set_exclude_aid0(_adapter *padapter, const u8 *map) +{ + return rtw_bmp_not_empty_exclude_bit0(map, padapter->stapriv.aid_bmp_len); +} +#endif /* CONFIG_AP_MODE */ + +#if 0 +unsigned int setup_beacon_frame(_adapter *padapter, unsigned char *beacon_frame) +{ + unsigned short ATIMWindow; + unsigned char *pframe; + struct tx_desc *ptxdesc; + struct rtw_ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + unsigned int rate_len, len = 0; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + WLAN_BSSID_EX *cur_network = &(pmlmeinfo->network); + u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + _rtw_memset(beacon_frame, 0, 256); + + pframe = beacon_frame + TXDESC_SIZE; + + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN); + + set_frame_sub_type(pframe, WIFI_BEACON); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + len = sizeof(struct rtw_ieee80211_hdr_3addr); + + /* timestamp will be inserted by hardware */ + pframe += 8; + len += 8; + + /* beacon interval: 2 bytes */ + _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); + + pframe += 2; + len += 2; + + /* capability info: 2 bytes */ + _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2); + + pframe += 2; + len += 2; + + /* SSID */ + pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &len); + + /* supported rates... */ + rate_len = rtw_get_rateset_len(cur_network->SupportedRates); + pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &len); + + /* DS parameter set */ + pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &len); + + /* IBSS Parameter Set... */ + /* ATIMWindow = cur->Configuration.ATIMWindow; */ + ATIMWindow = 0; + pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &len); + + /* todo: ERP IE */ + + /* EXTERNDED SUPPORTED RATE */ + if (rate_len > 8) + pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &len); + + if ((len + TXDESC_SIZE) > 256) { + /* RTW_INFO("marc: beacon frame too large\n"); */ + return 0; + } + + /* fill the tx descriptor */ + ptxdesc = (struct tx_desc *)beacon_frame; + + /* offset 0 */ + ptxdesc->txdw0 |= cpu_to_le32(len & 0x0000ffff); + ptxdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00ff0000); /* default = 32 bytes for TX Desc */ + + /* offset 4 */ + ptxdesc->txdw1 |= cpu_to_le32((0x10 << QSEL_SHT) & 0x00001f00); + + /* offset 8 */ + ptxdesc->txdw2 |= cpu_to_le32(BMC); + ptxdesc->txdw2 |= cpu_to_le32(BK); + + /* offset 16 */ + ptxdesc->txdw4 = 0x80000000; + + /* offset 20 */ + ptxdesc->txdw5 = 0x00000000; /* 1M */ + + return len + TXDESC_SIZE; +} +#endif + +_adapter *dvobj_get_port0_adapter(struct dvobj_priv *dvobj) +{ + _adapter *port0_iface = NULL; + int i; + for (i = 0; i < dvobj->iface_nums; i++) { + if (get_hw_port(dvobj->padapters[i]) == HW_PORT0) + break; + } + + if (i < 0 || i >= dvobj->iface_nums) + rtw_warn_on(1); + else + port0_iface = dvobj->padapters[i]; + + return port0_iface; +} + +_adapter *dvobj_get_unregisterd_adapter(struct dvobj_priv *dvobj) +{ + _adapter *adapter = NULL; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (dvobj->padapters[i]->registered == 0) + break; + } + + if (i < dvobj->iface_nums) + adapter = dvobj->padapters[i]; + + return adapter; +} + +_adapter *dvobj_get_adapter_by_addr(struct dvobj_priv *dvobj, u8 *addr) +{ + _adapter *adapter = NULL; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + if (_rtw_memcmp(dvobj->padapters[i]->mac_addr, addr, ETH_ALEN) == _TRUE) + break; + } + + if (i < dvobj->iface_nums) + adapter = dvobj->padapters[i]; + + return adapter; +} + +#ifdef CONFIG_WOWLAN +bool rtw_wowlan_parser_pattern_cmd(u8 *input, char *pattern, + int *pattern_len, char *bit_mask) +{ + char *cp = NULL; + size_t len = 0; + int pos = 0, mask_pos = 0, res = 0; + + /* To get the pattern string after "=", when we use : + * iwpriv wlanX pattern=XX:XX:..:XX + */ + cp = strchr(input, '='); + if (cp) { + *cp = 0; + cp++; + input = cp; + } + + /* To take off the newline character '\n'(0x0a) at the end of pattern string, + * when we use echo xxxx > /proc/xxxx + */ + cp = strchr(input, '\n'); + if (cp) + *cp = 0; + + while (input) { + cp = strsep((char **)(&input), ":"); + + if (bit_mask && (strcmp(cp, "-") == 0 || + strcmp(cp, "xx") == 0 || + strcmp(cp, "--") == 0)) { + /* skip this byte and leave mask bit unset */ + } else { + u8 hex; + + if (strlen(cp) != 2) { + RTW_ERR("%s:[ERROR] hex len != 2, input=[%s]\n", + __func__, cp); + goto error; + } + + if (hexstr2bin(cp, &hex, 1) < 0) { + RTW_ERR("%s:[ERROR] pattern is invalid, input=[%s]\n", + __func__, cp); + goto error; + } + + pattern[pos] = hex; + mask_pos = pos / 8; + if (bit_mask) + bit_mask[mask_pos] |= 1 << (pos % 8); + } + + pos++; + } + + (*pattern_len) = pos; + + return _TRUE; +error: + return _FALSE; +} + +void rtw_wow_pattern_sw_reset(_adapter *adapter) +{ + int i; + struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(adapter); + + if (pwrctrlpriv->default_patterns_en == _TRUE) + pwrctrlpriv->wowlan_pattern_idx = DEFAULT_PATTERN_NUM; + else + pwrctrlpriv->wowlan_pattern_idx = 0; + + for (i = 0 ; i < MAX_WKFM_CAM_NUM; i++) { + _rtw_memset(pwrctrlpriv->patterns[i].content, '\0', sizeof(pwrctrlpriv->patterns[i].content)); + _rtw_memset(pwrctrlpriv->patterns[i].mask, '\0', sizeof(pwrctrlpriv->patterns[i].mask)); + pwrctrlpriv->patterns[i].len = 0; + } +} + +u8 rtw_set_default_pattern(_adapter *adapter) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info; + u8 index = 0; + u8 multicast_addr[3] = {0x01, 0x00, 0x5e}; + u8 multicast_ip[4] = {0xe0, 0x28, 0x28, 0x2a}; + + u8 unicast_mask[5] = {0x3f, 0x70, 0x80, 0xc0, 0x03}; + u8 icmpv6_mask[7] = {0x00, 0x70, 0x10, 0x00, 0xc0, 0xc0, 0x3f}; + u8 multicast_mask[5] = {0x07, 0x70, 0x80, 0xc0, 0x03}; + + u8 ip_protocol[3] = {0x08, 0x00, 0x45}; + u8 ipv6_protocol[3] = {0x86, 0xdd, 0x60}; + + u8 *target = NULL; + + if (pwrpriv->default_patterns_en == _FALSE) + return 0; + + for (index = 0 ; index < DEFAULT_PATTERN_NUM ; index++) { + _rtw_memset(pwrpriv->patterns[index].content, 0, + sizeof(pwrpriv->patterns[index].content)); + _rtw_memset(pwrpriv->patterns[index].mask, 0, + sizeof(pwrpriv->patterns[index].mask)); + pwrpriv->patterns[index].len = 0; + } + + /*TCP/ICMP unicast*/ + for (index = 0 ; index < DEFAULT_PATTERN_NUM ; index++) { + switch (index) { + case 0: + target = pwrpriv->patterns[index].content; + _rtw_memcpy(target, adapter_mac_addr(adapter), + ETH_ALEN); + + target += ETH_TYPE_OFFSET; + _rtw_memcpy(target, &ip_protocol, + sizeof(ip_protocol)); + + /* TCP */ + target += (PROTOCOL_OFFSET - ETH_TYPE_OFFSET); + _rtw_memset(target, 0x06, 1); + + target += (IP_OFFSET - PROTOCOL_OFFSET); + + _rtw_memcpy(target, pmlmeinfo->ip_addr, + RTW_IP_ADDR_LEN); + + _rtw_memcpy(pwrpriv->patterns[index].mask, + &unicast_mask, sizeof(unicast_mask)); + + pwrpriv->patterns[index].len = + IP_OFFSET + RTW_IP_ADDR_LEN; + break; + case 1: + target = pwrpriv->patterns[index].content; + _rtw_memcpy(target, adapter_mac_addr(adapter), + ETH_ALEN); + + target += ETH_TYPE_OFFSET; + _rtw_memcpy(target, &ip_protocol, sizeof(ip_protocol)); + + /* ICMP */ + target += (PROTOCOL_OFFSET - ETH_TYPE_OFFSET); + _rtw_memset(target, 0x01, 1); + + target += (IP_OFFSET - PROTOCOL_OFFSET); + _rtw_memcpy(target, pmlmeinfo->ip_addr, + RTW_IP_ADDR_LEN); + + _rtw_memcpy(pwrpriv->patterns[index].mask, + &unicast_mask, sizeof(unicast_mask)); + pwrpriv->patterns[index].len = + + IP_OFFSET + RTW_IP_ADDR_LEN; + break; +#ifdef CONFIG_IPV6 + case 2: + if (pwrpriv->wowlan_ns_offload_en == _TRUE) { + target = pwrpriv->patterns[index].content; + target += ETH_TYPE_OFFSET; + + _rtw_memcpy(target, &ipv6_protocol, + sizeof(ipv6_protocol)); + + /* ICMPv6 */ + target += (IPv6_PROTOCOL_OFFSET - + ETH_TYPE_OFFSET); + _rtw_memset(target, 0x3a, 1); + + target += (IPv6_OFFSET - IPv6_PROTOCOL_OFFSET); + _rtw_memcpy(target, pmlmeinfo->ip6_addr, + RTW_IPv6_ADDR_LEN); + + _rtw_memcpy(pwrpriv->patterns[index].mask, + &icmpv6_mask, sizeof(icmpv6_mask)); + pwrpriv->patterns[index].len = + IPv6_OFFSET + RTW_IPv6_ADDR_LEN; + } + break; +#endif /*CONFIG_IPV6*/ + case 3: + target = pwrpriv->patterns[index].content; + _rtw_memcpy(target, &multicast_addr, + sizeof(multicast_addr)); + + target += ETH_TYPE_OFFSET; + _rtw_memcpy(target, &ip_protocol, sizeof(ip_protocol)); + + /* UDP */ + target += (PROTOCOL_OFFSET - ETH_TYPE_OFFSET); + _rtw_memset(target, 0x11, 1); + + target += (IP_OFFSET - PROTOCOL_OFFSET); + _rtw_memcpy(target, &multicast_ip, + sizeof(multicast_ip)); + + _rtw_memcpy(pwrpriv->patterns[index].mask, + &multicast_mask, sizeof(multicast_mask)); + + pwrpriv->patterns[index].len = + IP_OFFSET + sizeof(multicast_ip); + break; + default: + break; + } + } + return index; +} + +void rtw_dump_priv_pattern(_adapter *adapter, u8 idx) +{ + struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(adapter); + char str_1[128]; + char *p_str; + u8 val8 = 0; + int i = 0, j = 0, len = 0, max_len = 0; + + RTW_INFO("=========[%d]========\n", idx); + + RTW_INFO(">>>priv_pattern_content:\n"); + p_str = str_1; + max_len = sizeof(str_1); + for (i = 0 ; i < MAX_WKFM_PATTERN_SIZE / 8 ; i++) { + _rtw_memset(p_str, 0, max_len); + len = 0; + for (j = 0 ; j < 8 ; j++) { + val8 = pwrctl->patterns[idx].content[i * 8 + j]; + len += snprintf(p_str + len, max_len - len, + "%02x ", val8); + } + RTW_INFO("%s\n", p_str); + } + + RTW_INFO(">>>priv_pattern_mask:\n"); + for (i = 0 ; i < MAX_WKFM_SIZE / 8 ; i++) { + _rtw_memset(p_str, 0, max_len); + len = 0; + for (j = 0 ; j < 8 ; j++) { + val8 = pwrctl->patterns[idx].mask[i * 8 + j]; + len += snprintf(p_str + len, max_len - len, + "%02x ", val8); + } + RTW_INFO("%s\n", p_str); + } + + RTW_INFO(">>>priv_pattern_len:\n"); + RTW_INFO("%s: len: %d\n", __func__, pwrctl->patterns[idx].len); +} + +void rtw_wow_pattern_sw_dump(_adapter *adapter) +{ + int i = 0, total = 0; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + total = pwrpriv->wowlan_pattern_idx; + + RTW_INFO("********[RTK priv-patterns]*********\n"); + for (i = 0 ; i < total; i++) + rtw_dump_priv_pattern(adapter, i); +} + +void rtw_get_sec_iv(PADAPTER padapter, u8 *pcur_dot11txpn, u8 *StaAddr) +{ + struct sta_info *psta; + struct security_priv *psecpriv = &padapter->securitypriv; + + _rtw_memset(pcur_dot11txpn, 0, 8); + if (NULL == StaAddr) + return; + psta = rtw_get_stainfo(&padapter->stapriv, StaAddr); + RTW_INFO("%s(): StaAddr: %02x %02x %02x %02x %02x %02x\n", + __func__, StaAddr[0], StaAddr[1], StaAddr[2], + StaAddr[3], StaAddr[4], StaAddr[5]); + + if (psta) { + if ((psecpriv->dot11PrivacyAlgrthm == _AES_) || + (psecpriv->dot11PrivacyAlgrthm == _CCMP_256_)) + AES_IV(pcur_dot11txpn, psta->dot11txpn, 0); + else if (psecpriv->dot11PrivacyAlgrthm == _TKIP_) + TKIP_IV(pcur_dot11txpn, psta->dot11txpn, 0); + else if ((psecpriv->dot11PrivacyAlgrthm == _GCMP_) || + (psecpriv->dot11PrivacyAlgrthm == _GCMP_256_)) + GCMP_IV(pcur_dot11txpn, psta->dot11txpn, 0); + + RTW_INFO("%s(): CurrentIV: %02x %02x %02x %02x %02x %02x %02x %02x\n" + , __func__, pcur_dot11txpn[0], pcur_dot11txpn[1], + pcur_dot11txpn[2], pcur_dot11txpn[3], pcur_dot11txpn[4], + pcur_dot11txpn[5], pcur_dot11txpn[6], pcur_dot11txpn[7]); + } +} + +#ifdef CONFIG_WAR_OFFLOAD +#if defined(CONFIG_OFFLOAD_MDNS_V4) || defined(CONFIG_OFFLOAD_MDNS_V6) +void rtw_wow_war_mdns_dump_buf(struct seq_file *m, u8 *title, u8 *buf, u32 len) +{ + u32 i; + + RTW_PRINT_SEL(m, "\t%s (%d)\n\t\t", title, len); + for (i = 1; i <= len; i++) + { + RTW_PRINT_SEL(m, "%2.2x-", *(buf + i - 1)); + if( (i%16 == 0) && (len != i) ) RTW_PRINT_SEL(m, "\n\t\t"); + } + RTW_PRINT_SEL(m, "\n\n"); +} + +void rtw_wow_war_mdns_dump_txt(struct seq_file *m, u8 *title, u8 *buf, u32 len) +{ + u16 idx=1, offset=0; /* offset = the location of L in the Length.Value */ + + RTW_PRINT_SEL(m, "\t%s (%d)\n\t", title, len); + for (; offset < len; idx++) + { + int item_len = buf[offset]; + u8 item_buf[256]={0}; + + _rtw_memcpy(item_buf, (buf + offset + 1), item_len); + RTW_PRINT_SEL(m, "\t[%d] => %s (%d)\n\t", idx, item_buf, item_len); + _rtw_memset(item_buf, 0, sizeof(item_buf)); + offset += (1+item_len); + } + RTW_PRINT_SEL(m, "\n\n"); +} + +bool rtw_wow_war_mdns_parser_pattern(u8 *input, char *target, + u32 *target_len, u32 type) +{ + char *cp = NULL, *end = NULL; + size_t len = 0; + int pos = 0, mask_pos = 0, res = 0; + u8 member[2] = {0}; + + /* reset */ + _rtw_memset(target, '\0', type); + (*target_len) = 0; + + cp = strchr(input, '='); + if (cp) { + *cp = 0; + cp++; + input = cp; + } + + while (1) { + cp = strchr(input, ':'); + + if (cp) { + len = strlen(input) - strlen(cp); + *cp = 0; + cp++; + } else + len = 2; + + { + u8 hex,idx=0, pos_in_unit_as_4bit = 0; + + strncpy(member, input, len); + res = sscanf(member, "%02hhx", &hex); + + target[pos] = hex; + + /* RTW_INFO("==> in; input-member = %s, hex = %x, target[%d] = %x\n", member, hex, target[pos], pos); */ + + for(idx = 0; idx<2;idx++) + { + pos_in_unit_as_4bit = pos*2 + (1-idx); + mask_pos = (pos_in_unit_as_4bit /8); + + if(!IsHexDigit(member[idx])) + { + RTW_ERR("%s:[ERROR] pattern is invalid!!(%c)\n",__func__, member[idx]); + goto error; + } + + /* RTW_INFO("==> in; pos = %d, pos_in_unit_as_4bit = %d, mask-pos = %d \n", pos, pos_in_unit_as_4bit, mask_pos); + RTW_INFO("==> in; hex(0x%02x), member(%c%c) \n", pattern[pos], member[1], member[0]); */ + } + /* RTW_INFO_DUMP("Pattern Mask: ",bit_mask, 6); */ + } + + pos++; + if (!cp) + break; + input = cp; + } + + (*target_len) = pos; + + return _TRUE; +error: + return _FALSE; + +} + +static struct war_mdns_service_info default_sinfo[] = { +/* example of default setting */ + RTW_MDNS_SRV_INFO("_ipp", 4, "_tcp", 4, "local", 5, 0x02, 0x77, 7200, "KM1", 3, 0), + RTW_MDNS_SRV_INFO("_ipps", 5, "_tcp", 4, "local", 5, 0x02, 0x77, 7200, "KM2", 3, 0), + RTW_MDNS_SRV_INFO("_http", 5, "_tcp", 4, "local", 5, 0x00, 0x50, 7200, "KM3", 3, 2), + RTW_MDNS_SRV_INFO("_privet", 7, "_tcp", 4, "local", 5, 0x00, 0x50, 7200, "KM4", 3, 3), + RTW_MDNS_SRV_INFO("_https", 6, "_tcp", 4, "local", 5, 0x01, 0xbb, 7200, "KM5", 3, 2), + RTW_MDNS_SRV_INFO("_uscan", 6, "_tcp", 4, "local", 5, 0x1f, 0x91, 7200, "KM6", 3, 4), + RTW_MDNS_SRV_INFO("_printer", 8, "_tcp", 4, "local", 5, 0x23, 0x8c, 7200, "KM7", 3, 1), + RTW_MDNS_SRV_INFO("_pdl-datastream", 15, "_tcp", 4, "local", 5, 0x23, 0x8c, 7200, "KM8", 3, 1) + +}; + +void rtw_wow_war_mdns_parms_reset(_adapter *adapter, u8 is_set_default) +{ + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter); + u8 i =0; + u16 offset=0; + u8 default_domain_name[] = "Generic"; + //u8 default_machine_name[] = { 0x0a, 0x5f, 0x75, 0x6e, 0x69, 0x76, 0x65, 0x72, 0x73, 0x61, 0x6c, 0x04, 0x5f, 0x73, 0x75, 0x62 }; + //u8 default_machine_name_len = 16; + u8 default_machine_name[] = { 0x0a, 0x5f, 0x75, 0x6e, 0x69, 0x76, 0x65, 0x72, 0x73, 0x61, 0x6c}; /* length : 10 name : _universal */ + u8 default_machine_name_len = 11; + + /* set default txt value*/ + char *default_txt_rsp_0_for_serive[2] = { "_ipp", "_ipps" }; + char *default_txt_rsp_0[25] = { + "txtvers=1", "qtotal=1", "usb_MFG=KONICA MINOLTA", "usb_MDL=C754Series", + "rp=ipp/print","priority=54","tr=Generic 35c-4", "product=DriverName", + "pdl=application/postscript,image/urf,application/octet-stream,image/jpeg", + "adminurl=http://KM00D91C.local./wcd/a_network.xml", + "note=Copy Room", "Transparent=T", "Binary=T", "TBCP=T", + "URF=V1,4,w8,SRGB24,ADOBERGB24-48,DEVW8,DEVRGB24,DEVCMYK32,RS150000000,IS19-20-21,MT1-3,OB1,PQ4,DM1,FN3-14,CP255", + "rfo=ipp/faxout", "Fax=T", "Scan=T", "Duplex=T", "Color=T", "air=none", + "Kind=document,envelope,photo", + "PaperMax=tabloid-A3", "UUID=6c183832-69ba-541b-baf6-6d947c144325", "TLS=1.2" + }; + + char *default_txt_rsp_1_for_serive[2] = { "_printer", "_pdl-datastream" }; + char *default_txt_rsp_1[13] = { + "txtvers=1", "qtotal=1", "usb_MFG=KONICA MINOLTA", "usb_MDL=C754Series", + "rp=print","priority=51","tr=Generic 35c-4", "product=DriverName", + "pdl=application/postscript", "note=Copy Room", "Transparent=T", "Binary=T", "TBCP=F" + }; + + char *default_txt_rsp_2_for_serive[2] = { "_http", "_https" }; + char *default_txt_rsp_2[1] = { + "Path=/" + }; + + char *default_txt_rsp_3_for_serive[1] = { "_privet" }; + char *default_txt_rsp_3[5] = { + "txtvers=1", "url=https://www.google.com/cloudprint", + "type=printer", "cs=not-configured","note=Copy Room" + }; + + char *default_txt_rsp_4_for_serive[1] = { "_uscan" }; + char *default_txt_rsp_4[11] = { + "txtvers=1", "vers=2.5", "adminurl=http://KM00D91C.local./wsd/a_network_airprint.xml", + "representation=http://KM00D91C.local./wcd/DeviceIcon_1283png", + "rs=eSCL", "ty=KONICA MINOLTA bishub C287", "note=japan", + "pdl=image/jpeg,image/tiff,application/pdf", + "UUID=dd5454cc-e196-5711-aa1f-35be49a6ca9f", + "cs=color,grayscale,binary", "is=platen,adf,duplex=T" + }; + + + /* reset ===> */ + + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_domain_name, 0, MAX_MDNS_DOMAIN_NAME_LEN); + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_mnane, 0, sizeof(pwrpriv->wowlan_war_offload_mdns_mnane)); + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_service, 0, sizeof(pwrpriv->wowlan_war_offload_mdns_service)); + _rtw_memset(pwrpriv->wowlan_war_offload_mdns_txt_rsp, 0, sizeof(pwrpriv->wowlan_war_offload_mdns_txt_rsp)); + + pwrpriv->wowlan_war_offload_mdns_domain_name_len = 0; + pwrpriv->wowlan_war_offload_mdns_mnane_num = 0; + pwrpriv->wowlan_war_offload_mdns_service_info_num = 0; + pwrpriv->wowlan_war_offload_mdns_txt_rsp_num = 0; + pwrpriv->wowlan_war_offload_mdns_para_cur_size = 0; + pwrpriv->wowlan_war_offload_mdns_rsp_cur_size = 0; + + /* init ===> */ + + if(is_set_default) + { + // domain_name + pwrpriv->wowlan_war_offload_mdns_domain_name_len = strlen(default_domain_name); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_domain_name, default_domain_name, sizeof(default_domain_name)); + + // machine name + pwrpriv->wowlan_war_offload_mdns_mnane_num = 1; + pwrpriv->wowlan_war_offload_mdns_mnane[0].name_len = default_machine_name_len; + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_mnane[0].name, default_machine_name, default_machine_name_len); + + // service info + pwrpriv->wowlan_war_offload_mdns_service_info_num = 8; + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_service, default_sinfo, sizeof(default_sinfo)); + + // type txt rsp 0~5 + // 0 + for(offset=0, i=0; i<25; i++) + { + pwrpriv->wowlan_war_offload_mdns_txt_rsp[0].txt[offset++] = strlen(default_txt_rsp_0[i]); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_txt_rsp[0].txt + offset, default_txt_rsp_0[i], strlen(default_txt_rsp_0[i])); + offset += strlen(default_txt_rsp_0[i]); + RTW_INFO("==> default_txt_rsp_0[%d]: [%s](%zu), offset(%d)\n", i, default_txt_rsp_0[i], strlen(default_txt_rsp_0[i]), offset); + } + pwrpriv->wowlan_war_offload_mdns_txt_rsp[0].txt_len = offset; + // RTW_INFO("==> offset = %d\n\n", offset); + + + // 1 + for(offset=0, i=0; i<13; i++) + { + pwrpriv->wowlan_war_offload_mdns_txt_rsp[1].txt[offset++] = strlen(default_txt_rsp_1[i]); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_txt_rsp[1].txt + offset, default_txt_rsp_1[i], strlen(default_txt_rsp_1[i])); + offset += strlen(default_txt_rsp_1[i]); + } + pwrpriv->wowlan_war_offload_mdns_txt_rsp[1].txt_len = offset; + // RTW_INFO("==> offset = %d\n\n", offset); + + // 2 + for(offset=0, i=0; i<1; i++) + { + pwrpriv->wowlan_war_offload_mdns_txt_rsp[2].txt[offset++] = strlen(default_txt_rsp_2[i]); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_txt_rsp[2].txt + offset, default_txt_rsp_2[i], strlen(default_txt_rsp_2[i])); + offset += strlen(default_txt_rsp_2[i]); + } + pwrpriv->wowlan_war_offload_mdns_txt_rsp[2].txt_len = offset; + // RTW_INFO("==> offset = %d\n\n", offset); + + // 3 + for(offset=0, i=0; i<5; i++) + { + pwrpriv->wowlan_war_offload_mdns_txt_rsp[3].txt[offset++] = strlen(default_txt_rsp_3[i]); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_txt_rsp[3].txt + offset, default_txt_rsp_3[i], strlen(default_txt_rsp_3[i])); + offset += strlen(default_txt_rsp_3[i]); + } + pwrpriv->wowlan_war_offload_mdns_txt_rsp[3].txt_len = offset; + // RTW_INFO("==> offset = %d\n\n", offset); + + // 4 + for(offset=0, i=0; i<11; i++) + { + pwrpriv->wowlan_war_offload_mdns_txt_rsp[4].txt[offset++] = strlen(default_txt_rsp_4[i]); + _rtw_memcpy(pwrpriv->wowlan_war_offload_mdns_txt_rsp[4].txt + offset, default_txt_rsp_4[i], strlen(default_txt_rsp_4[i])); + offset += strlen(default_txt_rsp_4[i]); + } + pwrpriv->wowlan_war_offload_mdns_txt_rsp[4].txt_len = offset; + // RTW_INFO("==> offset = %d\n\n", offset); + + /* txt_rsp_num is always as MAX_MDNS_TXT_NUM because the input mechanism(new/append) makes the entities are not in order */ + pwrpriv->wowlan_war_offload_mdns_txt_rsp_num = MAX_MDNS_TXT_NUM; + } +} + + +#endif /* defined(CONFIG_OFFLOAD_MDNS_V4) || defined(CONFIG_OFFLOAD_MDNS_V6) */ +#endif /* CONFIG_WAR_OFFLOAD */ +#endif /* CONFIG_WOWLAN */ + +inline bool _rtw_wow_chk_cap(_adapter *adapter, u8 cap) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct wow_ctl_t *wow_ctl = &dvobj->wow_ctl; + + if (wow_ctl->wow_cap & cap) + return _TRUE; + return _FALSE; +} + +#ifdef CONFIG_PNO_SUPPORT +#define CSCAN_TLV_TYPE_SSID_IE 'S' +#define CIPHER_IE "key_mgmt=" +#define CIPHER_NONE "NONE" +#define CIPHER_WPA_PSK "WPA-PSK" +#define CIPHER_WPA_EAP "WPA-EAP IEEE8021X" +/* + * SSIDs list parsing from cscan tlv list + */ +int rtw_parse_ssid_list_tlv(char **list_str, pno_ssid_t *ssid, + int max, int *bytes_left) +{ + char *str; + + int idx = 0; + + if ((list_str == NULL) || (*list_str == NULL) || (*bytes_left < 0)) { + RTW_INFO("%s error paramters\n", __func__); + return -1; + } + + str = *list_str; + while (*bytes_left > 0) { + + if (str[0] != CSCAN_TLV_TYPE_SSID_IE) { + *list_str = str; + RTW_INFO("nssid=%d left_parse=%d %d\n", idx, *bytes_left, str[0]); + return idx; + } + + /* Get proper CSCAN_TLV_TYPE_SSID_IE */ + *bytes_left -= 1; + str += 1; + + if (str[0] == 0) { + /* Broadcast SSID */ + ssid[idx].SSID_len = 0; + memset((char *)ssid[idx].SSID, 0x0, WLAN_SSID_MAXLEN); + *bytes_left -= 1; + str += 1; + + RTW_INFO("BROADCAST SCAN left=%d\n", *bytes_left); + } else if (str[0] <= WLAN_SSID_MAXLEN) { + /* Get proper SSID size */ + ssid[idx].SSID_len = str[0]; + *bytes_left -= 1; + str += 1; + + /* Get SSID */ + if (ssid[idx].SSID_len > *bytes_left) { + RTW_INFO("%s out of memory range len=%d but left=%d\n", + __func__, ssid[idx].SSID_len, *bytes_left); + return -1; + } + + memcpy((char *)ssid[idx].SSID, str, ssid[idx].SSID_len); + + *bytes_left -= ssid[idx].SSID_len; + str += ssid[idx].SSID_len; + + RTW_INFO("%s :size=%d left=%d\n", + (char *)ssid[idx].SSID, ssid[idx].SSID_len, *bytes_left); + } else { + RTW_INFO("### SSID size more that %d\n", str[0]); + return -1; + } + + if (idx++ > max) { + RTW_INFO("%s number of SSIDs more that %d\n", __func__, idx); + return -1; + } + } + + *list_str = str; + return idx; +} + +int rtw_parse_cipher_list(struct pno_nlo_info *nlo_info, char *list_str) +{ + + char *pch, *pnext, *pend; + u8 key_len = 0, index = 0; + + pch = list_str; + + if (nlo_info == NULL || list_str == NULL) { + RTW_INFO("%s error paramters\n", __func__); + return -1; + } + + while (strlen(pch) != 0) { + pnext = strstr(pch, "key_mgmt="); + if (pnext != NULL) { + pch = pnext + strlen(CIPHER_IE); + pend = strstr(pch, "}"); + if (strncmp(pch, CIPHER_NONE, + strlen(CIPHER_NONE)) == 0) + nlo_info->ssid_cipher_info[index] = 0x00; + else if (strncmp(pch, CIPHER_WPA_PSK, + strlen(CIPHER_WPA_PSK)) == 0) + nlo_info->ssid_cipher_info[index] = 0x66; + else if (strncmp(pch, CIPHER_WPA_EAP, + strlen(CIPHER_WPA_EAP)) == 0) + nlo_info->ssid_cipher_info[index] = 0x01; + index++; + pch = pend + 1; + } else + break; + } + return 0; +} + +int rtw_dev_nlo_info_set(struct pno_nlo_info *nlo_info, pno_ssid_t *ssid, + int num, int pno_time, int pno_repeat, int pno_freq_expo_max) +{ + + int i = 0; + struct file *fp; + mm_segment_t fs; + loff_t pos = 0; + u8 *source = NULL; + long len = 0; + + RTW_INFO("+%s+\n", __func__); + + nlo_info->fast_scan_period = pno_time; + nlo_info->ssid_num = num & BIT_LEN_MASK_32(8); + nlo_info->hidden_ssid_num = num & BIT_LEN_MASK_32(8); + nlo_info->slow_scan_period = (pno_time * 2); + nlo_info->fast_scan_iterations = 5; + + if (nlo_info->hidden_ssid_num > 8) + nlo_info->hidden_ssid_num = 8; + + /* TODO: channel list and probe index is all empty. */ + for (i = 0 ; i < num ; i++) { + nlo_info->ssid_length[i] + = ssid[i].SSID_len; + } + + /* cipher array */ + fp = filp_open("/data/misc/wifi/wpa_supplicant.conf", O_RDONLY, 0644); + if (IS_ERR(fp)) { + RTW_INFO("Error, wpa_supplicant.conf doesn't exist.\n"); + RTW_INFO("Error, cipher array using default value.\n"); + return 0; + } + + len = i_size_read(fp->f_path.dentry->d_inode); + if (len < 0 || len > 2048) { + RTW_INFO("Error, file size is bigger than 2048.\n"); + RTW_INFO("Error, cipher array using default value.\n"); + return 0; + } + + fs = get_fs(); + set_fs(KERNEL_DS); + + source = rtw_zmalloc(2048); + + if (source != NULL) { + len = vfs_read(fp, source, len, &pos); + rtw_parse_cipher_list(nlo_info, source); + rtw_mfree(source, 2048); + } + + set_fs(fs); + filp_close(fp, NULL); + + RTW_INFO("-%s-\n", __func__); + return 0; +} + +int rtw_dev_ssid_list_set(struct pno_ssid_list *pno_ssid_list, + pno_ssid_t *ssid, u8 num) +{ + + int i = 0; + if (num > MAX_PNO_LIST_COUNT) + num = MAX_PNO_LIST_COUNT; + + for (i = 0 ; i < num ; i++) { + _rtw_memcpy(&pno_ssid_list->node[i].SSID, + ssid[i].SSID, ssid[i].SSID_len); + pno_ssid_list->node[i].SSID_len = ssid[i].SSID_len; + } + return 0; +} + +int rtw_dev_scan_info_set(_adapter *padapter, pno_ssid_t *ssid, + unsigned char ch, unsigned char ch_offset, unsigned short bw_mode) +{ + + struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter); + struct pno_scan_info *scan_info = pwrctl->pscan_info; + u8 band = ch <= 14 ? BAND_ON_2_4G : BAND_ON_5G; + int i; + + scan_info->channel_num = MAX_SCAN_LIST_COUNT; + scan_info->orig_ch = ch; + scan_info->orig_bw = bw_mode; + scan_info->orig_40_offset = ch_offset; + + for (i = 0 ; i < scan_info->channel_num ; i++) { + if (i < 11) + scan_info->ssid_channel_info[i].active = 1; + else + scan_info->ssid_channel_info[i].active = 0; + + scan_info->ssid_channel_info[i].timeout = 100; + + scan_info->ssid_channel_info[i].tx_power = + phy_get_tx_power_index_ex(padapter, 0, CCK, MGN_1M, bw_mode, band, i + 1, i + 1); + + scan_info->ssid_channel_info[i].channel = i + 1; + } + + RTW_INFO("%s, channel_num: %d, orig_ch: %d, orig_bw: %d orig_40_offset: %d\n", + __func__, scan_info->channel_num, scan_info->orig_ch, + scan_info->orig_bw, scan_info->orig_40_offset); + return 0; +} + +int rtw_dev_pno_set(struct net_device *net, pno_ssid_t *ssid, int num, + int pno_time, int pno_repeat, int pno_freq_expo_max) +{ + + _adapter *padapter = (_adapter *)rtw_netdev_priv(net); + struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter); + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + int ret = -1; + + if (num == 0) { + RTW_INFO("%s, nssid is zero, no need to setup pno ssid list\n", __func__); + return 0; + } + + if (pwrctl == NULL) { + RTW_INFO("%s, ERROR: pwrctl is NULL\n", __func__); + return -1; + } else { + pwrctl->pnlo_info = + (pno_nlo_info_t *)rtw_zmalloc(sizeof(pno_nlo_info_t)); + pwrctl->pno_ssid_list = + (pno_ssid_list_t *)rtw_zmalloc(sizeof(pno_ssid_list_t)); + pwrctl->pscan_info = + (pno_scan_info_t *)rtw_zmalloc(sizeof(pno_scan_info_t)); + } + + if (pwrctl->pnlo_info == NULL || + pwrctl->pscan_info == NULL || + pwrctl->pno_ssid_list == NULL) { + RTW_INFO("%s, ERROR: alloc nlo_info, ssid_list, scan_info fail\n", __func__); + goto failing; + } + + pwrctl->wowlan_in_resume = _FALSE; + + pwrctl->pno_inited = _TRUE; + /* NLO Info */ + ret = rtw_dev_nlo_info_set(pwrctl->pnlo_info, ssid, num, + pno_time, pno_repeat, pno_freq_expo_max); + + /* SSID Info */ + ret = rtw_dev_ssid_list_set(pwrctl->pno_ssid_list, ssid, num); + + /* SCAN Info */ + ret = rtw_dev_scan_info_set(padapter, ssid, pmlmeext->cur_channel, + pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); + + RTW_INFO("+%s num: %d, pno_time: %d, pno_repeat:%d, pno_freq_expo_max:%d+\n", + __func__, num, pno_time, pno_repeat, pno_freq_expo_max); + + return 0; + +failing: + if (pwrctl->pnlo_info) { + rtw_mfree((u8 *)pwrctl->pnlo_info, sizeof(pno_nlo_info_t)); + pwrctl->pnlo_info = NULL; + } + if (pwrctl->pno_ssid_list) { + rtw_mfree((u8 *)pwrctl->pno_ssid_list, sizeof(pno_ssid_list_t)); + pwrctl->pno_ssid_list = NULL; + } + if (pwrctl->pscan_info) { + rtw_mfree((u8 *)pwrctl->pscan_info, sizeof(pno_scan_info_t)); + pwrctl->pscan_info = NULL; + } + + return -1; +} + +#ifdef CONFIG_PNO_SET_DEBUG +void rtw_dev_pno_debug(struct net_device *net) +{ + _adapter *padapter = (_adapter *)rtw_netdev_priv(net); + struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter); + int i = 0, j = 0; + + RTW_INFO("*******NLO_INFO********\n"); + RTW_INFO("ssid_num: %d\n", pwrctl->pnlo_info->ssid_num); + RTW_INFO("fast_scan_iterations: %d\n", + pwrctl->pnlo_info->fast_scan_iterations); + RTW_INFO("fast_scan_period: %d\n", pwrctl->pnlo_info->fast_scan_period); + RTW_INFO("slow_scan_period: %d\n", pwrctl->pnlo_info->slow_scan_period); + + + + for (i = 0 ; i < MAX_PNO_LIST_COUNT ; i++) { + RTW_INFO("%d SSID (%s) length (%d) cipher(%x) channel(%d)\n", + i, pwrctl->pno_ssid_list->node[i].SSID, pwrctl->pnlo_info->ssid_length[i], + pwrctl->pnlo_info->ssid_cipher_info[i], pwrctl->pnlo_info->ssid_channel_info[i]); + } + + RTW_INFO("******SCAN_INFO******\n"); + RTW_INFO("ch_num: %d\n", pwrctl->pscan_info->channel_num); + RTW_INFO("orig_ch: %d\n", pwrctl->pscan_info->orig_ch); + RTW_INFO("orig bw: %d\n", pwrctl->pscan_info->orig_bw); + RTW_INFO("orig 40 offset: %d\n", pwrctl->pscan_info->orig_40_offset); + for (i = 0 ; i < MAX_SCAN_LIST_COUNT ; i++) { + RTW_INFO("[%02d] avtive:%d, timeout:%d, tx_power:%d, ch:%02d\n", + i, pwrctl->pscan_info->ssid_channel_info[i].active, + pwrctl->pscan_info->ssid_channel_info[i].timeout, + pwrctl->pscan_info->ssid_channel_info[i].tx_power, + pwrctl->pscan_info->ssid_channel_info[i].channel); + } + RTW_INFO("*****************\n"); +} +#endif /* CONFIG_PNO_SET_DEBUG */ +#endif /* CONFIG_PNO_SUPPORT */ + +inline void rtw_collect_bcn_info(_adapter *adapter) +{ + struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv; + + if (!is_client_associated_to_ap(adapter)) + return; + + pmlmeext->cur_bcn_cnt = pmlmeext->bcn_cnt - pmlmeext->last_bcn_cnt; + pmlmeext->last_bcn_cnt = pmlmeext->bcn_cnt; + /*TODO get offset of bcn's timestamp*/ + /*pmlmeext->bcn_timestamp;*/ +} + +static u32 rtw_get_vht_bitrate(u8 mcs, u8 bw, u8 nss, u8 sgi) +{ + static const u32 base[4][10] = { + { 6500000, + 13000000, + 19500000, + 26000000, + 39000000, + 52000000, + 58500000, + 65000000, + 78000000, + /* not in the spec, but some devices use this: */ + 86500000, + }, + { 13500000, + 27000000, + 40500000, + 54000000, + 81000000, + 108000000, + 121500000, + 135000000, + 162000000, + 180000000, + }, + { 29300000, + 58500000, + 87800000, + 117000000, + 175500000, + 234000000, + 263300000, + 292500000, + 351000000, + 390000000, + }, + { 58500000, + 117000000, + 175500000, + 234000000, + 351000000, + 468000000, + 526500000, + 585000000, + 702000000, + 780000000, + }, + }; + u32 bitrate; + int bw_idx; + + if (mcs > 9) { + RTW_INFO("Invalid mcs = %d\n", mcs); + return 0; + } + + if (nss > 4 || nss < 1) { + RTW_INFO("Now only support nss = 1, 2, 3, 4\n"); + } + + switch (bw) { + case CHANNEL_WIDTH_160: + bw_idx = 3; + break; + case CHANNEL_WIDTH_80: + bw_idx = 2; + break; + case CHANNEL_WIDTH_40: + bw_idx = 1; + break; + case CHANNEL_WIDTH_20: + bw_idx = 0; + break; + default: + RTW_INFO("bw = %d currently not supported\n", bw); + return 0; + } + + bitrate = base[bw_idx][mcs]; + bitrate *= nss; + + if (sgi) + bitrate = (bitrate / 9) * 10; + + /* do NOT round down here */ + return (bitrate + 50000) / 100000; +} + +static u32 rtw_get_ht_bitrate(u8 mcs, u8 bw, u8 sgi) +{ + int modulation, streams, bitrate; + + /* the formula below does only work for MCS values smaller than 32 */ + if (mcs >= 32) { + RTW_INFO("Invalid mcs = %d\n", mcs); + return 0; + } + + if (bw > 1) { + RTW_INFO("Now HT only support bw = 0(20Mhz), 1(40Mhz)\n"); + return 0; + } + + modulation = mcs & 7; + streams = (mcs >> 3) + 1; + + bitrate = (bw == 1) ? 13500000 : 6500000; + + if (modulation < 4) + bitrate *= (modulation + 1); + else if (modulation == 4) + bitrate *= (modulation + 2); + else + bitrate *= (modulation + 3); + + bitrate *= streams; + + if (sgi) + bitrate = (bitrate / 9) * 10; + + return (bitrate + 50000) / 100000; +} + +/** + * @bw: 0(20Mhz), 1(40Mhz), 2(80Mhz), 3(160Mhz) + * @rate_idx: DESC_RATEXXXX & 0x7f + * @sgi: DESC_RATEXXXX >> 7 + * Returns: bitrate in 100kbps + */ +u32 rtw_desc_rate_to_bitrate(u8 bw, u8 rate_idx, u8 sgi) +{ + u32 bitrate; + + if (rate_idx <= DESC_RATE54M){ + u16 ofdm_rate[12] = {10, 20, 55, 110, + 60, 90, 120, 180, 240, 360, 480, 540}; + + bitrate = ofdm_rate[rate_idx]; + } else if ((DESC_RATEMCS0 <= rate_idx) && + (rate_idx <= DESC_RATEMCS31)) { + u8 mcs = rate_idx - DESC_RATEMCS0; + + bitrate = rtw_get_ht_bitrate(mcs, bw, sgi); + } else if ((DESC_RATEVHTSS1MCS0 <= rate_idx) && + (rate_idx <= DESC_RATEVHTSS4MCS9)) { + u8 mcs = (rate_idx - DESC_RATEVHTSS1MCS0) % 10; + u8 nss = ((rate_idx - DESC_RATEVHTSS1MCS0) / 10) + 1; + + bitrate = rtw_get_vht_bitrate(mcs, bw, nss, sgi); + } else { + /* TODO: 60Ghz */ + bitrate = 1; + } + + return bitrate; +} + +#ifdef CONFIG_RTW_MULTI_AP +u8 rtw_get_ch_utilization(_adapter *adapter) +{ + u16 clm = rtw_phydm_clm_ratio(adapter); + u16 nhm = rtw_phydm_nhm_ratio(adapter); + u16 ch_util; + + ch_util = clm / 3 + (2 * (nhm / 3)); + /* For Multi-AP, scaling 0-100 to 0-255 */ + ch_util = 255 * ch_util / 100; + + return (u8)ch_util; +} + +void rtw_ch_util_rpt(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + _adapter *iface; + int i, j; + u8 i_rpts = 0; + u8 *ch_util; + u8 **bssid; + u8 threshold = GET_PRIMARY_ADAPTER(adapter)->ch_util_threshold; + u8 need_rpt = 0; + + if (threshold == 0) + return; + + ch_util = rtw_zmalloc(sizeof(u8) * dvobj->iface_nums); + if (!ch_util) + goto err_out; + bssid = (u8 **) rtw_zmalloc(sizeof(u8 *) * dvobj->iface_nums); + if (!bssid) + goto err_out1; + for (j = 0; j < dvobj->iface_nums; j++) { + *(bssid + j) = (u8 *) rtw_zmalloc(sizeof(u8) * ETH_ALEN); + if (!(*(bssid + j))) + goto err_out2; + } + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + if ((iface) && MLME_IS_AP(iface)) { + *(ch_util + i_rpts) = rtw_get_ch_utilization(iface); + _rtw_memcpy(*(bssid + i_rpts), iface->mac_addr, ETH_ALEN); + + if (*(ch_util + i_rpts) > threshold) + need_rpt = 1; + + i_rpts++; + } + } + + if (need_rpt) + rtw_nlrtw_ch_util_rpt(adapter, i_rpts, ch_util, bssid); + + rtw_mfree(ch_util, sizeof(u8) * dvobj->iface_nums); + for (i = 0; i < dvobj->iface_nums; i++) + rtw_mfree(*(bssid + i), ETH_ALEN); + rtw_mfree(bssid, sizeof(u8 *) * dvobj->iface_nums); + + return; + +err_out2: + for (i = 0; i < j; i++) + rtw_mfree(*(bssid + i), sizeof(u8) * ETH_ALEN); + rtw_mfree(bssid, sizeof(sizeof(u8 *) * dvobj->iface_nums)); +err_out1: + rtw_mfree(ch_util, sizeof(u8) * dvobj->iface_nums); +err_out: + RTW_INFO("[%s] rtw_zmalloc fail\n", __func__); +} +#endif + diff --git a/core/rtw_wnm.c b/core/rtw_wnm.c new file mode 100644 index 0000000..293eb86 --- /dev/null +++ b/core/rtw_wnm.c @@ -0,0 +1,1098 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ + +#include +#include + +#ifndef RTW_WNM_DBG + #define RTW_WNM_DBG 0 +#endif +#if RTW_WNM_DBG + #define RTW_WNM_INFO(fmt, arg...) \ + RTW_INFO(fmt, arg) + #define RTW_WNM_DUMP(str, data, len) \ + RTW_INFO_DUMP(str, data, len) +#else + #define RTW_WNM_INFO(fmt, arg...) do {} while (0) + #define RTW_WNM_DUMP(str, data, len) do {} while (0) +#endif + +#ifdef CONFIG_RTW_WNM + +static u32 wnm_defualt_validity_time = 6000; +static u32 wnm_default_disassoc_time = 5000; +static u32 wnm_disassoc_wait_time = 500; + +/* for wifi test, need more validity time to wait scan done */ +static u32 wnm_ext_validity_time = 4000; + +static void rtw_wmn_btm_cache_update(_adapter *padapter, struct btm_req_hdr *phdr) +{ + struct btm_rpt_cache *pcache = &(padapter->mlmepriv.nb_info.btm_cache); + + pcache->dialog_token = phdr->dialog_token; + pcache->req_mode = phdr->req_mode; + pcache->disassoc_timer = le16_to_cpu(phdr->disassoc_timer); + + if (phdr->validity_interval > 0) + pcache->validity_interval = phdr->validity_interval; + + pcache->term_duration.id = phdr->term_duration.id; + pcache->term_duration.len = phdr->term_duration.len; + pcache->term_duration.tsf = le64_to_cpu(phdr->term_duration.tsf); + pcache->term_duration.duration = le16_to_cpu(phdr->term_duration.duration); + + RTW_WNM_INFO("%s: req_mode(0x%02x), disassoc_timer(0x%04x), " + "validity_interval(0x%02x %s), tsf(0x%llx), duration(0x%02x)\n", + __func__, pcache->req_mode, pcache->disassoc_timer, + pcache->validity_interval, (!phdr->validity_interval)?"default":"", + pcache->term_duration.tsf, + pcache->term_duration.duration); + + if (pcache->validity_interval > 0) { + pcache->validity_time = pcache->validity_interval * 100; + #ifdef CONFIG_RTW_MBO + if (rtw_mbo_wifi_logo_test(padapter)) + pcache->validity_time += wnm_ext_validity_time; + #endif + } + + if (pcache->disassoc_timer > 0) { + pcache->disassoc_time= pcache->disassoc_timer * 100; + #ifdef CONFIG_RTW_MBO + if (rtw_mbo_wifi_logo_test(padapter)) + pcache->disassoc_time += wnm_ext_validity_time; + #endif + } + + pcache->req_stime = rtw_get_current_time(); + + RTW_WNM_INFO("%s: validity_time=%u, disassoc_time=%u\n", + __func__, pcache->validity_time, pcache->disassoc_time); +} + +static u8 rtw_wnm_btm_candidate_validity(struct btm_rpt_cache *pcache, u8 flag) +{ + u8 is_validity =_TRUE; + u32 req_validity_time = rtw_get_passing_time_ms(pcache->req_stime); + + if ((flag & BIT(0)) && (req_validity_time > pcache->validity_time)) + is_validity = _FALSE; + + if ((flag & BIT(1)) && (req_validity_time > pcache->disassoc_time)) + is_validity = _FALSE; + + RTW_WNM_INFO("%s : validity=%u, rtime=%u, vtime=%u. dtime=%u\n", + __func__, is_validity, req_validity_time, + pcache->validity_time, pcache->disassoc_time); + return is_validity; +} + +u8 rtw_wmn_btm_rsp_reason_decision(_adapter *padapter, u8* req_mode) +{ + struct recv_priv *precvpriv = &padapter->recvpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 reason = 0; + + if (!rtw_wnm_btm_diff_bss(padapter)) { + /* Reject - No suitable BSS transition candidates */ + reason = 7; + goto candidate_remove; + } + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_chk_flags(padapter, RTW_FT_BTM_ROAM)) { + /* Accept */ + reason = 0; + goto under_survey; + } +#endif + + if (((*req_mode) & DISASSOC_IMMINENT) == 0) { + /* Reject - Unspecified reject reason */ + reason = 1; + goto candidate_remove; + } + + if (precvpriv->signal_strength_data.avg_val >= pmlmepriv->roam_rssi_threshold) { + reason = 1; + RTW_WNM_INFO("%s : Reject - under high roam rssi(%u, %u) \n", + __func__, precvpriv->signal_strength_data.avg_val, + pmlmepriv->roam_rssi_threshold); + goto candidate_remove; + } + +#ifdef CONFIG_RTW_80211R +under_survey: + if (check_fwstate(pmlmepriv, WIFI_UNDER_SURVEY)) { + RTW_WNM_INFO("%s reject due to _FW_UNDER_SURVEY\n", __func__); + reason = 1; + } +#endif + +candidate_remove: + if (reason !=0) + rtw_wnm_reset_btm_candidate(&pmlmepriv->nb_info); + + return reason; +} + +static u32 rtw_wnm_btm_candidates_offset_get(u8* pframe) +{ + u32 offset = 0; + + if (!pframe) + return 0; + + offset += 7; + + /* BSS Termination Duration check */ + if (wnm_btm_bss_term_inc(pframe)) + offset += 12; + + /* Session Information URL check*/ + if (wnm_btm_ess_disassoc_im(pframe)) { + /*URL length field + URL variable length*/ + offset = 1 + *(pframe + offset); + } + + RTW_WNM_INFO("%s : hdr offset=%u\n", __func__, offset); + return offset; +} + +static void rtw_wnm_btm_req_hdr_parsing(u8* pframe, struct btm_req_hdr *phdr) +{ + u8 *pos; + u32 offset = 0; + + if (!pframe || !phdr) + return; + + _rtw_memset(phdr, 0, sizeof(struct btm_req_hdr)); + phdr->dialog_token = wnm_btm_dialog_token(pframe); + phdr->req_mode = wnm_btm_req_mode(pframe); + phdr->disassoc_timer = wnm_btm_disassoc_timer(pframe); + phdr->validity_interval = wnm_btm_valid_interval(pframe); + if (wnm_btm_bss_term_inc(pframe)) { + pos = wnm_btm_term_duration_offset(pframe); + if (*pos == WNM_BTM_TERM_DUR_SUBEID) { + phdr->term_duration.id = *pos; + phdr->term_duration.len = *(pos + 1); + phdr->term_duration.tsf = *((u64*)(pos + 2)); + phdr->term_duration.duration= *((u16*)(pos + 10)); + } else + RTW_WNM_INFO("%s : invaild BSS Termination Duration content!\n", __func__); + } + + RTW_WNM_INFO("WNM: req_mode(0x%02x), disassoc_timer(0x%04x), validity_interval(0x%02x)\n", + phdr->req_mode, phdr->disassoc_timer, phdr->validity_interval); + if (wnm_btm_bss_term_inc(pframe)) + RTW_WNM_INFO("WNM: tsf(0x%llx), duration(0x%4x)\n", + phdr->term_duration.tsf, phdr->term_duration.duration); +} + +u8 rtw_wnm_btm_reassoc_req(_adapter *padapter) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct roam_nb_info *pnb = &(pmlmepriv->nb_info); + u8 breassoc = _FALSE; + + if (_rtw_memcmp(get_my_bssid(&(pmlmeinfo->network)), + pnb->roam_target_addr, ETH_ALEN)) { + RTW_WNM_INFO("%s : bss "MAC_FMT" found in roam_target "MAC_FMT"\n", + __func__, MAC_ARG(get_my_bssid(&(pmlmeinfo->network))), + MAC_ARG(pnb->roam_target_addr)); + + breassoc = _TRUE; + } + + return breassoc; +} + +void rtw_wnm_roam_scan_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + + if (rtw_is_scan_deny(padapter)) + RTW_WNM_INFO("%s: roam scan would abort by scan_deny!\n", __func__); + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_chk_flags(padapter, RTW_FT_BTM_ROAM)) { + pmlmepriv->need_to_roam = _TRUE; + rtw_set_to_roam(padapter, padapter->registrypriv.max_roaming_times); + RTW_WNM_INFO("%s : enable roaming\n", __func__); + } + + rtw_drv_scan_by_self(padapter, RTW_AUTO_SCAN_REASON_ROAM); +#endif +} + +static void rtw_wnm_roam_scan(_adapter *padapter) +{ + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + + if (rtw_is_scan_deny(padapter)) { + _cancel_timer_ex(&pnb->roam_scan_timer); + _set_timer(&pnb->roam_scan_timer, 1000); + } else + rtw_wnm_roam_scan_hdl((void *)padapter); +} + +void rtw_wnm_disassoc_chk_hdl(void *ctx) +{ + _adapter *padapter = (_adapter *)ctx; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct roam_nb_info *pnb = &(pmlmepriv->nb_info); + + RTW_WNM_INFO("%s : expired\n", __func__); + if (pnb->disassoc_waiting <= 0 ) { + RTW_WNM_INFO("%s : btm roam is interrupted by disassoc\n", __func__); + return; + } + + pnb->disassoc_waiting = _FALSE; + rtw_wnm_roam_scan(padapter); +} + +u8 rtw_wnm_try_btm_roam_imnt(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct roam_nb_info *pnb = &(pmlmepriv->nb_info); + struct btm_rpt_cache *pcache = &(pnb->btm_cache); + u8 reason = 0, flag = 0; + + if (!rtw_wnm_btm_preference_cap(padapter)) { + RTW_WNM_INFO("%s : no btm candidate can be used!\n", __func__); + return 1; + } + + flag = BIT(0) | BIT(1); + if (!rtw_wnm_btm_candidate_validity(pcache, flag)) + return 1; + +#ifdef CONFIG_RTW_MBO + if (!rtw_mbo_wifi_logo_test(padapter) + && !(pcache->req_mode & DISASSOC_IMMINENT)) { + RTW_WNM_INFO("%s : non-disassoc imminet req\n", __func__); + return 1; + } +#endif + + RTW_WNM_INFO("%s : disassoc_waiting(%d)\n", __func__, pnb->disassoc_waiting); + if (pnb->disassoc_waiting) { + _cancel_timer_ex(&pnb->disassoc_chk_timer); + pnb->disassoc_waiting = _FALSE; + rtw_wnm_roam_scan_hdl((void *)padapter); + } else if (!pnb->disassoc_waiting) + RTW_WNM_INFO("%s : waiting for btm roaming start/finish\n", __func__); + else + reason = 1; + + return reason; +} + +void rtw_wnm_process_btm_req(_adapter *padapter, u8* pframe, u32 frame_len) +{ + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + struct btm_req_hdr req_hdr; + u8 *ptr, reason; + u32 elem_len, offset; + + rtw_wnm_btm_req_hdr_parsing(pframe, &req_hdr); + offset = rtw_wnm_btm_candidates_offset_get(pframe); + if (offset == 0) + return; + + if ((frame_len - offset) <= 15) { + RTW_INFO("WNM : Reject - no suitable BSS transition candidates!\n"); + rtw_wnm_issue_action(padapter, + RTW_WLAN_ACTION_WNM_BTM_RSP, 7, req_hdr.dialog_token); + return; + } + + rtw_wmn_btm_cache_update(padapter, &req_hdr); + + ptr = (pframe + offset); + elem_len = (frame_len - offset); + rtw_wnm_btm_candidates_survey(padapter, ptr, elem_len, _TRUE); + reason = rtw_wmn_btm_rsp_reason_decision(padapter, &pframe[3]); + +#ifdef CONFIG_RTW_MBO + /* for wifi-test; AP2 could power-off when BTM-req received */ + if ((reason > 0) && (rtw_mbo_wifi_logo_test(padapter))) { + _rtw_memcpy(pnb->roam_target_addr, pnb->nb_rpt[0].bssid, ETH_ALEN); + RTW_WNM_INFO("%s : used report 0 as roam_target_addr(reason=%u)\n", + __func__, reason); + reason = 0; + pnb->preference_en = _TRUE; + pnb->nb_rpt_valid = _FALSE; + } +#endif + + rtw_wnm_issue_action(padapter, + RTW_WLAN_ACTION_WNM_BTM_RSP, reason, req_hdr.dialog_token); + + if (reason == 0) { + pnb->disassoc_waiting = _TRUE; + _set_timer(&pnb->disassoc_chk_timer, wnm_disassoc_wait_time); + } + +} + +void rtw_wnm_reset_btm_candidate(struct roam_nb_info *pnb) +{ + pnb->preference_en = _FALSE; + _rtw_memset(pnb->roam_target_addr, 0, ETH_ALEN); +} + +void rtw_wnm_reset_btm_cache(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct roam_nb_info *pnb = &(pmlmepriv->nb_info); + struct btm_rpt_cache *pcache = &(pnb->btm_cache); + u8 flag = 0; + + flag |= BIT(0); + if (rtw_wnm_btm_candidate_validity(pcache, flag)) + return; + + rtw_wnm_reset_btm_candidate(pnb); + _rtw_memset(pcache, 0, sizeof(struct btm_rpt_cache)); + pcache->validity_time = wnm_defualt_validity_time; + pcache->disassoc_time= wnm_default_disassoc_time; + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_chk_flags(padapter, RTW_FT_BTM_ROAM)) { + pmlmepriv->need_to_roam = _FALSE; + rtw_set_to_roam(padapter, 0); + RTW_WNM_INFO("%s : disabled roaming\n", __func__); + } +#endif +} + +void rtw_wnm_reset_btm_state(_adapter *padapter) +{ + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + + pnb->last_nb_rpt_entries = 0; + pnb->nb_rpt_is_same = _TRUE; + pnb->nb_rpt_valid = _FALSE; + pnb->nb_rpt_ch_list_num = 0; + pnb->disassoc_waiting = -1; + _rtw_memset(&pnb->nb_rpt, 0, sizeof(pnb->nb_rpt)); + _rtw_memset(&pnb->nb_rpt_ch_list, 0, sizeof(pnb->nb_rpt_ch_list)); + rtw_wnm_reset_btm_cache(padapter); +} + +u32 rtw_wnm_btm_rsp_candidates_sz_get( + _adapter *padapter, u8* pframe, u32 frame_len) +{ + u32 num = 0, sz = 0; + u8 status; + u8 *ptr; + + if (!pframe || (frame_len <= 5)) + goto exit; + + status = wnm_btm_rsp_status(pframe); + if (((status != 0) && (status != 6)) || (frame_len < 23)) + goto exit; + + if (status == 0) + num = (frame_len - 5 - ETH_ALEN)/18; + else + num = (frame_len - 5)/18; + sz = sizeof(struct wnm_btm_cant) * num; +exit: + RTW_WNM_INFO("WNM: %u candidates(sz=%u) in BTM rsp\n", num, sz); + return sz; +} + +void rtw_wnm_process_btm_rsp(_adapter *padapter, + u8* pframe, u32 frame_len, struct btm_rsp_hdr *prsp) +{ + prsp->dialog_token = wnm_btm_dialog_token(pframe); + prsp->status = wnm_btm_rsp_status(pframe); + prsp->termination_delay = wnm_btm_rsp_term_delay(pframe); + + if ((pframe == NULL) || (frame_len == 0)) + return; + + prsp->status = *(pframe + 3); + prsp->termination_delay = *(pframe + 4); + + /* no Target BSSID & Candidate in frame */ + if (frame_len <= 5) + return; + + /* accept */ + if ((prsp->status == 0) && (frame_len >= 11)) + _rtw_memcpy(prsp->bssid, (pframe + 5), ETH_ALEN); + + /* STA BSS Transition Candidate List provided, + and at least one NB report exist */ + if (((prsp->status == 0) || (prsp->status == 6)) && (frame_len >= 23)) { + struct wnm_btm_cant cant; + u8 *ptr, *pend; + u32 idx = 0; + + ptr = pframe + 5; + if (prsp->status == 0) + ptr += ETH_ALEN; + + pend = ptr + frame_len; + prsp->candidates_num = 0; + while (ptr < pend) { + if (*ptr != RTW_WLAN_ACTION_WNM_NB_RPT_ELEM) + break; + _rtw_memset(&cant, 0, sizeof(cant)); + cant.nb_rpt.id = *ptr; + cant.nb_rpt.len = *(ptr + 1); + _rtw_memcpy(cant.nb_rpt.bssid, (ptr + 2), ETH_ALEN); + cant.nb_rpt.bss_info = *((u32 *)(ptr + 8)); + cant.nb_rpt.reg_class = *(ptr + 12); + cant.nb_rpt.ch_num = *(ptr + 13); + cant.nb_rpt.phy_type= *(ptr + 14); + + if (*(ptr + 15) == WNM_BTM_CAND_PREF_SUBEID) + cant.preference = *(ptr + 17); + ptr = ptr + cant.nb_rpt.len + 2; + if (prsp->pcandidates) { + prsp->candidates_num++; + _rtw_memcpy((prsp->pcandidates + sizeof(cant) * idx), &cant, sizeof(cant)); + } + + idx++; + RTW_WNM_INFO("WNM: btm rsp candidate bssid("MAC_FMT + ") ,bss_info(0x%04X), reg_class(0x%02X), ch(%d)," + " phy_type(0x%02X), preference(0x%02X)\n", + MAC_ARG(cant.nb_rpt.bssid), cant.nb_rpt.bss_info, + cant.nb_rpt.reg_class, cant.nb_rpt.ch_num, + cant.nb_rpt.phy_type, cant.preference); + if ((prsp->pcandidates) && (prsp->candidates_num > 0)) + RTW_WNM_DUMP("WNM candidates: ", prsp->pcandidates, + (sizeof(struct wnm_btm_cant) * prsp->candidates_num)); + } + } + +} + +void rtw_wnm_hdr_init(_adapter *padapter, + struct xmit_frame *pactionframe, u8 *pmac, + u8 action, u8 **pcontent) +{ + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct pkt_attrib *pattrib; + struct rtw_ieee80211_hdr *pwlanhdr; + u16 *pfctrl; + u8 category; + + pattrib = &(pactionframe->attrib); + update_mgntframe_attrib(padapter, pattrib); + _rtw_memset(pactionframe->buf_addr, 0, (WLANHDR_OFFSET + TXDESC_OFFSET)); + + *pcontent = (u8 *)(pactionframe->buf_addr + TXDESC_OFFSET); + pwlanhdr = (struct rtw_ieee80211_hdr *)(*pcontent); + pfctrl = &(pwlanhdr->frame_ctl); + *(pfctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, pmac, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(*pcontent, WIFI_ACTION); + + *pcontent += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + category = RTW_WLAN_CATEGORY_WNM; + *pcontent = rtw_set_fixed_ie(*pcontent, 1, &(category), &(pattrib->pktlen)); + *pcontent = rtw_set_fixed_ie(*pcontent, 1, &(action), &(pattrib->pktlen)); +} + +void rtw_wnm_build_btm_req_ies(_adapter *padapter, + u8 **pframe, struct pkt_attrib *pattrib, + struct btm_req_hdr *phdr, u8 *purl, u32 url_len, + u8 *pcandidates, u8 candidate_cnt) +{ + int i; + + *pframe = rtw_set_fixed_ie(*pframe, 1, + &phdr->dialog_token, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &phdr->req_mode, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 2, + (u8 *)&phdr->disassoc_timer, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &phdr->validity_interval, &(pattrib->pktlen)); + + if (phdr->req_mode & BSS_TERMINATION_INCLUDED) { + *pframe = rtw_set_fixed_ie(*pframe, 1, + &phdr->term_duration.id, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &phdr->term_duration.len, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 8, + (u8 *)&phdr->term_duration.tsf, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 2, + (u8 *)&phdr->term_duration.duration, &(pattrib->pktlen)); + } + + if ((purl != NULL) && (url_len > 0) && + (phdr->req_mode & ESS_DISASSOC_IMMINENT)) { + *pframe = rtw_set_fixed_ie(*pframe, 1, + (u8 *)&url_len, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, + url_len, purl, &(pattrib->pktlen)); + } + + if ((pcandidates != NULL) && (candidate_cnt > 0)) { + for (i=0; inb_rpt); + + *pframe = rtw_set_fixed_ie(*pframe, 1, + &pcandidate->nb_rpt.id, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &pcandidate->nb_rpt.len, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, ETH_ALEN, + pcandidate->nb_rpt.bssid, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 4, + (u8 *)&pcandidate->nb_rpt.bss_info, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &pcandidate->nb_rpt.reg_class, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &pcandidate->nb_rpt.ch_num, &(pattrib->pktlen)); + *pframe = rtw_set_fixed_ie(*pframe, 1, + &pcandidate->nb_rpt.phy_type, &(pattrib->pktlen)); + *pframe = rtw_set_ie(*pframe, WNM_BTM_CAND_PREF_SUBEID, 1, + (u8 *)&pcandidate->preference, &(pattrib->pktlen)); + } + } + +} + +void rtw_wnm_issue_btm_req(_adapter *padapter, + u8 *pmac, struct btm_req_hdr *phdr, u8 *purl, u32 url_len, + u8 *pcandidates, u8 candidate_cnt) +{ + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + u8 action, *pframe, dialog_token = 0; + + if (!pmac || is_zero_mac_addr(pmac) + || is_broadcast_mac_addr(pmac)) + return ; + + if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL) + return ; + + rtw_wnm_hdr_init(padapter, pmgntframe, pmac, + RTW_WLAN_ACTION_WNM_BTM_REQ, &pframe); + + pattrib = &(pmgntframe->attrib); + rtw_wnm_build_btm_req_ies(padapter, &pframe, pattrib, + phdr, purl, url_len, pcandidates, candidate_cnt); + + if (0) { + u8 *__p = (u8 *)(pmgntframe->buf_addr + TXDESC_OFFSET); + RTW_WNM_DUMP("WNM BTM REQ :", __p, pattrib->pktlen); + } + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(padapter, pmgntframe); + RTW_INFO("WNM: BSS Transition Management Request sent\n"); +} + +void rtw_wnm_issue_action(_adapter *padapter, + u8 action, u8 reason, u8 dialog) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + struct xmit_frame *pmgntframe; + struct rtw_ieee80211_hdr *pwlanhdr; + struct pkt_attrib *pattrib; + u8 category, termination_delay, *pframe, dialog_token = 0; +#ifdef CONFIG_RTW_MBO + u8 mbo_trans_rej_res = 1; /* Unspecified reason */ + u8 mbo_notif_req_type ; +#endif + u16 *fctrl; + + if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL) + return ; + + pattrib = &(pmgntframe->attrib); + update_mgntframe_attrib(padapter, pattrib); + _rtw_memset(pmgntframe->buf_addr, 0, (WLANHDR_OFFSET + TXDESC_OFFSET)); + + pframe = (u8 *)(pmgntframe->buf_addr + TXDESC_OFFSET); + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + + fctrl = &(pwlanhdr->frame_ctl); + *(fctrl) = 0; + + _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + set_frame_sub_type(pframe, WIFI_ACTION); + + pframe += sizeof(struct rtw_ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr); + + category = RTW_WLAN_CATEGORY_WNM; + pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen)); + + switch (action) { + case RTW_WLAN_ACTION_WNM_BTM_QUERY: + dialog_token++; + pframe = rtw_set_fixed_ie(pframe, 1, &(dialog_token), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(reason), &(pattrib->pktlen)); + RTW_INFO("WNM: BSS Transition Management Query sent\n"); + break; + case RTW_WLAN_ACTION_WNM_BTM_RSP: + dialog_token = dialog; + termination_delay = 0; + pframe = rtw_set_fixed_ie(pframe, 1, &(dialog_token), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(reason), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(termination_delay), &(pattrib->pktlen)); + if (!reason && !is_zero_mac_addr(pmlmepriv->nb_info.roam_target_addr)) { + pframe = rtw_set_fixed_ie(pframe, 6, + pmlmepriv->nb_info.roam_target_addr, &(pattrib->pktlen)); + } + +#ifdef CONFIG_RTW_MBO + rtw_mbo_build_trans_reject_reason_attr(padapter, + &pframe, pattrib, &mbo_trans_rej_res); +#endif + + RTW_INFO("WNM: BSS Transition Management Response sent(reason:%d)\n", reason); + break; + case RTW_WLAN_ACTION_WNM_NOTIF_REQ: +#ifdef CONFIG_RTW_MBO + dialog_token++; + mbo_notif_req_type = WLAN_EID_VENDOR_SPECIFIC; + pframe = rtw_set_fixed_ie(pframe, 1, &(dialog_token), &(pattrib->pktlen)); + pframe = rtw_set_fixed_ie(pframe, 1, &(mbo_notif_req_type), &(pattrib->pktlen)); + rtw_mbo_build_wnm_notification(padapter, &pframe, pattrib); + RTW_INFO("WNM: Notification request sent\n"); +#endif + break; + default: + goto exit; + } + + pattrib->last_txcmdsz = pattrib->pktlen; + dump_mgntframe(padapter, pmgntframe); + +exit: + return; +} + +/* argument req_ie@cfg80211_roamed()/cfg80211_connect_result() + is association request IEs format. if driver used reassoc-req format, + RSN IE could not be parsed @supplicant process */ +void rtw_wnm_update_reassoc_req_ie(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + u32 dup_len, offset; + u8 *pdup; + + if (!pmlmepriv->assoc_req || !pmlmepriv->assoc_req_len) + return; + + /* total len is assoc req len without Current AP Field*/ + dup_len = pmlmepriv->assoc_req_len - ETH_ALEN; + + /* offset is a len of 80211 header + capability(2B) + listen interval(2B) */ + offset = sizeof(struct rtw_ieee80211_hdr_3addr) + 4; + + pdup = rtw_zmalloc(dup_len); + if (pdup) { + /* remove Current AP Field @reassoc req IE */ + _rtw_memcpy(pdup, pmlmepriv->assoc_req, offset); + _rtw_memcpy(pdup + offset, pmlmepriv->assoc_req + offset + ETH_ALEN, + pmlmepriv->assoc_req_len - offset); + rtw_buf_update(&pmlmepriv->assoc_req, + &pmlmepriv->assoc_req_len, pdup, dup_len); + rtw_mfree(pdup, dup_len); + } +} +#endif /* CONFIG_RTW_WNM */ + +#if defined(CONFIG_RTW_WNM) || defined(CONFIG_RTW_80211K) +void rtw_roam_nb_info_init(_adapter *padapter) +{ + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + struct btm_rpt_cache *pcache = &(pnb->btm_cache); + + _rtw_memset(&pnb->nb_rpt, 0, sizeof(pnb->nb_rpt)); + _rtw_memset(&pnb->nb_rpt_ch_list, 0, sizeof(pnb->nb_rpt_ch_list)); + _rtw_memset(&pnb->roam_target_addr, 0, ETH_ALEN); + pnb->nb_rpt_valid = _FALSE; + pnb->nb_rpt_ch_list_num = 0; + pnb->preference_en = _FALSE; + pnb->nb_rpt_is_same = _TRUE; + pnb->last_nb_rpt_entries = 0; + pnb->disassoc_waiting = -1; +#ifdef CONFIG_RTW_WNM + pnb->features = 0; + /* pnb->features |= RTW_WNM_FEATURE_BTM_REQ_EN; */ + +#ifdef CONFIG_PLATFORM_CMAP_INTFS + pnb->features |= RTW_WNM_FEATURE_BTM_REQ_EN; +#endif + + rtw_init_timer(&pnb->roam_scan_timer, + padapter, rtw_wnm_roam_scan_hdl, + padapter); + rtw_init_timer(&pnb->disassoc_chk_timer, + padapter, rtw_wnm_disassoc_chk_hdl, + padapter); + + _rtw_memset(pcache, 0, sizeof(struct btm_rpt_cache)); + pcache->validity_time = wnm_defualt_validity_time; + pcache->disassoc_time= wnm_default_disassoc_time ; +#endif +} + +u8 rtw_roam_nb_scan_list_set( + _adapter *padapter, struct sitesurvey_parm *pparm) +{ + u8 ret = _FALSE; + u32 i; + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct roam_nb_info *pnb = &(pmlmepriv->nb_info); + +#ifdef CONFIG_RTW_80211R + if (!rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE) + && !rtw_ft_chk_flags(padapter, RTW_FT_BTM_ROAM)) + return ret; +#endif + + if (!pmlmepriv->need_to_roam) + return ret; + + if ((!pmlmepriv->nb_info.nb_rpt_valid) || (!pnb->nb_rpt_ch_list_num)) + return ret; + + if (!pparm) + return ret; + + rtw_init_sitesurvey_parm(padapter, pparm); + if (rtw_roam_busy_scan(padapter, pnb)) { + pparm->ch_num = 1; + pparm->ch[pmlmepriv->ch_cnt].hw_value = + pnb->nb_rpt_ch_list[pmlmepriv->ch_cnt].hw_value; + pmlmepriv->ch_cnt++; + ret = _TRUE; + + RTW_WNM_INFO("%s: ch_cnt=%u, (%u)hw_value=%u\n", + __func__, pparm->ch_num, pmlmepriv->ch_cnt, + pparm->ch[pmlmepriv->ch_cnt].hw_value); + + if (pmlmepriv->ch_cnt == pnb->nb_rpt_ch_list_num) { + pmlmepriv->nb_info.nb_rpt_valid = _FALSE; + pmlmepriv->ch_cnt = 0; + } + goto set_bssid_list; + } + + pparm->ch_num = (pnb->nb_rpt_ch_list_num > RTW_CHANNEL_SCAN_AMOUNT)? + (RTW_CHANNEL_SCAN_AMOUNT):(pnb->nb_rpt_ch_list_num); + for (i=0; ich_num; i++) { + pparm->ch[i].hw_value = pnb->nb_rpt_ch_list[i].hw_value; + pparm->ch[i].flags = RTW_IEEE80211_CHAN_PASSIVE_SCAN; + } + + pmlmepriv->nb_info.nb_rpt_valid = _FALSE; + pmlmepriv->ch_cnt = 0; + ret = _TRUE; + +set_bssid_list: + rtw_set_802_11_bssid_list_scan(padapter, pparm); + return ret; +} + +static u8 rtw_wnm_nb_elem_parsing( + u8* pdata, u32 data_len, u8 from_btm, + u32 *nb_rpt_num, u8 *nb_rpt_is_same, + struct roam_nb_info *pnb, struct wnm_btm_cant *pcandidates) +{ + u8 bfound = _FALSE, ret = _SUCCESS; + u8 *ptr, *pend, *op; + u32 elem_len, subelem_len, op_len; + u32 i, nb_rpt_entries = 0; + struct nb_rpt_hdr *pie; + struct wnm_btm_cant *pcandidate; + + if ((!pdata) || (!pnb)) + return _FAIL; + + if ((from_btm) && (!pcandidates)) + return _FAIL; + + ptr = pdata; + pend = ptr + data_len; + elem_len = data_len; + subelem_len = (u32)*(pdata+1); + + for (i=0; i < RTW_MAX_NB_RPT_NUM; i++) { + if (((ptr + 7) > pend) || (elem_len < subelem_len)) + break; + + if (*ptr != RTW_WLAN_ACTION_WNM_NB_RPT_ELEM) { + RTW_WNM_INFO("WNM: end of data(0x%2x)!\n", *ptr); + break; + } + + pie = (struct nb_rpt_hdr *)ptr; + if (from_btm) { + op = rtw_get_ie((u8 *)(ptr+15), + WNM_BTM_CAND_PREF_SUBEID, + &op_len, (subelem_len - 15)); + } + + ptr = (u8 *)(ptr + subelem_len + 2); + elem_len -= (subelem_len +2); + subelem_len = *(ptr+1); + if (from_btm) { + pcandidate = (pcandidates + i); + _rtw_memcpy(&pcandidate->nb_rpt, pie, sizeof(struct nb_rpt_hdr)); + if (op && (op_len !=0)) { + pcandidate->preference = *(op + 2); + bfound = _TRUE; + } else + pcandidate->preference = 0; + + RTW_WNM_INFO("WNM: preference check bssid("MAC_FMT + ") ,bss_info(0x%04X), reg_class(0x%02X), ch(%d)," + " phy_type(0x%02X), preference(0x%02X)\n", + MAC_ARG(pcandidate->nb_rpt.bssid), pcandidate->nb_rpt.bss_info, + pcandidate->nb_rpt.reg_class, pcandidate->nb_rpt.ch_num, + pcandidate->nb_rpt.phy_type, pcandidate->preference); + } else { + if (_rtw_memcmp(&pnb->nb_rpt[i], pie, sizeof(struct nb_rpt_hdr)) == _FALSE) + *nb_rpt_is_same = _FALSE; + _rtw_memcpy(&pnb->nb_rpt[i], pie, sizeof(struct nb_rpt_hdr)); + } + nb_rpt_entries++; + } + + if (from_btm) + pnb->preference_en = (bfound)?_TRUE:_FALSE; + + *nb_rpt_num = nb_rpt_entries; + return ret; +} + +/* selection sorting based on preference value + * IN : nb_rpt_entries - candidate num + * IN/OUT : pcandidates - candidate list + * return : TRUE - means pcandidates is updated. + */ +static u8 rtw_wnm_candidates_sorting( + u32 nb_rpt_entries, struct wnm_btm_cant *pcandidates) +{ + u8 updated = _FALSE; + u32 i, j, pos; + struct wnm_btm_cant swap; + struct wnm_btm_cant *pcant_1, *pcant_2; + + if ((!nb_rpt_entries) || (!pcandidates)) + return updated; + + for (i=0; i < (nb_rpt_entries - 1); i++) { + pos = i; + for (j=(i + 1); j < nb_rpt_entries; j++) { + pcant_1 = pcandidates+pos; + pcant_2 = pcandidates+j; + if ((pcant_1->preference) < (pcant_2->preference)) + pos = j; + } + + if (pos != i) { + updated = _TRUE; + _rtw_memcpy(&swap, (pcandidates+i), sizeof(struct wnm_btm_cant)); + _rtw_memcpy((pcandidates+i), (pcandidates+pos), sizeof(struct wnm_btm_cant)); + _rtw_memcpy((pcandidates+pos), &swap, sizeof(struct wnm_btm_cant)); + } + } + return updated; +} + +static void rtw_wnm_nb_info_update( + u32 nb_rpt_entries, u8 from_btm, + struct roam_nb_info *pnb, struct wnm_btm_cant *pcandidates, + u8 *nb_rpt_is_same) +{ + u8 is_found; + u32 i, j; + struct wnm_btm_cant *pcand; + + if (!pnb) + return; + + pnb->nb_rpt_ch_list_num = 0; + for (i=0; inb_rpt[i], &pcand->nb_rpt, + sizeof(struct nb_rpt_hdr)) == _FALSE) + *nb_rpt_is_same = _FALSE; + _rtw_memcpy(&pnb->nb_rpt[i], &pcand->nb_rpt, sizeof(struct nb_rpt_hdr)); + } + + RTW_WNM_INFO("WNM: bssid(" MAC_FMT + ") , bss_info(0x%04X), reg_class(0x%02X), ch_num(%d), phy_type(0x%02X)\n", + MAC_ARG(pnb->nb_rpt[i].bssid), pnb->nb_rpt[i].bss_info, + pnb->nb_rpt[i].reg_class, pnb->nb_rpt[i].ch_num, + pnb->nb_rpt[i].phy_type); + + if (pnb->nb_rpt[i].ch_num == 0) + continue; + + for (j=0; jnb_rpt[i].ch_num == pnb->nb_rpt_ch_list[j].hw_value) { + is_found = _TRUE; + break; + } + } + + if (!is_found) { + pnb->nb_rpt_ch_list[pnb->nb_rpt_ch_list_num].hw_value = pnb->nb_rpt[i].ch_num; + pnb->nb_rpt_ch_list_num++; + } + } +} + +static void rtw_wnm_btm_candidate_select(_adapter *padapter) +{ + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + struct wlan_network *pnetwork; + u8 bfound = _FALSE; + u8 ignore_currrent = _FALSE; + u32 i; + +#ifdef CONFIG_RTW_80211R + if (rtw_ft_chk_flags(padapter, RTW_FT_BTM_ROAM)) + ignore_currrent = _TRUE; +#endif + + for (i = 0; i < pnb->last_nb_rpt_entries; i++) { + if (ignore_currrent && + (_rtw_memcmp(pnb->nb_rpt[i].bssid,\ + padapter->mlmepriv.cur_network.network.MacAddress, ETH_ALEN))) { + RTW_WNM_INFO("WNM : ignore candidate "MAC_FMT" for it's connected(%u)!\n", + MAC_ARG(pnb->nb_rpt[i].bssid), i); + continue; + } + + pnetwork = rtw_find_network( + &(pmlmepriv->scanned_queue), + pnb->nb_rpt[i].bssid); + + if (pnetwork) { + bfound = _TRUE; + break; + } + } + + if (bfound) { + _rtw_memcpy(pnb->roam_target_addr, pnb->nb_rpt[i].bssid, ETH_ALEN); + RTW_INFO("WNM : select btm entry(%d) - %s("MAC_FMT", ch:%u) rssi:%d\n" + , i + , pnetwork->network.Ssid.Ssid + , MAC_ARG(pnetwork->network.MacAddress) + , pnetwork->network.Configuration.DSConfig + , (int)pnetwork->network.Rssi); + } else + _rtw_memset(pnb->roam_target_addr,0, ETH_ALEN); +} + +u32 rtw_wnm_btm_candidates_survey( + _adapter *padapter, u8* pframe, u32 elem_len, u8 from_btm) +{ + struct roam_nb_info *pnb = &(padapter->mlmepriv.nb_info); + struct wnm_btm_cant *pcandidate_list = NULL; + u8 nb_rpt_is_same = _TRUE; + u32 ret = _FAIL; + u32 nb_rpt_entries = 0; + + if (from_btm) { + u32 mlen = sizeof(struct wnm_btm_cant) * RTW_MAX_NB_RPT_NUM; + pcandidate_list = (struct wnm_btm_cant *)rtw_malloc(mlen); + if (pcandidate_list == NULL) + goto exit; + } + + /*clean the status set last time*/ + _rtw_memset(&pnb->nb_rpt_ch_list, 0, sizeof(pnb->nb_rpt_ch_list)); + pnb->nb_rpt_valid = _FALSE; + if (!rtw_wnm_nb_elem_parsing( + pframe, elem_len, from_btm, + &nb_rpt_entries, &nb_rpt_is_same, + pnb, pcandidate_list)) + goto exit; + + if (nb_rpt_entries != 0) { + if ((from_btm) && (rtw_wnm_btm_preference_cap(padapter))) + rtw_wnm_candidates_sorting(nb_rpt_entries, pcandidate_list); + + rtw_wnm_nb_info_update( + nb_rpt_entries, from_btm, + pnb, pcandidate_list, &nb_rpt_is_same); + } + + RTW_WNM_INFO("nb_rpt_is_same = %d, nb_rpt_entries = %d, last_nb_rpt_entries = %d\n", + nb_rpt_is_same, nb_rpt_entries, pnb->last_nb_rpt_entries); + if ((nb_rpt_is_same == _TRUE) && (nb_rpt_entries == pnb->last_nb_rpt_entries)) + pnb->nb_rpt_is_same = _TRUE; + else { + pnb->nb_rpt_is_same = _FALSE; + pnb->last_nb_rpt_entries = nb_rpt_entries; + } + + if ((from_btm) && (nb_rpt_entries != 0)) + rtw_wnm_btm_candidate_select(padapter); + + pnb->nb_rpt_valid = _TRUE; + ret = _SUCCESS; + +exit: + if (from_btm && pcandidate_list) + rtw_mfree((u8 *)pcandidate_list, sizeof(struct wnm_btm_cant) * RTW_MAX_NB_RPT_NUM); + + return ret; +} + +#endif /*defined(CONFIG_RTW_WNM) || defined(CONFIG_RTW_80211K) */ + diff --git a/core/rtw_xmit.c b/core/rtw_xmit.c new file mode 100644 index 0000000..346e508 --- /dev/null +++ b/core/rtw_xmit.c @@ -0,0 +1,6673 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_XMIT_C_ + +#include +#include + +static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 }; +static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 }; + +static void _init_txservq(struct tx_servq *ptxservq) +{ + _rtw_init_listhead(&ptxservq->tx_pending); + _rtw_init_queue(&ptxservq->sta_pending); + ptxservq->qcnt = 0; +} + + +void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv) +{ + + + _rtw_memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv)); + + _rtw_spinlock_init(&psta_xmitpriv->lock); + + /* for(i = 0 ; i < MAX_NUMBLKS; i++) */ + /* _init_txservq(&(psta_xmitpriv->blk_q[i])); */ + + _init_txservq(&psta_xmitpriv->be_q); + _init_txservq(&psta_xmitpriv->bk_q); + _init_txservq(&psta_xmitpriv->vi_q); + _init_txservq(&psta_xmitpriv->vo_q); +#ifdef CONFIG_RTW_MGMT_QUEUE + _init_txservq(&psta_xmitpriv->mgmt_q); +#endif + _rtw_init_listhead(&psta_xmitpriv->legacy_dz); + _rtw_init_listhead(&psta_xmitpriv->apsd); + + +} + +void rtw_init_xmit_block(_adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + _rtw_spinlock_init(&dvobj->xmit_block_lock); + dvobj->xmit_block = XMIT_BLOCK_NONE; + +} +void rtw_free_xmit_block(_adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + _rtw_spinlock_free(&dvobj->xmit_block_lock); +} + +s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, _adapter *padapter) +{ + int i; + struct xmit_buf *pxmitbuf; + struct xmit_frame *pxframe; + sint res = _SUCCESS; + + + /* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */ + /* _rtw_memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv)); */ + + _rtw_spinlock_init(&pxmitpriv->lock); + _rtw_spinlock_init(&pxmitpriv->lock_sctx); + _rtw_init_sema(&pxmitpriv->xmit_sema, 0); + + /* + Please insert all the queue initializaiton using _rtw_init_queue below + */ + + pxmitpriv->adapter = padapter; + + /* for(i = 0 ; i < MAX_NUMBLKS; i++) */ + /* _rtw_init_queue(&pxmitpriv->blk_strms[i]); */ + + _rtw_init_queue(&pxmitpriv->be_pending); + _rtw_init_queue(&pxmitpriv->bk_pending); + _rtw_init_queue(&pxmitpriv->vi_pending); + _rtw_init_queue(&pxmitpriv->vo_pending); + _rtw_init_queue(&pxmitpriv->mgmt_pending); + + /* _rtw_init_queue(&pxmitpriv->legacy_dz_queue); */ + /* _rtw_init_queue(&pxmitpriv->apsd_queue); */ + + _rtw_init_queue(&pxmitpriv->free_xmit_queue); +#ifdef CONFIG_LAYER2_ROAMING + _rtw_init_queue(&pxmitpriv->rpkt_queue); +#endif + + /* + Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME, + and initialize free_xmit_frame below. + Please also apply free_txobj to link_up all the xmit_frames... + */ + + pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4); + + if (pxmitpriv->pallocated_frame_buf == NULL) { + pxmitpriv->pxmit_frame_buf = NULL; + res = _FAIL; + goto exit; + } + pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4); + /* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */ + /* ((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3); */ + + pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf; + + for (i = 0; i < NR_XMITFRAME; i++) { + _rtw_init_listhead(&(pxframe->list)); + + pxframe->padapter = padapter; + pxframe->frame_tag = NULL_FRAMETAG; + + pxframe->pkt = NULL; + + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + rtw_list_insert_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue)); + + pxframe++; + } + + pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME; + + pxmitpriv->frag_len = MAX_FRAG_THRESHOLD; + + + /* init xmit_buf */ + _rtw_init_queue(&pxmitpriv->free_xmitbuf_queue); + _rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue); + + pxmitpriv->pallocated_xmitbuf = rtw_zvmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4); + + if (pxmitpriv->pallocated_xmitbuf == NULL) { + res = _FAIL; + goto exit; + } + + pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4); + /* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */ + /* ((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3); */ + + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf; + + for (i = 0; i < NR_XMITBUFF; i++) { + _rtw_init_listhead(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_DATA; + + /* Tx buf allocation may fail sometimes, so sleep and retry. */ + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), _TRUE); + if (res == _FAIL) { + rtw_msleep_os(10); + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), _TRUE); + if (res == _FAIL) + goto exit; + } + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; +#endif + + pxmitbuf->flags = XMIT_VO_QUEUE; + + rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue)); +#ifdef DBG_XMIT_BUF + pxmitbuf->no = i; +#endif + + pxmitbuf++; + + } + + pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF; + + /* init xframe_ext queue, the same count as extbuf */ + _rtw_init_queue(&pxmitpriv->free_xframe_ext_queue); + + pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4); + + if (pxmitpriv->xframe_ext_alloc_addr == NULL) { + pxmitpriv->xframe_ext = NULL; + res = _FAIL; + goto exit; + } + pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4); + pxframe = (struct xmit_frame *)pxmitpriv->xframe_ext; + + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + _rtw_init_listhead(&(pxframe->list)); + + pxframe->padapter = padapter; + pxframe->frame_tag = NULL_FRAMETAG; + + pxframe->pkt = NULL; + + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + pxframe->ext_tag = 1; + + rtw_list_insert_tail(&(pxframe->list), &(pxmitpriv->free_xframe_ext_queue.queue)); + + pxframe++; + } + pxmitpriv->free_xframe_ext_cnt = NR_XMIT_EXTBUFF; + + /* Init xmit extension buff */ + _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue); + + pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4); + + if (pxmitpriv->pallocated_xmit_extbuf == NULL) { + res = _FAIL; + goto exit; + } + + pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4); + + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf; + + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + _rtw_init_listhead(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_MGNT; + + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ, _TRUE); + if (res == _FAIL) { + res = _FAIL; + goto exit; + } + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMIT_EXTBUF_SZ; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; +#endif + + rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue)); +#ifdef DBG_XMIT_BUF_EXT + pxmitbuf->no = i; +#endif + pxmitbuf++; + + } + + pxmitpriv->free_xmit_extbuf_cnt = NR_XMIT_EXTBUFF; + + for (i = 0; i < CMDBUF_MAX; i++) { + pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i]; + if (pxmitbuf) { + _rtw_init_listhead(&pxmitbuf->list); + + pxmitbuf->priv_data = NULL; + pxmitbuf->padapter = padapter; + pxmitbuf->buf_tag = XMITBUF_CMD; + + res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_CMDBUF_SZ + XMITBUF_ALIGN_SZ, _TRUE); + if (res == _FAIL) { + res = _FAIL; + goto exit; + } + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->phead = pxmitbuf->pbuf; + pxmitbuf->pend = pxmitbuf->pbuf + MAX_CMDBUF_SZ; + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; +#endif + pxmitbuf->alloc_sz = MAX_CMDBUF_SZ + XMITBUF_ALIGN_SZ; + } + } + + rtw_alloc_hwxmits(padapter); + rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry); + + for (i = 0; i < 4; i++) + pxmitpriv->wmm_para_seq[i] = i; + +#ifdef CONFIG_USB_HCI + pxmitpriv->txirp_cnt = 1; + + _rtw_init_sema(&(pxmitpriv->tx_retevt), 0); + + /* per AC pending irp */ + pxmitpriv->beq_cnt = 0; + pxmitpriv->bkq_cnt = 0; + pxmitpriv->viq_cnt = 0; + pxmitpriv->voq_cnt = 0; +#endif + + +#ifdef CONFIG_XMIT_ACK + pxmitpriv->ack_tx = _FALSE; + _rtw_mutex_init(&pxmitpriv->ack_tx_mutex); + rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0); +#endif + +#ifdef CONFIG_TX_AMSDU + rtw_init_timer(&(pxmitpriv->amsdu_vo_timer), padapter, + rtw_amsdu_vo_timeout_handler, padapter); + pxmitpriv->amsdu_vo_timeout = RTW_AMSDU_TIMER_UNSET; + + rtw_init_timer(&(pxmitpriv->amsdu_vi_timer), padapter, + rtw_amsdu_vi_timeout_handler, padapter); + pxmitpriv->amsdu_vi_timeout = RTW_AMSDU_TIMER_UNSET; + + rtw_init_timer(&(pxmitpriv->amsdu_be_timer), padapter, + rtw_amsdu_be_timeout_handler, padapter); + pxmitpriv->amsdu_be_timeout = RTW_AMSDU_TIMER_UNSET; + + rtw_init_timer(&(pxmitpriv->amsdu_bk_timer), padapter, + rtw_amsdu_bk_timeout_handler, padapter); + pxmitpriv->amsdu_bk_timeout = RTW_AMSDU_TIMER_UNSET; + + pxmitpriv->amsdu_debug_set_timer = 0; + pxmitpriv->amsdu_debug_timeout = 0; + pxmitpriv->amsdu_debug_coalesce_one = 0; + pxmitpriv->amsdu_debug_coalesce_two = 0; +#endif +#ifdef DBG_TXBD_DESC_DUMP + pxmitpriv->dump_txbd_desc = 0; +#endif + rtw_init_xmit_block(padapter); + rtw_hal_init_xmit_priv(padapter); + +exit: + + + return res; +} + +void rtw_mfree_xmit_priv_lock(struct xmit_priv *pxmitpriv); +void rtw_mfree_xmit_priv_lock(struct xmit_priv *pxmitpriv) +{ + _rtw_spinlock_free(&pxmitpriv->lock); + _rtw_free_sema(&pxmitpriv->xmit_sema); + + _rtw_spinlock_free(&pxmitpriv->be_pending.lock); + _rtw_spinlock_free(&pxmitpriv->bk_pending.lock); + _rtw_spinlock_free(&pxmitpriv->vi_pending.lock); + _rtw_spinlock_free(&pxmitpriv->vo_pending.lock); + _rtw_spinlock_free(&pxmitpriv->mgmt_pending.lock); + + /* _rtw_spinlock_free(&pxmitpriv->legacy_dz_queue.lock); */ + /* _rtw_spinlock_free(&pxmitpriv->apsd_queue.lock); */ + + _rtw_spinlock_free(&pxmitpriv->free_xmit_queue.lock); + _rtw_spinlock_free(&pxmitpriv->free_xmitbuf_queue.lock); + _rtw_spinlock_free(&pxmitpriv->pending_xmitbuf_queue.lock); +} + + +void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv) +{ + int i; + _adapter *padapter = pxmitpriv->adapter; + struct xmit_frame *pxmitframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf; + struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf; + + + rtw_hal_free_xmit_priv(padapter); + + rtw_mfree_xmit_priv_lock(pxmitpriv); + + if (pxmitpriv->pxmit_frame_buf == NULL) + goto out; + + for (i = 0; i < NR_XMITFRAME; i++) { + rtw_os_xmit_complete(padapter, pxmitframe); + + pxmitframe++; + } + + for (i = 0; i < NR_XMITBUFF; i++) { + rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), _TRUE); + + pxmitbuf++; + } + + if (pxmitpriv->pallocated_frame_buf) + rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4); + + + if (pxmitpriv->pallocated_xmitbuf) + rtw_vmfree(pxmitpriv->pallocated_xmitbuf, NR_XMITBUFF * sizeof(struct xmit_buf) + 4); + + /* free xframe_ext queue, the same count as extbuf */ + if ((pxmitframe = (struct xmit_frame *)pxmitpriv->xframe_ext)) { + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + rtw_os_xmit_complete(padapter, pxmitframe); + pxmitframe++; + } + } + if (pxmitpriv->xframe_ext_alloc_addr) + rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4); + _rtw_spinlock_free(&pxmitpriv->free_xframe_ext_queue.lock); + + /* free xmit extension buff */ + _rtw_spinlock_free(&pxmitpriv->free_xmit_extbuf_queue.lock); +#ifdef CONFIG_LAYER2_ROAMING + _rtw_spinlock_free(&pxmitpriv->rpkt_queue.lock); +#endif + pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf; + for (i = 0; i < NR_XMIT_EXTBUFF; i++) { + rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ), _TRUE); + + pxmitbuf++; + } + + if (pxmitpriv->pallocated_xmit_extbuf) + rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4); + + for (i = 0; i < CMDBUF_MAX; i++) { + pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i]; + if (pxmitbuf != NULL) + rtw_os_xmit_resource_free(padapter, pxmitbuf, MAX_CMDBUF_SZ + XMITBUF_ALIGN_SZ , _TRUE); + } + + rtw_free_hwxmits(padapter); + +#ifdef CONFIG_XMIT_ACK + _rtw_mutex_free(&pxmitpriv->ack_tx_mutex); +#endif + rtw_free_xmit_block(padapter); +out: + return; +} + +u8 rtw_get_tx_bw_mode(_adapter *adapter, struct sta_info *sta) +{ + u8 bw; + + bw = sta->cmn.bw_mode; + if (MLME_STATE(adapter) & WIFI_ASOC_STATE) { + if (adapter->mlmeextpriv.cur_channel <= 14) + bw = rtw_min(bw, ADAPTER_TX_BW_2G(adapter)); + else + bw = rtw_min(bw, ADAPTER_TX_BW_5G(adapter)); + } + + return bw; +} + +void rtw_get_adapter_tx_rate_bmp_by_bw(_adapter *adapter, u8 bw, u16 *r_bmp_cck_ofdm, u32 *r_bmp_ht, u64 *r_bmp_vht) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + u8 fix_bw = 0xFF; + u16 bmp_cck_ofdm = 0; + u32 bmp_ht = 0; + u64 bmp_vht = 0; + int i; + + if (adapter->fix_rate != 0xFF && adapter->fix_bw != 0xFF) + fix_bw = adapter->fix_bw; + + /* TODO: adapter->fix_rate */ + + for (i = 0; i < macid_ctl->num; i++) { + if (!rtw_macid_is_used(macid_ctl, i)) + continue; + if (!rtw_macid_is_iface_specific(macid_ctl, i, adapter)) + continue; + + if (bw == CHANNEL_WIDTH_20) /* CCK, OFDM always 20MHz */ + bmp_cck_ofdm |= macid_ctl->rate_bmp0[i] & 0x00000FFF; + + /* bypass mismatch bandwidth for HT, VHT */ + if ((fix_bw != 0xFF && fix_bw != bw) || (fix_bw == 0xFF && macid_ctl->bw[i] != bw)) + continue; + + if (macid_ctl->vht_en[i]) + bmp_vht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20); + else + bmp_ht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20); + } + + /* TODO: mlmeext->tx_rate*/ + + if (r_bmp_cck_ofdm) + *r_bmp_cck_ofdm = bmp_cck_ofdm; + if (r_bmp_ht) + *r_bmp_ht = bmp_ht; + if (r_bmp_vht) + *r_bmp_vht = bmp_vht; +} + +void rtw_get_shared_macid_tx_rate_bmp_by_bw(struct dvobj_priv *dvobj, u8 bw, u16 *r_bmp_cck_ofdm, u32 *r_bmp_ht, u64 *r_bmp_vht) +{ + struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj); + u16 bmp_cck_ofdm = 0; + u32 bmp_ht = 0; + u64 bmp_vht = 0; + int i; + + for (i = 0; i < macid_ctl->num; i++) { + if (!rtw_macid_is_used(macid_ctl, i)) + continue; + if (!rtw_macid_is_iface_shared(macid_ctl, i)) + continue; + + if (bw == CHANNEL_WIDTH_20) /* CCK, OFDM always 20MHz */ + bmp_cck_ofdm |= macid_ctl->rate_bmp0[i] & 0x00000FFF; + + /* bypass mismatch bandwidth for HT, VHT */ + if (macid_ctl->bw[i] != bw) + continue; + + if (macid_ctl->vht_en[i]) + bmp_vht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20); + else + bmp_ht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20); + } + + if (r_bmp_cck_ofdm) + *r_bmp_cck_ofdm = bmp_cck_ofdm; + if (r_bmp_ht) + *r_bmp_ht = bmp_ht; + if (r_bmp_vht) + *r_bmp_vht = bmp_vht; +} + +void rtw_get_adapter_tx_rate_bmp(_adapter *adapter, u16 r_bmp_cck_ofdm[], u32 r_bmp_ht[], u64 r_bmp_vht[]) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + u8 bw; + u16 bmp_cck_ofdm, tmp_cck_ofdm; + u32 bmp_ht, tmp_ht; + u64 bmp_vht, tmp_vht; + int i; + + for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) { + bmp_cck_ofdm = bmp_ht = bmp_vht = 0; + if (hal_is_bw_support(adapter, bw)) { + { + rtw_get_adapter_tx_rate_bmp_by_bw(adapter, bw, &tmp_cck_ofdm, &tmp_ht, &tmp_vht); + bmp_cck_ofdm |= tmp_cck_ofdm; + bmp_ht |= tmp_ht; + bmp_vht |= tmp_vht; + } + rtw_get_shared_macid_tx_rate_bmp_by_bw(dvobj, bw, &tmp_cck_ofdm, &tmp_ht, &tmp_vht); + bmp_cck_ofdm |= tmp_cck_ofdm; + bmp_ht |= tmp_ht; + bmp_vht |= tmp_vht; + } + if (bw == CHANNEL_WIDTH_20) + r_bmp_cck_ofdm[bw] = bmp_cck_ofdm; + if (bw <= CHANNEL_WIDTH_40) + r_bmp_ht[bw] = bmp_ht; + if (bw <= CHANNEL_WIDTH_160) + r_bmp_vht[bw] = bmp_vht; + } +} + +void rtw_update_tx_rate_bmp(struct dvobj_priv *dvobj) +{ + struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter); + u8 bw; + u16 bmp_cck_ofdm, tmp_cck_ofdm; + u32 bmp_ht, tmp_ht, ori_bmp_ht[2]; + u64 bmp_vht, tmp_vht, ori_bmp_vht[4]; + int i; + + for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) { + /* backup the original ht & vht bmp */ + if (bw <= CHANNEL_WIDTH_40) + ori_bmp_ht[bw] = rf_ctl->rate_bmp_ht_by_bw[bw]; + if (bw <= CHANNEL_WIDTH_160) + ori_bmp_vht[bw] = rf_ctl->rate_bmp_vht_by_bw[bw]; + + bmp_cck_ofdm = bmp_ht = bmp_vht = 0; + if (hal_is_bw_support(dvobj_get_primary_adapter(dvobj), bw)) { + for (i = 0; i < dvobj->iface_nums; i++) { + if (!dvobj->padapters[i]) + continue; + rtw_get_adapter_tx_rate_bmp_by_bw(dvobj->padapters[i], bw, &tmp_cck_ofdm, &tmp_ht, &tmp_vht); + bmp_cck_ofdm |= tmp_cck_ofdm; + bmp_ht |= tmp_ht; + bmp_vht |= tmp_vht; + } + rtw_get_shared_macid_tx_rate_bmp_by_bw(dvobj, bw, &tmp_cck_ofdm, &tmp_ht, &tmp_vht); + bmp_cck_ofdm |= tmp_cck_ofdm; + bmp_ht |= tmp_ht; + bmp_vht |= tmp_vht; + } + if (bw == CHANNEL_WIDTH_20) + rf_ctl->rate_bmp_cck_ofdm = bmp_cck_ofdm; + if (bw <= CHANNEL_WIDTH_40) + rf_ctl->rate_bmp_ht_by_bw[bw] = bmp_ht; + if (bw <= CHANNEL_WIDTH_160) + rf_ctl->rate_bmp_vht_by_bw[bw] = bmp_vht; + } + +#if CONFIG_TXPWR_LIMIT +#ifndef DBG_HIGHEST_RATE_BMP_BW_CHANGE +#define DBG_HIGHEST_RATE_BMP_BW_CHANGE 0 +#endif + + if (hal_data->txpwr_limit_loaded) { + u8 ori_highest_ht_rate_bw_bmp; + u8 ori_highest_vht_rate_bw_bmp; + u8 highest_rate_bw; + u8 highest_rate_bw_bmp; + u8 update_ht_rs = _FALSE; + u8 update_vht_rs = _FALSE; + + /* backup the original ht & vht highest bw bmp */ + ori_highest_ht_rate_bw_bmp = rf_ctl->highest_ht_rate_bw_bmp; + ori_highest_vht_rate_bw_bmp = rf_ctl->highest_vht_rate_bw_bmp; + + highest_rate_bw_bmp = BW_CAP_20M; + highest_rate_bw = CHANNEL_WIDTH_20; + for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_40; bw++) { + if (rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw] < rf_ctl->rate_bmp_ht_by_bw[bw]) { + highest_rate_bw_bmp = ch_width_to_bw_cap(bw); + highest_rate_bw = bw; + } else if (rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw] == rf_ctl->rate_bmp_ht_by_bw[bw]) + highest_rate_bw_bmp |= ch_width_to_bw_cap(bw); + } + rf_ctl->highest_ht_rate_bw_bmp = highest_rate_bw_bmp; + + if (ori_highest_ht_rate_bw_bmp != rf_ctl->highest_ht_rate_bw_bmp + || largest_bit(ori_bmp_ht[highest_rate_bw]) != largest_bit(rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw]) + ) { + if (DBG_HIGHEST_RATE_BMP_BW_CHANGE) { + RTW_INFO("highest_ht_rate_bw_bmp:0x%02x=>0x%02x\n", ori_highest_ht_rate_bw_bmp, rf_ctl->highest_ht_rate_bw_bmp); + RTW_INFO("rate_bmp_ht_by_bw[%u]:0x%08x=>0x%08x\n", highest_rate_bw, ori_bmp_ht[highest_rate_bw], rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw]); + } + if (rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw]) + update_ht_rs = _TRUE; + } + + highest_rate_bw_bmp = BW_CAP_20M; + highest_rate_bw = CHANNEL_WIDTH_20; + for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) { + if (rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw] < rf_ctl->rate_bmp_vht_by_bw[bw]) { + highest_rate_bw_bmp = ch_width_to_bw_cap(bw); + highest_rate_bw = bw; + } else if (rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw] == rf_ctl->rate_bmp_vht_by_bw[bw]) + highest_rate_bw_bmp |= ch_width_to_bw_cap(bw); + } + rf_ctl->highest_vht_rate_bw_bmp = highest_rate_bw_bmp; + + if (ori_highest_vht_rate_bw_bmp != rf_ctl->highest_vht_rate_bw_bmp + || largest_bit_64(ori_bmp_vht[highest_rate_bw]) != largest_bit_64(rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw]) + ) { + if (DBG_HIGHEST_RATE_BMP_BW_CHANGE) { + RTW_INFO("highest_vht_rate_bw_bmp:0x%02x=>0x%02x\n", ori_highest_vht_rate_bw_bmp, rf_ctl->highest_vht_rate_bw_bmp); + RTW_INFO("rate_bmp_vht_by_bw[%u]:0x%016llx=>0x%016llx\n", highest_rate_bw, ori_bmp_vht[highest_rate_bw], rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw]); + } + if (rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw]) + update_vht_rs = _TRUE; + } + + /* TODO: per rfpath and rate section handling? */ + if (update_ht_rs == _TRUE || update_vht_rs == _TRUE) + rtw_hal_update_txpwr_level(adapter); + } +#endif /* CONFIG_TXPWR_LIMIT */ +} + +u8 rtw_get_tx_bw_bmp_of_ht_rate(struct dvobj_priv *dvobj, u8 rate, u8 max_bw) +{ + struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj); + u8 bw; + u8 bw_bmp = 0; + u32 rate_bmp; + + if (!IS_HT_RATE(rate)) { + rtw_warn_on(1); + goto exit; + } + + rate_bmp = 1 << (rate - MGN_MCS0); + + if (max_bw > CHANNEL_WIDTH_40) + max_bw = CHANNEL_WIDTH_40; + + for (bw = CHANNEL_WIDTH_20; bw <= max_bw; bw++) { + /* RA may use lower rate for retry */ + if (rf_ctl->rate_bmp_ht_by_bw[bw] >= rate_bmp) + bw_bmp |= ch_width_to_bw_cap(bw); + } + +exit: + return bw_bmp; +} + +u8 rtw_get_tx_bw_bmp_of_vht_rate(struct dvobj_priv *dvobj, u8 rate, u8 max_bw) +{ + struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj); + u8 bw; + u8 bw_bmp = 0; + u64 rate_bmp; + + if (!IS_VHT_RATE(rate)) { + rtw_warn_on(1); + goto exit; + } + + rate_bmp = BIT_ULL(rate - MGN_VHT1SS_MCS0); + + if (max_bw > CHANNEL_WIDTH_160) + max_bw = CHANNEL_WIDTH_160; + + for (bw = CHANNEL_WIDTH_20; bw <= max_bw; bw++) { + /* RA may use lower rate for retry */ + if (rf_ctl->rate_bmp_vht_by_bw[bw] >= rate_bmp) + bw_bmp |= ch_width_to_bw_cap(bw); + } + +exit: + return bw_bmp; +} + +s16 rtw_adapter_get_oper_txpwr_max_mbm(_adapter *adapter, bool eirp) +{ + s16 mbm = -100 * MBM_PDBM; + + if (MLME_IS_ASOC(adapter)) { + struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv; + u8 ch = mlmeext->cur_channel; + u8 bw = mlmeext->cur_bwmode; + u8 offset = mlmeext->cur_ch_offset; + u8 cch = rtw_get_center_ch(ch, bw, offset); + u8 hw_rate = MRateToHwRate(mlmeext->tx_rate); + u16 bmp_cck_ofdm_by_bw[1] = {0}; + u32 bmp_ht_by_bw[2] = {0}; + u64 bmp_vht_by_bw[4] = {0}; + u16 bmp_cck_ofdm = 0; + u32 bmp_ht = 0; + u64 bmp_vht = 0; + int i; + + rtw_get_adapter_tx_rate_bmp(adapter, bmp_cck_ofdm_by_bw, bmp_ht_by_bw, bmp_vht_by_bw); + + bmp_cck_ofdm |= bmp_cck_ofdm_by_bw[0]; + for (i = 0; i < 2; i++) + bmp_ht |= bmp_ht_by_bw[i]; + for (i = 0; i < 4; i++) + bmp_vht |= bmp_vht_by_bw[i]; + + if (IS_LEGACY_HRATE(hw_rate)) + bmp_cck_ofdm |= BIT(hw_rate); + else if (IS_HT_HRATE(hw_rate)) + bmp_ht |= BIT(hw_rate - DESC_RATEMCS0); + else if (IS_VHT_HRATE(hw_rate)) + bmp_vht |= BIT(hw_rate - DESC_RATEVHTSS1MCS0); + + mbm = phy_get_txpwr_total_max_mbm(adapter + , bw, cch, ch, bmp_cck_ofdm, bmp_ht, bmp_vht, 0, eirp); + } + + return mbm; +} + +s16 rtw_rfctl_get_oper_txpwr_max_mbm(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset, u8 ifbmp_mod, u8 if_op, bool eirp) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + s16 mbm = -100 * MBM_PDBM; + + if (ch) { + u8 cch = rtw_get_center_ch(ch, bw, offset); + u16 bmp_cck_ofdm = 0; + u32 bmp_ht = 0; + u64 bmp_vht = 0; + int i; + + for (i = 0; i < dvobj->iface_nums; i++) { + struct mlme_ext_priv *mlmeext; + u8 hw_rate; + + if (!dvobj->padapters[i]) + continue; + + if (ifbmp_mod & BIT(i)) { + if (!if_op) + continue; + } else if (!MLME_IS_ASOC(dvobj->padapters[i])) + continue; + + mlmeext = &(dvobj->padapters[i]->mlmeextpriv); + hw_rate = MRateToHwRate(mlmeext->tx_rate); + + if (IS_LEGACY_HRATE(hw_rate)) + bmp_cck_ofdm |= BIT(hw_rate); + else if (IS_HT_HRATE(hw_rate)) + bmp_ht |= BIT(hw_rate - DESC_RATEMCS0); + else if (IS_VHT_HRATE(hw_rate)) + bmp_vht |= BIT(hw_rate - DESC_RATEVHTSS1MCS0); + } + + bmp_cck_ofdm |= rfctl->rate_bmp_cck_ofdm; + for (i = 0; i < 2; i++) + bmp_ht |= rfctl->rate_bmp_ht_by_bw[i]; + for (i = 0; i < 4; i++) + bmp_vht |= rfctl->rate_bmp_vht_by_bw[i]; + + mbm = phy_get_txpwr_total_max_mbm(adapter + , bw, cch, ch, bmp_cck_ofdm, bmp_ht, bmp_vht, 0, eirp); + } + + return mbm; +} + +s16 rtw_get_oper_txpwr_max_mbm(struct dvobj_priv *dvobj, bool eirp) +{ + struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + s16 mbm = -100 * MBM_PDBM; + u8 ch = rfctl->op_ch, bw, offset; + + if (rtw_get_bw_offset_by_op_class_ch(rfctl->op_class, ch, &bw, &offset)) + mbm = rtw_rfctl_get_oper_txpwr_max_mbm(rfctl, ch, bw, offset, 0, 0, eirp); + + return mbm; +} + +s16 rtw_rfctl_get_reg_max_txpwr_mbm(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset, bool eirp) +{ + struct dvobj_priv *dvobj = rfctl_to_dvobj(rfctl); + struct registry_priv *regsty = dvobj_to_regsty(dvobj); + _adapter *adapter = dvobj_get_primary_adapter(dvobj); + s16 mbm = -100 * MBM_PDBM; + u8 cch = rtw_get_center_ch(ch, bw, offset); + u16 bmp_cck_ofdm = 0; + u32 bmp_ht = 0; + u64 bmp_vht = 0; + + if (ch <= 14) + bmp_cck_ofdm |= RATE_BMP_CCK; + + /* TODO: NO OFDM? */ + bmp_cck_ofdm |= RATE_BMP_OFDM; + +#ifdef CONFIG_80211N_HT + if (regsty->ht_enable && is_supported_ht(regsty->wireless_mode)) { + switch (GET_HAL_TX_NSS(adapter)) { + case 1: + bmp_ht |= RATE_BMP_HT_1SS; + break; + case 2: + bmp_ht |= RATE_BMP_HT_2SS | RATE_BMP_HT_1SS; + break; + case 3: + bmp_ht |= RATE_BMP_HT_3SS | RATE_BMP_HT_2SS | RATE_BMP_HT_1SS; + break; + case 4: + bmp_ht |= RATE_BMP_HT_4SS | RATE_BMP_HT_3SS | RATE_BMP_HT_2SS | RATE_BMP_HT_1SS; + break; + default: + rtw_warn_on(1); + } + } +#endif + +#ifdef CONFIG_80211AC_VHT + if (ch > 14 && REGSTY_IS_11AC_ENABLE(regsty) && is_supported_vht(regsty->wireless_mode) + && (!rfctl->country_ent || COUNTRY_CHPLAN_EN_11AC(rfctl->country_ent)) + ) { + switch (GET_HAL_TX_NSS(adapter)) { + case 1: + bmp_vht |= RATE_BMP_VHT_1SS; + break; + case 2: + bmp_vht |= RATE_BMP_VHT_2SS | RATE_BMP_VHT_1SS; + break; + case 3: + bmp_vht |= RATE_BMP_VHT_3SS | RATE_BMP_VHT_2SS | RATE_BMP_VHT_1SS; + break; + case 4: + bmp_vht |= RATE_BMP_VHT_4SS | RATE_BMP_VHT_3SS | RATE_BMP_VHT_2SS | RATE_BMP_VHT_1SS; + break; + default: + rtw_warn_on(1); + } + } +#endif + + mbm = phy_get_txpwr_total_max_mbm(adapter + , bw, cch, ch, bmp_cck_ofdm, bmp_ht, bmp_vht, 1, eirp); + + return mbm; +} + +u8 query_ra_short_GI(struct sta_info *psta, u8 bw) +{ + u8 sgi = _FALSE, sgi_20m = _FALSE, sgi_40m = _FALSE, sgi_80m = _FALSE; + +#ifdef CONFIG_80211N_HT +#ifdef CONFIG_80211AC_VHT + if (psta->vhtpriv.vht_option) + sgi_80m = psta->vhtpriv.sgi_80m; +#endif + sgi_20m = psta->htpriv.sgi_20m; + sgi_40m = psta->htpriv.sgi_40m; +#endif + + switch (bw) { + case CHANNEL_WIDTH_80: + sgi = sgi_80m; + break; + case CHANNEL_WIDTH_40: + sgi = sgi_40m; + break; + case CHANNEL_WIDTH_20: + default: + sgi = sgi_20m; + break; + } + + return sgi; +} + +/* This function references driver insmond parameters to decide vcs mode. */ +/* Driver insmond parameters: rtw_vrtl_carrier_sense and rtw_vcs_type */ +static u8 validate_vcs(_adapter *padapter, u8 mode) { + + u8 vcs_mode = NONE_VCS; + + switch(padapter->registrypriv.vrtl_carrier_sense) { + + case DISABLE_VCS: + vcs_mode = NONE_VCS; + break; + + case ENABLE_VCS: + vcs_mode = padapter->registrypriv.vcs_type; + break; + + case AUTO_VCS: + vcs_mode = mode; + break; + + default: + vcs_mode = NONE_VCS; + break; + } + + return vcs_mode; + +} + +static void update_attrib_vcs_info(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + u32 sz; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); + #ifdef RTW_FORCE_CTS_TO_SELF_UNDER_LOW_RSSI + s8 rssi = 0; + struct sta_info *psta = pattrib->psta; + #endif + /* + if(pattrib->psta) + { + psta = pattrib->psta; + } + else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] ); + } + + if(psta==NULL) + { + RTW_INFO("%s, psta==NUL\n", __func__); + return; + } + + if(!(psta->state &WIFI_ASOC_STATE)) + { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return; + } + */ + + if (pattrib->nr_frags != 1) + sz = padapter->xmitpriv.frag_len; + else /* no frag */ + sz = pattrib->last_txcmdsz; + + /* (1) RTS_Threshold is compared to the MPDU, not MSDU. */ + /* (2) If there are more than one frag in this MSDU, only the first frag uses protection frame. */ + /* Other fragments are protected by previous fragment. */ + /* So we only need to check the length of first fragment. */ + if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N || padapter->registrypriv.wifi_spec) { + if (sz > padapter->registrypriv.rts_thresh) + pattrib->vcs_mode = RTS_CTS; + else { + if (pattrib->rtsen) + pattrib->vcs_mode = RTS_CTS; + else if (pattrib->cts2self) + pattrib->vcs_mode = CTS_TO_SELF; + else + pattrib->vcs_mode = NONE_VCS; + } + } else { + while (_TRUE) { +#if 0 /* Todo */ + /* check IOT action */ + if (pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF) { + pattrib->vcs_mode = CTS_TO_SELF; + pattrib->rts_rate = MGN_24M; + break; + } else if (pHTInfo->IOTAction & (HT_IOT_ACT_FORCED_RTS | HT_IOT_ACT_PURE_N_MODE)) { + pattrib->vcs_mode = RTS_CTS; + pattrib->rts_rate = MGN_24M; + break; + } +#endif + + /* IOT action */ + if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && (pattrib->ampdu_en == _TRUE) && + (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) { + pattrib->vcs_mode = CTS_TO_SELF; + break; + } + + /* check ERP protection */ + if (pattrib->rtsen || pattrib->cts2self) { + if (pattrib->rtsen) + pattrib->vcs_mode = RTS_CTS; + else if (pattrib->cts2self) + pattrib->vcs_mode = CTS_TO_SELF; + + break; + } + + /* check HT op mode */ + if (pattrib->ht_en) { + u8 HTOpMode = pmlmeinfo->HT_protection; + if ((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) || + (!pmlmeext->cur_bwmode && HTOpMode == 3)) { + pattrib->vcs_mode = RTS_CTS; + break; + } + } + + /* check rts */ + if (sz > padapter->registrypriv.rts_thresh) { + pattrib->vcs_mode = RTS_CTS; + break; + } + + /* to do list: check MIMO power save condition. */ + + /* check AMPDU aggregation for TXOP */ + if ((pattrib->ampdu_en == _TRUE) && (!IS_HARDWARE_TYPE_8812(padapter))) { + pattrib->vcs_mode = RTS_CTS; + break; + } + + pattrib->vcs_mode = NONE_VCS; + break; + } + #ifdef RTW_FORCE_CTS_TO_SELF_UNDER_LOW_RSSI + /*RTStoCTS while let TP degree ,while enable full BW*/ + if (psta != NULL) { + rssi = psta->cmn.rssi_stat.rssi; + if ((rssi < 18) && (pattrib->vcs_mode == RTS_CTS)) + pattrib->vcs_mode = CTS_TO_SELF; + } + #endif + } + + pattrib->vcs_mode = validate_vcs(padapter, pattrib->vcs_mode); + + /* for debug : force driver control vrtl_carrier_sense. */ + if (padapter->driver_vcs_en == 1) { + /* u8 driver_vcs_en; */ /* Enable=1, Disable=0 driver control vrtl_carrier_sense. */ + /* u8 driver_vcs_type; */ /* force 0:disable VCS, 1:RTS-CTS, 2:CTS-to-self when vcs_en=1. */ + pattrib->vcs_mode = padapter->driver_vcs_type; + } + +} + +#ifdef CONFIG_WMMPS_STA +/* + * update_attrib_trigger_frame_info + * For Station mode, if a specific TID of driver setting and an AP support uapsd function, the data + * frame with corresponding TID will be a trigger frame when driver is in wmm power saving mode. + * + * Arguments: + * @padapter: _adapter pointer. + * @pattrib: pkt_attrib pointer. + * + * Auther: Arvin Liu + * Date: 2017/06/05 + */ +static void update_attrib_trigger_frame_info(_adapter *padapter, struct pkt_attrib *pattrib) { + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter); + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + u8 trigger_frame_en = 0; + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE) { + if ((pwrpriv->pwr_mode == PS_MODE_MIN) || (pwrpriv->pwr_mode == PS_MODE_MAX)) { + if((pqospriv->uapsd_ap_supported) && ((pqospriv->uapsd_tid & BIT(pattrib->priority)) == _TRUE)) { + trigger_frame_en = 1; + RTW_INFO("[WMMPS]"FUNC_ADPT_FMT": This is a Trigger Frame\n", FUNC_ADPT_ARG(padapter)); + } + } + } + + pattrib->trigger_frame = trigger_frame_en; +} +#endif /* CONFIG_WMMPS_STA */ + +static void update_attrib_phy_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta) +{ + struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv; + u8 bw; + + pattrib->rtsen = psta->rtsen; + pattrib->cts2self = psta->cts2self; + + pattrib->mdata = 0; + pattrib->eosp = 0; + pattrib->triggered = 0; + pattrib->ampdu_spacing = 0; + + /* ht_en, init rate, ,bw, ch_offset, sgi */ + + pattrib->raid = psta->cmn.ra_info.rate_id; + + bw = rtw_get_tx_bw_mode(padapter, psta); + pattrib->bwmode = rtw_min(bw, mlmeext->cur_bwmode); + pattrib->sgi = query_ra_short_GI(psta, pattrib->bwmode); + + pattrib->ldpc = psta->cmn.ldpc_en; + pattrib->stbc = psta->cmn.stbc_en; + +#ifdef CONFIG_80211N_HT + if(padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode)) { + pattrib->ht_en = psta->htpriv.ht_option; + pattrib->ch_offset = psta->htpriv.ch_offset; + pattrib->ampdu_en = _FALSE; + + if (padapter->driver_ampdu_spacing != 0xFF) /* driver control AMPDU Density for peer sta's rx */ + pattrib->ampdu_spacing = padapter->driver_ampdu_spacing; + else + pattrib->ampdu_spacing = psta->htpriv.rx_ampdu_min_spacing; + + /* check if enable ampdu */ + if (pattrib->ht_en && psta->htpriv.ampdu_enable) { + if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) { + pattrib->ampdu_en = _TRUE; + if (psta->htpriv.tx_amsdu_enable == _TRUE) + pattrib->amsdu_ampdu_en = _TRUE; + else + pattrib->amsdu_ampdu_en = _FALSE; + } + } + } +#endif /* CONFIG_80211N_HT */ + /* if(pattrib->ht_en && psta->htpriv.ampdu_enable) */ + /* { */ + /* if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) */ + /* pattrib->ampdu_en = _TRUE; */ + /* } */ + +#ifdef CONFIG_TDLS + if (pattrib->direct_link == _TRUE) { + psta = pattrib->ptdls_sta; + + pattrib->raid = psta->cmn.ra_info.rate_id; +#ifdef CONFIG_80211N_HT + if(padapter->registrypriv.ht_enable && + is_supported_ht(padapter->registrypriv.wireless_mode)) { + pattrib->bwmode = rtw_get_tx_bw_mode(padapter, psta); + pattrib->ht_en = psta->htpriv.ht_option; + pattrib->ch_offset = psta->htpriv.ch_offset; + pattrib->sgi = query_ra_short_GI(psta, pattrib->bwmode); + } +#endif /* CONFIG_80211N_HT */ + } +#endif /* CONFIG_TDLS */ + + pattrib->retry_ctrl = _FALSE; +} + +static s32 update_attrib_sec_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta, enum eap_type eapol_type) +{ + sint res = _SUCCESS; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + sint bmcast = IS_MCAST(pattrib->ra); + + _rtw_memset(pattrib->dot118021x_UncstKey.skey, 0, 16); + _rtw_memset(pattrib->dot11tkiptxmickey.skey, 0, 16); + pattrib->mac_id = psta->cmn.mac_id; + + /* Comment by Owen at 2020/05/19 + * Issue: RTK STA sends encrypted 4-way 4/4 when AP thinks the 4-way incomplete + * In TCL pressure test, AP may resend 4-way 3/4 with new replay counter in 2 ms. + * In this situation, STA sends unencrypted 4-way 4/4 with old replay counter after more + * than 2 ms, followed by the encrypted 4-way 4/4 with new replay counter. Because the + * AP only accepts unencrypted 4-way 4/4 with a new play counter, and the STA encrypts + * each 4-way 4/4 at this time, the 4-way handshake cannot be completed. + * So we modified that after STA receives unencrypted 4-way 1/4 and 4-way 3/4, + * 4-way 2/4 and 4-way 4/4 sent by STA in the next 100 ms are not encrypted. + */ + if (psta->ieee8021x_blocked == _TRUE || + ((eapol_type == EAPOL_2_4 || eapol_type == EAPOL_4_4) && + rtw_get_passing_time_ms(psta->resp_nonenc_eapol_key_starttime) <= 100)) { + + if (eapol_type == EAPOL_2_4 || eapol_type == EAPOL_4_4) + RTW_INFO("Respond unencrypted eapol key\n"); + + pattrib->encrypt = 0; + + if ((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _FALSE)) { +#ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == _TRUE, pattrib->ether_type(%04x) != 0x888e\n", __FUNCTION__, pattrib->ether_type); +#endif + res = _FAIL; + goto exit; + } + } else { + GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast); + +#ifdef CONFIG_WAPI_SUPPORT + if (pattrib->ether_type == 0x88B4) + pattrib->encrypt = _NO_PRIVACY_; +#endif + + switch (psecuritypriv->dot11AuthAlgrthm) { + case dot11AuthAlgrthm_Open: + case dot11AuthAlgrthm_Shared: + case dot11AuthAlgrthm_Auto: + pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex; + break; + case dot11AuthAlgrthm_8021X: + if (bmcast) + pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid; + else + pattrib->key_idx = 0; + break; + default: + pattrib->key_idx = 0; + break; + } + + /* For WPS 1.0 WEP, driver should not encrypt EAPOL Packet for WPS handshake. */ + if (((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) && (pattrib->ether_type == 0x888e)) + pattrib->encrypt = _NO_PRIVACY_; + + } + +#ifdef CONFIG_TDLS + if (pattrib->direct_link == _TRUE) { + if (pattrib->encrypt > 0) + pattrib->encrypt = _AES_; + } +#endif + + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + pattrib->iv_len = 4; + pattrib->icv_len = 4; + WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + break; + + case _TKIP_: + pattrib->iv_len = 8; + pattrib->icv_len = 4; + + if (psecuritypriv->busetkipkey == _FAIL) { +#ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s psecuritypriv->busetkipkey(%d)==_FAIL drop packet\n", __FUNCTION__, psecuritypriv->busetkipkey); +#endif + res = _FAIL; + goto exit; + } + + if (bmcast) + TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + TKIP_IV(pattrib->iv, psta->dot11txpn, 0); + + + _rtw_memcpy(pattrib->dot11tkiptxmickey.skey, psta->dot11tkiptxmickey.skey, 16); + + break; + + case _AES_: + + pattrib->iv_len = 8; + pattrib->icv_len = 8; + + if (bmcast) + AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + AES_IV(pattrib->iv, psta->dot11txpn, 0); + + break; + + case _GCMP_: + case _GCMP_256_: + + pattrib->iv_len = 8; + pattrib->icv_len = 16; + + if (bmcast) + GCMP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + GCMP_IV(pattrib->iv, psta->dot11txpn, 0); + + break; + + case _CCMP_256_: + + pattrib->iv_len = 8; + pattrib->icv_len = 16; + + if (bmcast) + GCMP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + GCMP_IV(pattrib->iv, psta->dot11txpn, 0); + + break; + +#ifdef CONFIG_WAPI_SUPPORT + case _SMS4_: + pattrib->iv_len = 18; + pattrib->icv_len = 16; + rtw_wapi_get_iv(padapter, pattrib->ra, pattrib->iv); + break; +#endif + default: + pattrib->iv_len = 0; + pattrib->icv_len = 0; + break; + } + + if (pattrib->encrypt > 0) { + _rtw_memcpy(pattrib->dot118021x_UncstKey.skey + , psta->dot118021x_UncstKey.skey + , (pattrib->encrypt & _SEC_TYPE_256_) ? 32 : 16); + } + + + if (pattrib->encrypt && + ((padapter->securitypriv.sw_encrypt == _TRUE) || (psecuritypriv->hw_decrypted == _FALSE))) { + pattrib->bswenc = _TRUE; + } else { + pattrib->bswenc = _FALSE; + } + + pattrib->bmc_camid = padapter->securitypriv.dot118021x_bmc_cam_id; + + if (pattrib->encrypt && bmcast && _rtw_camctl_chk_flags(padapter, SEC_STATUS_STA_PK_GK_CONFLICT_DIS_BMC_SEARCH)) + pattrib->bswenc = _TRUE; + +#ifdef CONFIG_WAPI_SUPPORT + if (pattrib->encrypt == _SMS4_) + pattrib->bswenc = _FALSE; +#endif + + if ((pattrib->encrypt) && (eapol_type == EAPOL_4_4)) + pattrib->bswenc = _TRUE; + +exit: + + return res; + +} + +u8 qos_acm(u8 acm_mask, u8 priority) +{ + u8 change_priority = priority; + + switch (priority) { + case 0: + case 3: + if (acm_mask & BIT(1)) + change_priority = 1; + break; + case 1: + case 2: + break; + case 4: + case 5: + if (acm_mask & BIT(2)) + change_priority = 0; + break; + case 6: + case 7: + if (acm_mask & BIT(3)) + change_priority = 5; + break; + default: + RTW_INFO("qos_acm(): invalid pattrib->priority: %d!!!\n", priority); + break; + } + + return change_priority; +} + +/* refer to IEEE802.11-2016 Table R-3; Comply with IETF RFC4594 */ +static u8 tos_to_up(u8 tos) +{ + u8 up = 0; + u8 dscp; + u8 mode = CONFIG_RTW_UP_MAPPING_RULE; + + + /* tos precedence mapping */ + if (mode == 0) { + up = tos >> 5; + return up; + } + + /* refer to IEEE802.11-2016 Table R-3; + * DCSP 32(CS4) comply with IETF RFC4594 + */ + dscp = (tos >> 2); + + if ( dscp == 0 ) + up = 0; + else if ( dscp >= 1 && dscp <= 9) + up = 1; + else if ( dscp >= 10 && dscp <= 16) + up = 2; + else if ( dscp >= 17 && dscp <= 23) + up = 3; + else if ( dscp >= 24 && dscp <= 31) + up = 4; + else if ( dscp >= 33 && dscp <= 40) + up = 5; + else if ((dscp >= 41 && dscp <= 47) || (dscp == 32)) + up = 6; + else if ( dscp >= 48 && dscp <= 63) + up = 7; + + return up; +} + +static void set_qos(_pkt *pkt, struct pkt_attrib *pattrib) +{ + s32 UserPriority = 0; + + if (!pkt) + goto null_pkt; + + /* get UserPriority from IP hdr */ + if (pattrib->ether_type == 0x0800) { + struct pkt_file ppktfile; + struct ethhdr etherhdr; + struct iphdr ip_hdr; + + _rtw_open_pktfile(pkt, &ppktfile); + _rtw_pktfile_read(&ppktfile, (unsigned char *)ðerhdr, ETH_HLEN); + _rtw_pktfile_read(&ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr)); + /* UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */ + UserPriority = tos_to_up(ip_hdr.tos); + } + /* + else if (pattrib->ether_type == 0x888e) { + + + UserPriority = 7; + } + */ + + #ifdef CONFIG_ICMP_VOQ + if(pattrib->icmp_pkt==1)/*use VO queue to send icmp packet*/ + UserPriority = 7; + #endif + #ifdef CONFIG_IP_R_MONITOR + if (pattrib->ether_type == ETH_P_ARP) + UserPriority = 7; + #endif/*CONFIG_IP_R_MONITOR*/ + +null_pkt: + pattrib->priority = UserPriority; + pattrib->hdrlen = XATTRIB_GET_WDS(pattrib) ? WLAN_HDR_A4_QOS_LEN : WLAN_HDR_A3_QOS_LEN; + pattrib->subtype = WIFI_QOS_DATA_TYPE; +} + +#ifdef CONFIG_TDLS +u8 rtw_check_tdls_established(_adapter *padapter, struct pkt_attrib *pattrib) +{ + pattrib->ptdls_sta = NULL; + + pattrib->direct_link = _FALSE; + if (padapter->tdlsinfo.link_established == _TRUE) { + pattrib->ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst); +#if 1 + if ((pattrib->ptdls_sta != NULL) && + (pattrib->ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) && + (pattrib->ether_type != 0x0806)) { + pattrib->direct_link = _TRUE; + /* RTW_INFO("send ptk to "MAC_FMT" using direct link\n", MAC_ARG(pattrib->dst)); */ + } +#else + if (pattrib->ptdls_sta != NULL && + pattrib->ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) { + pattrib->direct_link = _TRUE; +#if 0 + RTW_INFO("send ptk to "MAC_FMT" using direct link\n", MAC_ARG(pattrib->dst)); +#endif + } + + /* ARP frame may be helped by AP*/ + if (pattrib->ether_type != 0x0806) + pattrib->direct_link = _FALSE; +#endif + } + + return pattrib->direct_link; +} + +s32 update_tdls_attrib(_adapter *padapter, struct pkt_attrib *pattrib) +{ + + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + + s32 res = _SUCCESS; + + psta = rtw_get_stainfo(pstapriv, pattrib->ra); + if (psta == NULL) { + res = _FAIL; + goto exit; + } + + pattrib->mac_id = psta->cmn.mac_id; + pattrib->psta = psta; + pattrib->ack_policy = 0; + /* get ether_hdr_len */ + pattrib->pkt_hdrlen = ETH_HLEN; + + pattrib->qos_en = psta->qos_option; + + /* [TDLS] TODO: setup req/rsp should be AC_BK */ + if (pqospriv->qos_option && psta->qos_option) { + pattrib->priority = 4; /* tdls management frame should be AC_VI */ + pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN; + pattrib->subtype = WIFI_QOS_DATA_TYPE; + } else { + pattrib->priority = 0; + pattrib->hdrlen = WLAN_HDR_A3_LEN; + pattrib->subtype = WIFI_DATA_TYPE; + } + + /* TODO:_lock */ + if (update_attrib_sec_info(padapter, pattrib, psta, NON_EAPOL) == _FAIL) { + res = _FAIL; + goto exit; + } + + update_attrib_phy_info(padapter, pattrib, psta); + + +exit: + + return res; +} + +#endif /* CONFIG_TDLS */ + +/*get non-qos hw_ssn control register,mapping to REG_HW_SEQ 0,1,2,3*/ +inline u8 rtw_get_hwseq_no(_adapter *padapter) +{ + u8 hwseq_num = 0; + +#ifdef CONFIG_CONCURRENT_MODE + #if defined(CONFIG_RTL8822B) || defined(CONFIG_RTL8821C) || defined(CONFIG_RTL8822C) || defined(CONFIG_RTL8814B) \ + || defined(CONFIG_RTL8723F) + hwseq_num = padapter->iface_id; + if (hwseq_num > 3) + hwseq_num = 3; + #else + if (!is_primary_adapter(padapter)) + hwseq_num = 1; + #endif +#endif /* CONFIG_CONCURRENT_MODE */ + return hwseq_num; +} +#ifdef CONFIG_LPS +#define LPS_PT_NORMAL 0 +#define LPS_PT_SP 1/* only DHCP packets is as SPECIAL_PACKET*/ +#define LPS_PT_ICMP 2 + +/*If EAPOL , ARP , OR DHCP packet, driver must be in active mode.*/ +static u8 _rtw_lps_chk_packet_type(struct pkt_attrib *pattrib) +{ + u8 pkt_type = LPS_PT_NORMAL; /*normal data frame*/ + + #ifdef CONFIG_WAPI_SUPPORT + if ((pattrib->ether_type == 0x88B4) || (pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1)) + pkt_type = LPS_PT_SP; + #else /* !CONFIG_WAPI_SUPPORT */ + + #ifndef CONFIG_LPS_NOT_LEAVE_FOR_ICMP + if (pattrib->icmp_pkt == 1) + pkt_type = LPS_PT_ICMP; + else + #endif + if (pattrib->dhcp_pkt == 1) + pkt_type = LPS_PT_SP; + #endif + return pkt_type; +} +#endif +static s32 update_attrib(_adapter *padapter, _pkt *pkt, struct pkt_attrib *pattrib) +{ + uint i; + struct pkt_file pktfile; + struct sta_info *psta = NULL; + struct ethhdr etherhdr; + + sint bmcast; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + sint res = _SUCCESS; + enum eap_type eapol_type = NON_EAPOL; +#ifdef CONFIG_LPS + u8 pkt_type = 0; +#endif + + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib); + + _rtw_open_pktfile(pkt, &pktfile); + i = _rtw_pktfile_read(&pktfile, (u8 *)ðerhdr, ETH_HLEN); + + pattrib->ether_type = ntohs(etherhdr.h_proto); + + if (MLME_STATE(padapter) & (WIFI_AP_STATE | WIFI_MESH_STATE)) /* address resolve is done for ap/mesh */ + goto get_sta_info; + + _rtw_memcpy(pattrib->dst, ðerhdr.h_dest, ETH_ALEN); + _rtw_memcpy(pattrib->src, ðerhdr.h_source, ETH_ALEN); + _rtw_memcpy(pattrib->ta, adapter_mac_addr(padapter), ETH_ALEN); + + if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) { + _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc); + } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) { + #ifdef CONFIG_TDLS + if (rtw_check_tdls_established(padapter, pattrib) == _TRUE) + _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN); /* For TDLS direct link Tx, set ra to be same to dst */ + else + #endif + { + _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN); + #ifdef CONFIG_RTW_WDS + if (adapter_use_wds(padapter) + && _rtw_memcmp(pattrib->src, pattrib->ta, ETH_ALEN) == _FALSE + ) { + pattrib->wds = 1; + if (IS_MCAST(pattrib->dst)) + rtw_tx_wds_gptr_update(padapter, pattrib->src); + } + #endif + } + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_sta); + } else + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_unknown); + +get_sta_info: + bmcast = IS_MCAST(pattrib->ra); + if (bmcast) { + psta = rtw_get_bcmc_stainfo(padapter); + if (psta == NULL) { /* if we cannot get psta => drop the pkt */ + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta); + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra)); + #endif + res = _FAIL; + goto exit; + } + } else { + psta = rtw_get_stainfo(pstapriv, pattrib->ra); + if (psta == NULL) { /* if we cannot get psta => drop the pkt */ + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta); + #ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra)); + #endif + res = _FAIL; + goto exit; + } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE && !(psta->state & WIFI_ASOC_STATE)) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link); + res = _FAIL; + goto exit; + } + + #ifdef CONFIG_RTW_WDS + if (XATTRIB_GET_WDS(pattrib) && !(psta->flags & WLAN_STA_WDS)) + pattrib->wds = 0; + #endif + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_link); + RTW_INFO("%s-"ADPT_FMT" psta("MAC_FMT")->state(0x%x) != WIFI_ASOC_STATE\n", + __func__, ADPT_ARG(padapter), MAC_ARG(psta->cmn.mac_addr), psta->state); + res = _FAIL; + goto exit; + } + + pattrib->pktlen = pktfile.pkt_len; + + /* TODO: 802.1Q VLAN header */ + /* TODO: IPV6 */ + + if (ETH_P_IP == pattrib->ether_type) { + u8 ip[20]; + + _rtw_pktfile_read(&pktfile, ip, 20); + + if (GET_IPV4_IHL(ip) * 4 > 20) + _rtw_pktfile_read(&pktfile, NULL, GET_IPV4_IHL(ip) - 20); + + pattrib->icmp_pkt = 0; + pattrib->dhcp_pkt = 0; + pattrib->hipriority_pkt = 0; + + if (GET_IPV4_PROTOCOL(ip) == 0x01) { /* ICMP */ + pattrib->icmp_pkt = 1; + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_icmp); + + } else if (GET_IPV4_PROTOCOL(ip) == 0x11) { /* UDP */ + u8 udp[24]; + + _rtw_pktfile_read(&pktfile, udp, 24); + + if ((GET_UDP_SRC(udp) == 68 && GET_UDP_DST(udp) == 67) + || (GET_UDP_SRC(udp) == 67 && GET_UDP_DST(udp) == 68) + ) { + /* 67 : UDP BOOTP server, 68 : UDP BOOTP client */ + if (pattrib->pktlen > 282) { /* MINIMUM_DHCP_PACKET_SIZE */ + pattrib->dhcp_pkt = 1; + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp); + if (0) + RTW_INFO("send DHCP packet\n"); + } + } + + /* WaveAgent packet, increase priority so that the system can read data in time */ + if (((GET_UDP_SIG1(udp) == 0xcc) || (GET_UDP_SIG1(udp) == 0xdd)) && + (GET_UDP_SIG2(udp) == 0xe2)) { + pattrib->hipriority_pkt = 1; + } + + } else if (GET_IPV4_PROTOCOL(ip) == 0x06 /* TCP */ + && rtw_st_ctl_chk_reg_s_proto(&psta->st_ctl, 0x06) == _TRUE + ) { + u8 tcp[20]; + + _rtw_pktfile_read(&pktfile, tcp, 20); + + if (rtw_st_ctl_chk_reg_rule(&psta->st_ctl, padapter, IPV4_SRC(ip), TCP_SRC(tcp), IPV4_DST(ip), TCP_DST(tcp)) == _TRUE) { + if (GET_TCP_SYN(tcp) && GET_TCP_ACK(tcp)) { + session_tracker_add_cmd(padapter, psta + , IPV4_SRC(ip), TCP_SRC(tcp) + , IPV4_SRC(ip), TCP_DST(tcp)); + if (DBG_SESSION_TRACKER) + RTW_INFO(FUNC_ADPT_FMT" local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT" SYN-ACK\n" + , FUNC_ADPT_ARG(padapter) + , IP_ARG(IPV4_SRC(ip)), PORT_ARG(TCP_SRC(tcp)) + , IP_ARG(IPV4_DST(ip)), PORT_ARG(TCP_DST(tcp))); + } + if (GET_TCP_FIN(tcp)) { + session_tracker_del_cmd(padapter, psta + , IPV4_SRC(ip), TCP_SRC(tcp) + , IPV4_SRC(ip), TCP_DST(tcp)); + if (DBG_SESSION_TRACKER) + RTW_INFO(FUNC_ADPT_FMT" local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT" FIN\n" + , FUNC_ADPT_ARG(padapter) + , IP_ARG(IPV4_SRC(ip)), PORT_ARG(TCP_SRC(tcp)) + , IP_ARG(IPV4_DST(ip)), PORT_ARG(TCP_DST(tcp))); + } + } + } + + } else if (0x888e == pattrib->ether_type) + eapol_type = parsing_eapol_packet(padapter, pktfile.cur_addr, psta, 1); +#if defined (DBG_ARP_DUMP) || defined (DBG_IP_R_MONITOR) + else if (pattrib->ether_type == ETH_P_ARP) { + u8 arp[28] = {0}; + + _rtw_pktfile_read(&pktfile, arp, 28); + dump_arp_pkt(RTW_DBGDUMP, etherhdr.h_dest, etherhdr.h_source, arp, 1); + } +#endif + + if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1)) + rtw_mi_set_scan_deny(padapter, 3000); + + if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) && + pattrib->ether_type == ETH_P_ARP && + !IS_MCAST(pattrib->dst)) { + rtw_mi_set_scan_deny(padapter, 1000); + rtw_mi_scan_abort(padapter, _FALSE); /*rtw_scan_abort_no_wait*/ + } + +#ifdef CONFIG_LPS + pkt_type = _rtw_lps_chk_packet_type(pattrib); + + if (pkt_type == LPS_PT_SP) {/*packet is as SPECIAL_PACKET*/ + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active); + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 0); + } else if (pkt_type == LPS_PT_ICMP) + rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 0); +#endif /* CONFIG_LPS */ + +#ifdef CONFIG_BEAMFORMING + update_attrib_txbf_info(padapter, pattrib, psta); +#endif + + /* TODO:_lock */ + if (update_attrib_sec_info(padapter, pattrib, psta, eapol_type) == _FAIL) { + DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sec); + res = _FAIL; + goto exit; + } + + /* get ether_hdr_len */ + pattrib->pkt_hdrlen = ETH_HLEN;/* (pattrib->ether_type == 0x8100) ? (14 + 4 ): 14; */ /* vlan tag */ + + pattrib->hdrlen = XATTRIB_GET_WDS(pattrib) ? WLAN_HDR_A4_LEN : WLAN_HDR_A3_LEN; + pattrib->subtype = WIFI_DATA_TYPE; + pattrib->qos_en = psta->qos_option; + pattrib->priority = 0; + + if (check_fwstate(pmlmepriv, WIFI_AP_STATE | WIFI_MESH_STATE + | WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE) + ) { + if (pattrib->qos_en) { + set_qos(pkt, pattrib); + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + rtw_mesh_tx_set_whdr_mctrl_len(pattrib->mesh_frame_mode, pattrib); + #endif + } + } else { +#ifdef CONFIG_TDLS + if (pattrib->direct_link == _TRUE) { + if (pattrib->qos_en) + set_qos(pkt, pattrib); + } else +#endif + { + if (pqospriv->qos_option) { + set_qos(pkt, pattrib); + + if (pmlmepriv->acm_mask != 0) + pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority); + } + } + } + + update_attrib_phy_info(padapter, pattrib, psta); + + /* RTW_INFO("%s ==> mac_id(%d)\n",__FUNCTION__,pattrib->mac_id ); */ + + pattrib->psta = psta; + /* TODO:_unlock */ + +#ifdef CONFIG_AUTO_AP_MODE + if (psta->isrc && psta->pid > 0) + pattrib->pctrl = _TRUE; + else +#endif + pattrib->pctrl = 0; + + pattrib->ack_policy = 0; + + if (bmcast) + pattrib->rate = psta->init_rate; + + +#ifdef CONFIG_WMMPS_STA + update_attrib_trigger_frame_info(padapter, pattrib); +#endif /* CONFIG_WMMPS_STA */ + + /* pattrib->priority = 5; */ /* force to used VI queue, for testing */ + pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no; + rtw_set_tx_chksum_offload(pkt, pattrib); + +exit: + + + return res; +} + +static s32 xmitframe_addmic(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + sint curfragnum, length; + u8 *pframe, *payload, mic[8]; + struct mic_data micdata; + /* struct sta_info *stainfo; */ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct security_priv *psecuritypriv = &padapter->securitypriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + u8 priority[4] = {0x0, 0x0, 0x0, 0x0}; + u8 hw_hdr_offset = 0; + sint bmcst = IS_MCAST(pattrib->ra); + + /* + if(pattrib->psta) + { + stainfo = pattrib->psta; + } + else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]); + } + + if(stainfo==NULL) + { + RTW_INFO("%s, psta==NUL\n", __func__); + return _FAIL; + } + + if(!(stainfo->state &WIFI_ASOC_STATE)) + { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, stainfo->state); + return _FAIL; + } + */ + + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);; +#else +#ifdef CONFIG_TX_EARLY_MODE + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + if (pattrib->encrypt == _TKIP_) { /* if(psecuritypriv->dot11PrivacyAlgrthm==_TKIP_PRIVACY_) */ + /* encode mic code */ + /* if(stainfo!= NULL) */ + { + u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + + pframe = pxmitframe->buf_addr + hw_hdr_offset; + + if (bmcst) { + if (_rtw_memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16) == _TRUE) { + /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n"); */ + /* rtw_msleep_os(10); */ + return _FAIL; + } + /* start to calculate the mic code */ + rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey); + } else { + if (_rtw_memcmp(&pattrib->dot11tkiptxmickey.skey[0], null_key, 16) == _TRUE) { + /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n"); */ + /* rtw_msleep_os(10); */ + return _FAIL; + } + /* start to calculate the mic code */ + rtw_secmicsetkey(&micdata, &pattrib->dot11tkiptxmickey.skey[0]); + } + + if (pframe[1] & 1) { /* ToDS==1 */ + rtw_secmicappend(&micdata, &pframe[16], 6); /* DA */ + if (pframe[1] & 2) /* From Ds==1 */ + rtw_secmicappend(&micdata, &pframe[24], 6); + else + rtw_secmicappend(&micdata, &pframe[10], 6); + } else { /* ToDS==0 */ + rtw_secmicappend(&micdata, &pframe[4], 6); /* DA */ + if (pframe[1] & 2) /* From Ds==1 */ + rtw_secmicappend(&micdata, &pframe[16], 6); + else + rtw_secmicappend(&micdata, &pframe[10], 6); + + } + + if (pattrib->qos_en) + priority[0] = (u8)pxmitframe->attrib.priority; + + + rtw_secmicappend(&micdata, &priority[0], 4); + + payload = pframe; + + for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) { + payload = (u8 *)RND4((SIZE_PTR)(payload)); + + payload = payload + pattrib->hdrlen + pattrib->iv_len; + if ((curfragnum + 1) == pattrib->nr_frags) { + length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - ((pattrib->bswenc) ? pattrib->icv_len : 0); + rtw_secmicappend(&micdata, payload, length); + payload = payload + length; + } else { + length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - ((pattrib->bswenc) ? pattrib->icv_len : 0); + rtw_secmicappend(&micdata, payload, length); + payload = payload + length + pattrib->icv_len; + } + } + rtw_secgetmic(&micdata, &(mic[0])); + /* add mic code and add the mic code length in last_txcmdsz */ + + _rtw_memcpy(payload, &(mic[0]), 8); + pattrib->last_txcmdsz += 8; + + payload = payload - pattrib->last_txcmdsz + 8; + } + } + + + return _SUCCESS; +} + +/*#define DBG_TX_SW_ENCRYPTOR*/ + +static s32 xmitframe_swencrypt(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + /* struct security_priv *psecuritypriv=&padapter->securitypriv; */ + + + /* if((psecuritypriv->sw_encrypt)||(pattrib->bswenc)) */ + if (pattrib->bswenc) { +#ifdef DBG_TX_SW_ENCRYPTOR + RTW_INFO(ADPT_FMT" - sec_type:%s DO SW encryption\n", + ADPT_ARG(padapter), security_type_str(pattrib->encrypt)); +#endif + + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + rtw_wep_encrypt(padapter, (u8 *)pxmitframe); + break; + case _TKIP_: + rtw_tkip_encrypt(padapter, (u8 *)pxmitframe); + break; + case _AES_: + case _CCMP_256_: + rtw_aes_encrypt(padapter, (u8 *)pxmitframe); + break; + case _GCMP_: + case _GCMP_256_: + rtw_gcmp_encrypt(padapter, (u8 *)pxmitframe); + break; +#ifdef CONFIG_WAPI_SUPPORT + case _SMS4_: + rtw_sms4_encrypt(padapter, (u8 *)pxmitframe); +#endif + default: + break; + } + + } + + + return _SUCCESS; +} + +s32 rtw_make_wlanhdr(_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib) +{ + u16 *qc; + + struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + u8 qos_option = _FALSE; + sint res = _SUCCESS; + u16 *fctrl = &pwlanhdr->frame_ctl; + + /* struct sta_info *psta; */ + + /* sint bmcst = IS_MCAST(pattrib->ra); */ + + + /* + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if(pattrib->psta != psta) + { + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return; + } + + if(psta==NULL) + { + RTW_INFO("%s, psta==NUL\n", __func__); + return _FAIL; + } + + if(!(psta->state &WIFI_ASOC_STATE)) + { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FAIL; + } + */ + + _rtw_memset(hdr, 0, WLANHDR_OFFSET); + + set_frame_sub_type(fctrl, pattrib->subtype); + + if (pattrib->subtype & WIFI_DATA_TYPE) { + if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE)) { +#ifdef CONFIG_TDLS + if (pattrib->direct_link == _TRUE) { + /* TDLS data transfer, ToDS=0, FrDs=0 */ + _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN); + + if (pattrib->qos_en) + qos_option = _TRUE; + } else +#endif /* CONFIG_TDLS */ + { + #ifdef CONFIG_RTW_WDS + if (pattrib->wds) { + SetToDs(fctrl); + SetFrDs(fctrl); + _rtw_memcpy(pwlanhdr->addr1, pattrib->ra, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr4, pattrib->src, ETH_ALEN); + } else + #endif + { + /* to_ds = 1, fr_ds = 0; */ + /* 1.Data transfer to AP */ + /* 2.Arp pkt will relayed by AP */ + SetToDs(fctrl); + _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN); + } + + if (pqospriv->qos_option) + qos_option = _TRUE; + } + } else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)) { + #ifdef CONFIG_RTW_WDS + if (pattrib->wds) { + SetToDs(fctrl); + SetFrDs(fctrl); + _rtw_memcpy(pwlanhdr->addr1, pattrib->ra, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr4, pattrib->src, ETH_ALEN); + } else + #endif + { + /* to_ds = 0, fr_ds = 1; */ + SetFrDs(fctrl); + _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN); + } + + if (pattrib->qos_en) + qos_option = _TRUE; + } else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) || + (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) { + _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN); + + if (pattrib->qos_en) + qos_option = _TRUE; +#ifdef CONFIG_RTW_MESH + } else if (check_fwstate(pmlmepriv, WIFI_MESH_STATE) == _TRUE) { + rtw_mesh_tx_build_whdr(padapter, pattrib, fctrl, pwlanhdr); + if (pattrib->qos_en) + qos_option = _TRUE; + else { + RTW_WARN("[%s] !qos_en in Mesh\n", __FUNCTION__); + res = _FAIL; + goto exit; + } +#endif + } else { + res = _FAIL; + goto exit; + } + + if (pattrib->mdata) + SetMData(fctrl); + + if (pattrib->encrypt) + SetPrivacy(fctrl); + + if (qos_option) { + qc = (unsigned short *)(hdr + pattrib->hdrlen - 2); + + if (pattrib->priority) + SetPriority(qc, pattrib->priority); + + SetEOSP(qc, pattrib->eosp); + + SetAckpolicy(qc, pattrib->ack_policy); + + if(pattrib->amsdu) + SetAMsdu(qc, pattrib->amsdu); +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + /* active: don't care, light sleep: 0, deep sleep: 1*/ + set_mps_lv(qc, 0); //TBD + + /* TBD: temporary set (rspi, eosp) = (0, 1) which means End MPSP */ + set_rspi(qc, 0); + SetEOSP(qc, 1); + + set_mctrl_present(qc, 1); + } +#endif + } + + /* TODO: fill HT Control Field */ + + /* Update Seq Num will be handled by f/w */ + { + struct sta_info *psta; + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return _FAIL; + } + + if (psta == NULL) { + RTW_INFO("%s, psta==NUL\n", __func__); + return _FAIL; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FAIL; + } + + + if (psta) { + psta->sta_xmitpriv.txseq_tid[pattrib->priority]++; + psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF; + pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority]; + + SetSeqNum(hdr, pattrib->seqnum); + +#ifdef CONFIG_80211N_HT +#if 0 /* move into update_attrib_phy_info(). */ + /* check if enable ampdu */ + if (pattrib->ht_en && psta->htpriv.ampdu_enable) { + if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) + pattrib->ampdu_en = _TRUE; + } +#endif + /* re-check if enable ampdu by BA_starting_seqctrl */ + if (pattrib->ampdu_en == _TRUE) { + u16 tx_seq; + + tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f]; + + /* check BA_starting_seqctrl */ + if (SN_LESS(pattrib->seqnum, tx_seq)) { + /* RTW_INFO("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq); */ + pattrib->ampdu_en = _FALSE;/* AGG BK */ + } else if (SN_EQUAL(pattrib->seqnum, tx_seq)) { + psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq + 1) & 0xfff; + + pattrib->ampdu_en = _TRUE;/* AGG EN */ + } else { + /* RTW_INFO("tx ampdu over run\n"); */ + psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum + 1) & 0xfff; + pattrib->ampdu_en = _TRUE;/* AGG EN */ + } + + } +#endif /* CONFIG_80211N_HT */ + } + } + + } else { + + } + +exit: + + + return res; +} + +s32 rtw_txframes_pending(_adapter *padapter) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + return ((_rtw_queue_empty(&pxmitpriv->be_pending) == _FALSE) || + (_rtw_queue_empty(&pxmitpriv->bk_pending) == _FALSE) || + (_rtw_queue_empty(&pxmitpriv->vi_pending) == _FALSE) || + (_rtw_queue_empty(&pxmitpriv->vo_pending) == _FALSE) +#ifdef CONFIG_RTW_MGMT_QUEUE + || (_rtw_queue_empty(&pxmitpriv->mgmt_pending) == _FALSE) +#endif + ); +} + +s32 rtw_txframes_sta_ac_pending(_adapter *padapter, struct pkt_attrib *pattrib) +{ + struct sta_info *psta; + struct tx_servq *ptxservq; + int priority = pattrib->priority; + /* + if(pattrib->psta) + { + psta = pattrib->psta; + } + else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]); + } + */ + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return 0; + } + + if (psta == NULL) { + RTW_INFO("%s, psta==NUL\n", __func__); + return 0; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return 0; + } + + switch (priority) { + case 1: + case 2: + ptxservq = &(psta->sta_xmitpriv.bk_q); + break; + case 4: + case 5: + ptxservq = &(psta->sta_xmitpriv.vi_q); + break; + case 6: + case 7: + ptxservq = &(psta->sta_xmitpriv.vo_q); + break; + case 0: + case 3: + default: + ptxservq = &(psta->sta_xmitpriv.be_q); + break; + + } + + return ptxservq->qcnt; +} + +#ifdef CONFIG_TDLS + +int rtw_build_tdls_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt) +{ + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct sta_info *ptdls_sta = NULL; + int res = _SUCCESS; + + ptdls_sta = rtw_get_stainfo((&padapter->stapriv), pattrib->dst); + if (ptdls_sta == NULL) { + switch (ptxmgmt->action_code) { + case TDLS_DISCOVERY_REQUEST: + case TUNNELED_PROBE_REQ: + case TUNNELED_PROBE_RSP: + break; + default: + RTW_INFO("[TDLS] %s - Direct Link Peer = "MAC_FMT" not found for action = %d\n", __func__, MAC_ARG(pattrib->dst), ptxmgmt->action_code); + res = _FAIL; + goto exit; + } + } + + switch (ptxmgmt->action_code) { + case TDLS_SETUP_REQUEST: + rtw_build_tdls_setup_req_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; + case TDLS_SETUP_RESPONSE: + rtw_build_tdls_setup_rsp_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; + case TDLS_SETUP_CONFIRM: + rtw_build_tdls_setup_cfm_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; + case TDLS_TEARDOWN: + rtw_build_tdls_teardown_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; + case TDLS_DISCOVERY_REQUEST: + rtw_build_tdls_dis_req_ies(padapter, pxmitframe, pframe, ptxmgmt); + break; + case TDLS_PEER_TRAFFIC_INDICATION: + rtw_build_tdls_peer_traffic_indication_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; +#ifdef CONFIG_TDLS_CH_SW + case TDLS_CHANNEL_SWITCH_REQUEST: + rtw_build_tdls_ch_switch_req_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; + case TDLS_CHANNEL_SWITCH_RESPONSE: + rtw_build_tdls_ch_switch_rsp_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; +#endif + case TDLS_PEER_TRAFFIC_RESPONSE: + rtw_build_tdls_peer_traffic_rsp_ies(padapter, pxmitframe, pframe, ptxmgmt, ptdls_sta); + break; +#ifdef CONFIG_WFD + case TUNNELED_PROBE_REQ: + rtw_build_tunneled_probe_req_ies(padapter, pxmitframe, pframe); + break; + case TUNNELED_PROBE_RSP: + rtw_build_tunneled_probe_rsp_ies(padapter, pxmitframe, pframe); + break; +#endif /* CONFIG_WFD */ + default: + res = _FAIL; + break; + } + +exit: + return res; +} + +s32 rtw_make_tdls_wlanhdr(_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt) +{ + u16 *qc; + struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct qos_priv *pqospriv = &pmlmepriv->qospriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct sta_info *psta = NULL, *ptdls_sta = NULL; + u8 tdls_seq = 0, baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; + + sint res = _SUCCESS; + u16 *fctrl = &pwlanhdr->frame_ctl; + + + _rtw_memset(hdr, 0, WLANHDR_OFFSET); + + set_frame_sub_type(fctrl, pattrib->subtype); + + switch (ptxmgmt->action_code) { + case TDLS_SETUP_REQUEST: + case TDLS_SETUP_RESPONSE: + case TDLS_SETUP_CONFIRM: + case TDLS_PEER_TRAFFIC_INDICATION: + case TDLS_PEER_PSM_REQUEST: + case TUNNELED_PROBE_REQ: + case TUNNELED_PROBE_RSP: + case TDLS_DISCOVERY_REQUEST: + SetToDs(fctrl); + _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN); + break; + case TDLS_CHANNEL_SWITCH_REQUEST: + case TDLS_CHANNEL_SWITCH_RESPONSE: + case TDLS_PEER_PSM_RESPONSE: + case TDLS_PEER_TRAFFIC_RESPONSE: + _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN); + tdls_seq = 1; + break; + case TDLS_TEARDOWN: + if (ptxmgmt->status_code == _RSON_TDLS_TEAR_UN_RSN_) { + _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN); + tdls_seq = 1; + } else { + SetToDs(fctrl); + _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN); + _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN); + } + break; + } + + if (pattrib->encrypt) + SetPrivacy(fctrl); + + if (ptxmgmt->action_code == TDLS_PEER_TRAFFIC_RESPONSE) + SetPwrMgt(fctrl); + + if (pqospriv->qos_option) { + qc = (unsigned short *)(hdr + pattrib->hdrlen - 2); + if (pattrib->priority) + SetPriority(qc, pattrib->priority); + SetAckpolicy(qc, pattrib->ack_policy); + } + + psta = pattrib->psta; + + /* 1. update seq_num per link by sta_info */ + /* 2. rewrite encrypt to _AES_, also rewrite iv_len, icv_len */ + if (tdls_seq == 1) { + ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst); + if (ptdls_sta) { + ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority]++; + ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF; + pattrib->seqnum = ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority]; + SetSeqNum(hdr, pattrib->seqnum); + + if (pattrib->encrypt) { + pattrib->encrypt = _AES_; + pattrib->iv_len = 8; + pattrib->icv_len = 8; + pattrib->bswenc = _FALSE; + } + pattrib->mac_id = ptdls_sta->cmn.mac_id; + } else { + res = _FAIL; + goto exit; + } + } else if (psta) { + psta->sta_xmitpriv.txseq_tid[pattrib->priority]++; + psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF; + pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority]; + SetSeqNum(hdr, pattrib->seqnum); + } + + +exit: + + + return res; +} + +s32 rtw_xmit_tdls_coalesce(_adapter *padapter, struct xmit_frame *pxmitframe, struct tdls_txmgmt *ptxmgmt) +{ + s32 llc_sz; + + u8 *pframe, *mem_start; + + struct sta_info *psta; + struct sta_priv *pstapriv = &padapter->stapriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + u8 *pbuf_start; + s32 bmcst = IS_MCAST(pattrib->ra); + s32 res = _SUCCESS; + + + if (pattrib->psta) + psta = pattrib->psta; + else { + if (bmcst) + psta = rtw_get_bcmc_stainfo(padapter); + else + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + } + + if (psta == NULL) { + res = _FAIL; + goto exit; + } + + if (pxmitframe->buf_addr == NULL) { + res = _FAIL; + goto exit; + } + + pbuf_start = pxmitframe->buf_addr; + mem_start = pbuf_start + TXDESC_OFFSET; + + if (rtw_make_tdls_wlanhdr(padapter, mem_start, pattrib, ptxmgmt) == _FAIL) { + res = _FAIL; + goto exit; + } + + pframe = mem_start; + pframe += pattrib->hdrlen; + + /* adding icv, if necessary... */ + if (pattrib->iv_len) { + if (psta != NULL) { + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + break; + case _TKIP_: + if (bmcst) + TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + TKIP_IV(pattrib->iv, psta->dot11txpn, 0); + break; + case _AES_: + if (bmcst) + AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + AES_IV(pattrib->iv, psta->dot11txpn, 0); + break; + } + } + + _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len); + pframe += pattrib->iv_len; + + } + + llc_sz = rtw_put_snap(pframe, pattrib->ether_type); + pframe += llc_sz; + + /* pattrib->pktlen will be counted in rtw_build_tdls_ies */ + pattrib->pktlen = 0; + + rtw_build_tdls_ies(padapter, pxmitframe, pframe, ptxmgmt); + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + pframe += pattrib->pktlen; + _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); + pframe += pattrib->icv_len; + } + + pattrib->nr_frags = 1; + pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + llc_sz + + ((pattrib->bswenc) ? pattrib->icv_len : 0) + pattrib->pktlen; + + if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) { + res = _FAIL; + goto exit; + } + + xmitframe_swencrypt(padapter, pxmitframe); + + update_attrib_vcs_info(padapter, pxmitframe); + +exit: + + + return res; +} +#endif /* CONFIG_TDLS */ + +/* + * Calculate wlan 802.11 packet MAX size from pkt_attrib + * This function doesn't consider fragment case + */ +u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib) +{ + u32 len = 0; + + len = pattrib->hdrlen /* WLAN Header */ + + pattrib->iv_len /* IV */ + + XATTRIB_GET_MCTRL_LEN(pattrib) + + SNAP_SIZE + sizeof(u16) /* LLC */ + + pattrib->pktlen + + (pattrib->encrypt == _TKIP_ ? 8 : 0) /* MIC */ + + (pattrib->bswenc ? pattrib->icv_len : 0) /* ICV */ + ; + + return len; +} + +#ifdef CONFIG_TX_AMSDU +s32 check_amsdu(struct xmit_frame *pxmitframe) +{ + struct pkt_attrib *pattrib; + struct sta_info *psta = NULL; + s32 ret = _TRUE; + + if (!pxmitframe) + ret = _FALSE; + + pattrib = &pxmitframe->attrib; + + psta = rtw_get_stainfo(&pxmitframe->padapter->stapriv, &pattrib->ra[0]); + if (psta) { + if (psta->flags & WLAN_STA_AMSDU_DISABLE) + ret =_FALSE; + } + if (IS_MCAST(pattrib->ra)) + ret = _FALSE; + + if ((pattrib->ether_type == 0x888e) || + (pattrib->ether_type == 0x0806) || + (pattrib->ether_type == 0x88b4) || + (pattrib->dhcp_pkt == 1)) + ret = _FALSE; + + if ((pattrib->encrypt == _WEP40_) || + (pattrib->encrypt == _WEP104_) || + (pattrib->encrypt == _TKIP_)) + ret = _FALSE; + + if (!pattrib->qos_en) + ret = _FALSE; + + if (IS_AMSDU_AMPDU_NOT_VALID(pattrib)) + ret = _FALSE; + + return ret; +} + +s32 check_amsdu_tx_support(_adapter *padapter) +{ + struct dvobj_priv *pdvobjpriv; + int tx_amsdu; + int tx_amsdu_rate; + int current_tx_rate; + s32 ret = _FALSE; + + pdvobjpriv = adapter_to_dvobj(padapter); + tx_amsdu = padapter->tx_amsdu; + tx_amsdu_rate = padapter->tx_amsdu_rate; + current_tx_rate = pdvobjpriv->traffic_stat.cur_tx_tp; + + if (tx_amsdu == 1) + ret = _TRUE; + else if (tx_amsdu == 2 && (tx_amsdu_rate == 0 || current_tx_rate > tx_amsdu_rate)) + ret = _TRUE; + else + ret = _FALSE; + + return ret; +} + +s32 rtw_xmitframe_coalesce_amsdu(_adapter *padapter, struct xmit_frame *pxmitframe, struct xmit_frame *pxmitframe_queue) +{ + + struct pkt_file pktfile; + struct pkt_attrib *pattrib; + _pkt *pkt; + + struct pkt_file pktfile_queue; + struct pkt_attrib *pattrib_queue; + _pkt *pkt_queue; + + s32 llc_sz, mem_sz; + + s32 padding = 0; + + u8 *pframe, *mem_start; + u8 hw_hdr_offset; + + u16* len; + u8 *pbuf_start; + s32 res = _SUCCESS; + + if (pxmitframe->buf_addr == NULL) { + RTW_INFO("==> %s buf_addr==NULL\n", __FUNCTION__); + return _FAIL; + } + + + pbuf_start = pxmitframe->buf_addr; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE /* for SDIO && Tx Agg */ + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + mem_start = pbuf_start + hw_hdr_offset; //for DMA + + pattrib = &pxmitframe->attrib; + + pattrib->amsdu = 1; + + if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) { + RTW_INFO("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"); + res = _FAIL; + goto exit; + } + + llc_sz = 0; + + pframe = mem_start; + + //SetMFrag(mem_start); + ClearMFrag(mem_start); + + pframe += pattrib->hdrlen; + + /* adding icv, if necessary... */ + if (pattrib->iv_len) { + _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len); // queue or new? + + RTW_DBG("rtw_xmitframe_coalesce: keyid=%d pattrib->iv[3]=%.2x pframe=%.2x %.2x %.2x %.2x\n", + padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe + 1), *(pframe + 2), *(pframe + 3)); + + pframe += pattrib->iv_len; + } + + pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len; + + if(pxmitframe_queue) + { + pattrib_queue = &pxmitframe_queue->attrib; + pkt_queue = pxmitframe_queue->pkt; + + _rtw_open_pktfile(pkt_queue, &pktfile_queue); + _rtw_pktfile_read(&pktfile_queue, NULL, pattrib_queue->pkt_hdrlen); + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + /* mDA(6), mSA(6), len(2), mctrl */ + _rtw_memcpy(pframe, pattrib_queue->mda, ETH_ALEN); + pframe += ETH_ALEN; + _rtw_memcpy(pframe, pattrib_queue->msa, ETH_ALEN); + pframe += ETH_ALEN; + len = (u16*)pframe; + pframe += 2; + rtw_mesh_tx_build_mctrl(padapter, pattrib_queue, pframe); + pframe += XATTRIB_GET_MCTRL_LEN(pattrib_queue); + } else + #endif + { + /* 802.3 MAC Header DA(6) SA(6) Len(2)*/ + _rtw_memcpy(pframe, pattrib_queue->dst, ETH_ALEN); + pframe += ETH_ALEN; + _rtw_memcpy(pframe, pattrib_queue->src, ETH_ALEN); + pframe += ETH_ALEN; + len = (u16*)pframe; + pframe += 2; + } + + llc_sz = rtw_put_snap(pframe, pattrib_queue->ether_type); + pframe += llc_sz; + + mem_sz = _rtw_pktfile_read(&pktfile_queue, pframe, pattrib_queue->pktlen); + pframe += mem_sz; + + *len = htons(XATTRIB_GET_MCTRL_LEN(pattrib_queue) + llc_sz + mem_sz); + + //calc padding + padding = 4 - ((ETH_HLEN + XATTRIB_GET_MCTRL_LEN(pattrib_queue) + llc_sz + mem_sz) & (4-1)); + if(padding == 4) + padding = 0; + + //_rtw_memset(pframe,0xaa, padding); + pframe += padding; + + pattrib->last_txcmdsz += ETH_HLEN + XATTRIB_GET_MCTRL_LEN(pattrib_queue) + llc_sz + mem_sz + padding ; + } + + //2nd mpdu + + pkt = pxmitframe->pkt; + _rtw_open_pktfile(pkt, &pktfile); + _rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen); + +#ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + /* mDA(6), mSA(6), len(2), mctrl */ + _rtw_memcpy(pframe, pattrib->mda, ETH_ALEN); + pframe += ETH_ALEN; + _rtw_memcpy(pframe, pattrib->msa, ETH_ALEN); + pframe += ETH_ALEN; + len = (u16*)pframe; + pframe += 2; + rtw_mesh_tx_build_mctrl(padapter, pattrib, pframe); + pframe += XATTRIB_GET_MCTRL_LEN(pattrib); + } else +#endif + { + /* 802.3 MAC Header DA(6) SA(6) Len(2) */ + _rtw_memcpy(pframe, pattrib->dst, ETH_ALEN); + pframe += ETH_ALEN; + _rtw_memcpy(pframe, pattrib->src, ETH_ALEN); + pframe += ETH_ALEN; + len = (u16*)pframe; + pframe += 2; + } + + llc_sz = rtw_put_snap(pframe, pattrib->ether_type); + pframe += llc_sz; + + mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen); + + pframe += mem_sz; + + *len = htons(XATTRIB_GET_MCTRL_LEN(pattrib) + llc_sz + mem_sz); + + //the last ampdu has no padding + padding = 0; + + pattrib->nr_frags = 1; + + pattrib->last_txcmdsz += ETH_HLEN + XATTRIB_GET_MCTRL_LEN(pattrib) + llc_sz + mem_sz + padding + + ((pattrib->bswenc) ? pattrib->icv_len : 0) ; + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); + pframe += pattrib->icv_len; + } + + if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) { + RTW_INFO("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n"); + res = _FAIL; + goto exit; + } + + xmitframe_swencrypt(padapter, pxmitframe); + + update_attrib_vcs_info(padapter, pxmitframe); + +exit: + return res; +} +#endif /* CONFIG_TX_AMSDU */ + +/* + +This sub-routine will perform all the following: + +1. remove 802.3 header. +2. create wlan_header, based on the info in pxmitframe +3. append sta's iv/ext-iv +4. append LLC +5. move frag chunk from pframe to pxmitframe->mem +6. apply sw-encrypt, if necessary. + +*/ +s32 rtw_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe) +{ + struct pkt_file pktfile; + + s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz; + + SIZE_PTR addr; + + u8 *pframe, *mem_start; + u8 hw_hdr_offset; + + /* struct sta_info *psta; */ + /* struct sta_priv *pstapriv = &padapter->stapriv; */ + /* struct mlme_priv *pmlmepriv = &padapter->mlmepriv; */ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + u8 *pbuf_start; + + s32 bmcst = IS_MCAST(pattrib->ra); + s32 res = _SUCCESS; + + + /* + if (pattrib->psta) + { + psta = pattrib->psta; + } else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + } + + if(psta==NULL) + { + + RTW_INFO("%s, psta==NUL\n", __func__); + return _FAIL; + } + + + if(!(psta->state &WIFI_ASOC_STATE)) + { + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FAIL; + } + */ + if (pxmitframe->buf_addr == NULL) { + RTW_INFO("==> %s buf_addr==NULL\n", __FUNCTION__); + return _FAIL; + } + + pbuf_start = pxmitframe->buf_addr; + +#ifdef CONFIG_USB_TX_AGGREGATION + hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ); +#else +#ifdef CONFIG_TX_EARLY_MODE /* for SDIO && Tx Agg */ + hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE; +#else + hw_hdr_offset = TXDESC_OFFSET; +#endif +#endif + + mem_start = pbuf_start + hw_hdr_offset; + + if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) { + RTW_INFO("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"); + res = _FAIL; + goto exit; + } + + _rtw_open_pktfile(pkt, &pktfile); + _rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen); + + frg_inx = 0; + frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */ + + while (1) { + llc_sz = 0; + + mpdu_len = frg_len; + + pframe = mem_start; + + SetMFrag(mem_start); + + pframe += pattrib->hdrlen; + mpdu_len -= pattrib->hdrlen; + + /* adding icv, if necessary... */ + if (pattrib->iv_len) { +#if 0 + /* if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) */ + /* psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv)); */ + /* else */ + /* psta = rtw_get_stainfo(pstapriv, pattrib->ra); */ + + if (psta != NULL) { + switch (pattrib->encrypt) { + case _WEP40_: + case _WEP104_: + WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + break; + case _TKIP_: + if (bmcst) + TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + TKIP_IV(pattrib->iv, psta->dot11txpn, 0); + break; + case _AES_: + if (bmcst) + AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx); + else + AES_IV(pattrib->iv, psta->dot11txpn, 0); + break; +#ifdef CONFIG_WAPI_SUPPORT + case _SMS4_: + rtw_wapi_get_iv(padapter, pattrib->ra, pattrib->iv); + break; +#endif + } + } +#endif + _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len); + + + pframe += pattrib->iv_len; + + mpdu_len -= pattrib->iv_len; + } + + if (frg_inx == 0) { + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) { + rtw_mesh_tx_build_mctrl(padapter, pattrib, pframe); + pframe += XATTRIB_GET_MCTRL_LEN(pattrib); + mpdu_len -= XATTRIB_GET_MCTRL_LEN(pattrib); + } + #endif + + llc_sz = rtw_put_snap(pframe, pattrib->ether_type); + pframe += llc_sz; + mpdu_len -= llc_sz; + } + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) + mpdu_len -= pattrib->icv_len; + + + if (bmcst) { + /* don't do fragment to broadcat/multicast packets */ + mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen); + } else + mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len); + + pframe += mem_sz; + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); + pframe += pattrib->icv_len; + } + + frg_inx++; + + if (bmcst || (rtw_endofpktfile(&pktfile) == _TRUE)) { + pattrib->nr_frags = frg_inx; + + pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + + ((pattrib->nr_frags == 1) ? (XATTRIB_GET_MCTRL_LEN(pattrib) + llc_sz) : 0) + + ((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz; + + ClearMFrag(mem_start); + + break; + } + + addr = (SIZE_PTR)(pframe); + + mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset; + _rtw_memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen); + + } + + if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) { + RTW_INFO("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n"); + res = _FAIL; + goto exit; + } + + xmitframe_swencrypt(padapter, pxmitframe); + + if (bmcst == _FALSE) + update_attrib_vcs_info(padapter, pxmitframe); + else + pattrib->vcs_mode = NONE_VCS; + +exit: + + + return res; +} + +#if defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) +/* + * CCMP encryption for unicast robust mgmt frame and broadcast group privicy action + * BIP for broadcast robust mgmt frame + */ +s32 rtw_mgmt_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe) +{ +#define DBG_MGMT_XMIT_COALESEC_DUMP 0 +#define DBG_MGMT_XMIT_BIP_DUMP 0 +#define DBG_MGMT_XMIT_ENC_DUMP 0 + + struct pkt_file pktfile; + s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz; + SIZE_PTR addr; + u8 *pframe, *mem_start = NULL, *tmp_buf = NULL; + u8 hw_hdr_offset, subtype ; + u8 category = 0xFF; + struct sta_info *psta = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + u8 *pbuf_start; + s32 bmcst = IS_MCAST(pattrib->ra); + s32 res = _FAIL; + u8 *BIP_AAD = NULL; + u8 *MGMT_body = NULL; + + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct rtw_ieee80211_hdr *pwlanhdr; + u8 mme_cont[_MME_IE_LENGTH_ - 2]; + u8 mme_clen; + + _irqL irqL; + u32 ori_len; + union pn48 *pn = NULL; + enum security_type cipher = _NO_PRIVACY_; + u8 kid; + + if (pxmitframe->buf_addr == NULL) { + RTW_WARN(FUNC_ADPT_FMT" pxmitframe->buf_addr\n" + , FUNC_ADPT_ARG(padapter)); + return _FAIL; + } + + mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + subtype = get_frame_sub_type(pframe); /* bit(7)~bit(2) */ + + /* check if robust mgmt frame */ + if (subtype != WIFI_DEAUTH && subtype != WIFI_DISASSOC && subtype != WIFI_ACTION) + return _SUCCESS; + if (subtype == WIFI_ACTION) { + category = *(pframe + sizeof(struct rtw_ieee80211_hdr_3addr)); + if (CATEGORY_IS_NON_ROBUST(category)) + return _SUCCESS; + } + if (!bmcst) { + if (pattrib->psta) + psta = pattrib->psta; + else + pattrib->psta = psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (psta == NULL) { + RTW_INFO(FUNC_ADPT_FMT" unicast sta == NULL\n", FUNC_ADPT_ARG(padapter)); + return _FAIL; + } + if (!(psta->flags & WLAN_STA_MFP)) { + /* peer is not MFP capable, no need to encrypt */ + return _SUCCESS; + } + if (psta->bpairwise_key_installed != _TRUE) { + RTW_INFO(FUNC_ADPT_FMT" PTK is not installed\n" + , FUNC_ADPT_ARG(padapter)); + return _FAIL; + } + } + + ori_len = BIP_AAD_SIZE + pattrib->pktlen + _MME_IE_LENGTH_; + tmp_buf = BIP_AAD = rtw_zmalloc(ori_len); + if (BIP_AAD == NULL) + return _FAIL; + + _enter_critical_bh(&padapter->security_key_mutex, &irqL); + + if (bmcst) { + if (subtype == WIFI_ACTION && CATEGORY_IS_GROUP_PRIVACY(category)) { + /* broadcast group privacy action frame */ + #if DBG_MGMT_XMIT_COALESEC_DUMP + RTW_INFO(FUNC_ADPT_FMT" broadcast gp action(%u)\n" + , FUNC_ADPT_ARG(padapter), category); + #endif + + if (pattrib->psta) + psta = pattrib->psta; + else + pattrib->psta = psta = rtw_get_bcmc_stainfo(padapter); + if (psta == NULL) { + RTW_INFO(FUNC_ADPT_FMT" broadcast sta == NULL\n" + , FUNC_ADPT_ARG(padapter)); + goto xmitframe_coalesce_fail; + } + if (padapter->securitypriv.binstallGrpkey != _TRUE) { + RTW_INFO(FUNC_ADPT_FMT" GTK is not installed\n" + , FUNC_ADPT_ARG(padapter)); + goto xmitframe_coalesce_fail; + } + + pn = &psta->dot11txpn; + cipher = padapter->securitypriv.dot118021XGrpPrivacy; + kid = padapter->securitypriv.dot118021XGrpKeyid; + } else { + #ifdef CONFIG_IEEE80211W + /* broadcast robust mgmt frame, using BIP */ + int frame_body_len; + u8 mic[16]; + + /* IGTK key is not install ex: mesh MFP without IGTK */ + if (SEC_IS_BIP_KEY_INSTALLED(&padapter->securitypriv) != _TRUE) + goto xmitframe_coalesce_success; + + #if DBG_MGMT_XMIT_COALESEC_DUMP + if (subtype == WIFI_DEAUTH) + RTW_INFO(FUNC_ADPT_FMT" braodcast deauth\n", FUNC_ADPT_ARG(padapter)); + else if (subtype == WIFI_DISASSOC) + RTW_INFO(FUNC_ADPT_FMT" braodcast disassoc\n", FUNC_ADPT_ARG(padapter)); + else if (subtype == WIFI_ACTION) { + RTW_INFO(FUNC_ADPT_FMT" braodcast action(%u)\n" + , FUNC_ADPT_ARG(padapter), category); + } + #endif + + _rtw_memset(mme_cont, 0, _MME_IE_LENGTH_ - 2); + mme_clen = padapter->securitypriv.dot11wCipher == _BIP_CMAC_128_ ? 16 : 24; + + MGMT_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr); + pframe += pattrib->pktlen; + + /* octent 0 and 1 is key index ,BIP keyid is 4 or 5, LSB only need octent 0 */ + mme_cont[0] = padapter->securitypriv.dot11wBIPKeyid; + /* increase PN and apply to packet */ + padapter->securitypriv.dot11wBIPtxpn.val++; + RTW_PUT_LE64(&mme_cont[2], padapter->securitypriv.dot11wBIPtxpn.val); + + /* add MME IE with MIC all zero, MME string doesn't include element id and length */ + pframe = rtw_set_ie(pframe, _MME_IE_ , mme_clen , mme_cont, &(pattrib->pktlen)); + pattrib->last_txcmdsz = pattrib->pktlen; + /* total frame length - header length */ + frame_body_len = pattrib->pktlen - sizeof(struct rtw_ieee80211_hdr_3addr); + + /* conscruct AAD, copy frame control field */ + _rtw_memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2); + ClearRetry(BIP_AAD); + ClearPwrMgt(BIP_AAD); + ClearMData(BIP_AAD); + /* conscruct AAD, copy address 1 to address 3 */ + _rtw_memcpy(BIP_AAD + 2, GetAddr1Ptr((u8 *)pwlanhdr), 18); + /* copy management fram body */ + _rtw_memcpy(BIP_AAD + BIP_AAD_SIZE, MGMT_body, frame_body_len); + + #if DBG_MGMT_XMIT_BIP_DUMP + /* dump total packet include MME with zero MIC */ + { + int i; + printk("Total packet: "); + for (i = 0; i < BIP_AAD_SIZE + frame_body_len; i++) + printk(" %02x ", BIP_AAD[i]); + printk("\n"); + } + #endif + + /* calculate mic */ + if (rtw_calculate_bip_mic(padapter->securitypriv.dot11wCipher, + (u8 *)pwlanhdr, pattrib->pktlen, + padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey, + BIP_AAD, (BIP_AAD_SIZE + frame_body_len), mic) == _FAIL) + goto xmitframe_coalesce_fail; + + #if DBG_MGMT_XMIT_BIP_DUMP + /* dump calculated mic result */ + { + int i; + printk("Calculated mic result: "); + for (i = 0; i < 16; i++) + printk(" %02x ", mic[i]); + printk("\n"); + } + #endif + + /* copy right BIP mic value, total is 128bits, we use the 0~63 bits */ + if (padapter->securitypriv.dot11wCipher == _BIP_CMAC_128_) + _rtw_memcpy(pframe - 8, mic, 8); + else + _rtw_memcpy(pframe - 16, mic, 16); + + #if DBG_MGMT_XMIT_BIP_DUMP + /*dump all packet after mic ok */ + { + int pp; + printk("pattrib->pktlen = %d\n", pattrib->pktlen); + for(pp=0;pp< pattrib->pktlen; pp++) + printk(" %02x ", mem_start[pp]); + printk("\n"); + } + #endif + + #endif /* CONFIG_IEEE80211W */ + + goto xmitframe_coalesce_success; + } + } + else { + /* unicast robust mgmt frame */ + #if DBG_MGMT_XMIT_COALESEC_DUMP + if (subtype == WIFI_DEAUTH) { + RTW_INFO(FUNC_ADPT_FMT" unicast deauth to "MAC_FMT"\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(pattrib->ra)); + } else if (subtype == WIFI_DISASSOC) { + RTW_INFO(FUNC_ADPT_FMT" unicast disassoc to "MAC_FMT"\n" + , FUNC_ADPT_ARG(padapter), MAC_ARG(pattrib->ra)); + } else if (subtype == WIFI_ACTION) { + RTW_INFO(FUNC_ADPT_FMT" unicast action(%u) to "MAC_FMT"\n" + , FUNC_ADPT_ARG(padapter), category, MAC_ARG(pattrib->ra)); + } + #endif + + pn = &psta->dot11txpn; + cipher = psta->dot118021XPrivacy; + kid = 0; + + _rtw_memcpy(pattrib->dot118021x_UncstKey.skey + , psta->dot118021x_UncstKey.skey + , (cipher & _SEC_TYPE_256_) ? 32 : 16); + + /* To use wrong key */ + if (pattrib->key_type == IEEE80211W_WRONG_KEY) { + RTW_INFO("use wrong key\n"); + pattrib->dot118021x_UncstKey.skey[0] = 0xff; + } + } + + #if DBG_MGMT_XMIT_ENC_DUMP + /* before encrypt dump the management packet content */ + { + int i; + printk("Management pkt: "); + for(i=0; ipktlen; i++) + printk(" %02x ", pframe[i]); + printk("=======\n"); + } + #endif + + /* bakeup original management packet */ + _rtw_memcpy(tmp_buf, pframe, pattrib->pktlen); + /* move to data portion */ + pframe += pattrib->hdrlen; + + if (pattrib->key_type != IEEE80211W_NO_KEY) { + pattrib->encrypt = cipher; + pattrib->bswenc = _TRUE; + } + + /* + * 802.11w encrypted management packet must be: + * _AES_, _CCMP_256_, _GCMP_, _GCMP_256_ + */ + switch (pattrib->encrypt) { + case _AES_: + pattrib->iv_len = 8; + pattrib->icv_len = 8; + AES_IV(pattrib->iv, (*pn), kid); + break; + case _CCMP_256_: + pattrib->iv_len = 8; + pattrib->icv_len = 16; + AES_IV(pattrib->iv, (*pn), kid); + break; + case _GCMP_: + case _GCMP_256_: + pattrib->iv_len = 8; + pattrib->icv_len = 16; + GCMP_IV(pattrib->iv, (*pn), kid); + break; + default: + goto xmitframe_coalesce_fail; + } + + /* insert iv header into management frame */ + _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len); + pframe += pattrib->iv_len; + /* copy mgmt data portion after CCMP header */ + _rtw_memcpy(pframe, tmp_buf + pattrib->hdrlen, pattrib->pktlen - pattrib->hdrlen); + /* move pframe to end of mgmt pkt */ + pframe += pattrib->pktlen - pattrib->hdrlen; + /* add 8 bytes CCMP IV header to length */ + pattrib->pktlen += pattrib->iv_len; + + #if DBG_MGMT_XMIT_ENC_DUMP + /* dump management packet include AES IV header */ + { + int i; + printk("Management pkt + IV: "); + /* for(i=0; ipktlen; i++) */ + + printk("@@@@@@@@@@@@@\n"); + } + #endif + + if ((pattrib->icv_len > 0) && (pattrib->bswenc)) { + _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); + pframe += pattrib->icv_len; + } + /* add 8 bytes MIC */ + pattrib->pktlen += pattrib->icv_len; + /* set final tx command size */ + pattrib->last_txcmdsz = pattrib->pktlen; + + /* set protected bit must be beofre SW encrypt */ + SetPrivacy(mem_start); + + #if DBG_MGMT_XMIT_ENC_DUMP + /* dump management packet include AES header */ + { + int i; + printk("prepare to enc Management pkt + IV: "); + for (i = 0; i < pattrib->pktlen; i++) + printk(" %02x ", mem_start[i]); + printk("@@@@@@@@@@@@@\n"); + } + #endif + + /* software encrypt */ + xmitframe_swencrypt(padapter, pxmitframe); + +xmitframe_coalesce_success: + _exit_critical_bh(&padapter->security_key_mutex, &irqL); + rtw_mfree(BIP_AAD, ori_len); + return _SUCCESS; + +xmitframe_coalesce_fail: + _exit_critical_bh(&padapter->security_key_mutex, &irqL); + rtw_mfree(BIP_AAD, ori_len); + + return _FAIL; +} +#endif /* defined(CONFIG_IEEE80211W) || defined(CONFIG_RTW_MESH) */ + +/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header + * IEEE LLC/SNAP header contains 8 octets + * First 3 octets comprise the LLC portion + * SNAP portion, 5 octets, is divided into two fields: + * Organizationally Unique Identifier(OUI), 3 octets, + * type, defined by that organization, 2 octets. + */ +s32 rtw_put_snap(u8 *data, u16 h_proto) +{ + struct ieee80211_snap_hdr *snap; + u8 *oui; + + + snap = (struct ieee80211_snap_hdr *)data; + snap->dsap = 0xaa; + snap->ssap = 0xaa; + snap->ctrl = 0x03; + + if (h_proto == 0x8137 || h_proto == 0x80f3) + oui = P802_1H_OUI; + else + oui = RFC1042_OUI; + + snap->oui[0] = oui[0]; + snap->oui[1] = oui[1]; + snap->oui[2] = oui[2]; + + *(u16 *)(data + SNAP_SIZE) = htons(h_proto); + + + return SNAP_SIZE + sizeof(u16); +} + +void rtw_update_protection(_adapter *padapter, u8 *ie, uint ie_len) +{ + + uint protection; + u8 *perp; + sint erp_len; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct registry_priv *pregistrypriv = &padapter->registrypriv; + + + switch (pxmitpriv->vcs_setting) { + case DISABLE_VCS: + pxmitpriv->vcs = NONE_VCS; + break; + + case ENABLE_VCS: + break; + + case AUTO_VCS: + default: + perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len); + if (perp == NULL) + pxmitpriv->vcs = NONE_VCS; + else { + protection = (*(perp + 2)) & BIT(1); + if (protection) { + if (pregistrypriv->vcs_type == RTS_CTS) + pxmitpriv->vcs = RTS_CTS; + else + pxmitpriv->vcs = CTS_TO_SELF; + } else + pxmitpriv->vcs = NONE_VCS; + } + + break; + + } + + +} + +void rtw_count_tx_stats(PADAPTER padapter, struct xmit_frame *pxmitframe, int sz) +{ + struct sta_info *psta = NULL; + struct stainfo_stats *pstats = NULL; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + u8 pkt_num = 1; + + if ((pxmitframe->frame_tag & 0x0f) == DATA_FRAMETAG) { +#if defined(CONFIG_USB_TX_AGGREGATION) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pkt_num = pxmitframe->agg_num; +#endif + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pkt_num; + + pxmitpriv->tx_pkts += pkt_num; + + pxmitpriv->tx_bytes += sz; + + psta = pxmitframe->attrib.psta; + if (psta) { + pstats = &psta->sta_stats; + + pstats->tx_pkts += pkt_num; + + pstats->tx_bytes += sz; + #if defined(CONFIG_CHECK_LEAVE_LPS) && defined(CONFIG_LPS_CHK_BY_TP) + if (adapter_to_pwrctl(padapter)->lps_chk_by_tp) + traffic_check_for_leave_lps_by_tp(padapter, _TRUE, psta); + #endif /* CONFIG_LPS */ + } + +#ifdef CONFIG_CHECK_LEAVE_LPS + /* traffic_check_for_leave_lps(padapter, _TRUE); */ +#endif /* CONFIG_CHECK_LEAVE_LPS */ + + } +} + +static struct xmit_buf *__rtw_alloc_cmd_xmitbuf(struct xmit_priv *pxmitpriv, + enum cmdbuf_type buf_type) +{ + struct xmit_buf *pxmitbuf = NULL; + + + pxmitbuf = &pxmitpriv->pcmd_xmitbuf[buf_type]; + if (pxmitbuf != NULL) { + pxmitbuf->priv_data = NULL; + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->agg_num = 0; + pxmitbuf->pg_num = 0; +#endif +#ifdef CONFIG_PCI_HCI + pxmitbuf->len = 0; +#ifdef CONFIG_TRX_BD_ARCH + /*pxmitbuf->buf_desc = NULL;*/ +#else + pxmitbuf->desc = NULL; +#endif +#endif + + if (pxmitbuf->sctx) { + RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + } else + RTW_INFO("%s fail, no xmitbuf available !!!\n", __func__); + + return pxmitbuf; +} + +struct xmit_frame *__rtw_alloc_cmdxmitframe(struct xmit_priv *pxmitpriv, + enum cmdbuf_type buf_type) +{ + struct xmit_frame *pcmdframe; + struct xmit_buf *pxmitbuf; + + pcmdframe = rtw_alloc_xmitframe(pxmitpriv, 0); + if (pcmdframe == NULL) { + RTW_INFO("%s, alloc xmitframe fail\n", __FUNCTION__); + return NULL; + } + + pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type); + if (pxmitbuf == NULL) { + RTW_INFO("%s, alloc xmitbuf fail\n", __FUNCTION__); + rtw_free_xmitframe(pxmitpriv, pcmdframe); + return NULL; + } + + pcmdframe->frame_tag = MGNT_FRAMETAG; + + pcmdframe->pxmitbuf = pxmitbuf; + + pcmdframe->buf_addr = pxmitbuf->pbuf; + + /* initial memory to zero */ + _rtw_memset(pcmdframe->buf_addr, 0, MAX_CMDBUF_SZ); + + pxmitbuf->priv_data = pcmdframe; + + return pcmdframe; + +} + +struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv) +{ + _irqL irqL; + struct xmit_buf *pxmitbuf = NULL; + _list *plist, *phead; + _queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue; + + + _enter_critical(&pfree_queue->lock, &irqL); + + if (_rtw_queue_empty(pfree_queue) == _TRUE) + pxmitbuf = NULL; + else { + + phead = get_list_head(pfree_queue); + + plist = get_next(phead); + + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + + rtw_list_delete(&(pxmitbuf->list)); + } + + if (pxmitbuf != NULL) { + pxmitpriv->free_xmit_extbuf_cnt--; +#ifdef DBG_XMIT_BUF_EXT + RTW_INFO("DBG_XMIT_BUF_EXT ALLOC no=%d, free_xmit_extbuf_cnt=%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt); +#endif + + + pxmitbuf->priv_data = NULL; + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->agg_num = 1; +#endif +#ifdef CONFIG_PCI_HCI + pxmitbuf->len = 0; +#ifdef CONFIG_TRX_BD_ARCH + /*pxmitbuf->buf_desc = NULL;*/ +#else + pxmitbuf->desc = NULL; +#endif +#endif + + if (pxmitbuf->sctx) { + RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + + } + + _exit_critical(&pfree_queue->lock, &irqL); + + + return pxmitbuf; +} + +s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf) +{ + _irqL irqL; + _queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue; + + + if (pxmitbuf == NULL) + return _FAIL; + + _enter_critical(&pfree_queue->lock, &irqL); + + rtw_list_delete(&pxmitbuf->list); + + rtw_list_insert_tail(&(pxmitbuf->list), get_list_head(pfree_queue)); + pxmitpriv->free_xmit_extbuf_cnt++; +#ifdef DBG_XMIT_BUF_EXT + RTW_INFO("DBG_XMIT_BUF_EXT FREE no=%d, free_xmit_extbuf_cnt=%d\n", pxmitbuf->no , pxmitpriv->free_xmit_extbuf_cnt); +#endif + + _exit_critical(&pfree_queue->lock, &irqL); + + + return _SUCCESS; +} + +struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv) +{ + _irqL irqL; + struct xmit_buf *pxmitbuf = NULL; + _list *plist, *phead; + _queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue; + + + /* RTW_INFO("+rtw_alloc_xmitbuf\n"); */ + + _enter_critical(&pfree_xmitbuf_queue->lock, &irqL); + + if (_rtw_queue_empty(pfree_xmitbuf_queue) == _TRUE) + pxmitbuf = NULL; + else { + + phead = get_list_head(pfree_xmitbuf_queue); + + plist = get_next(phead); + + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + + rtw_list_delete(&(pxmitbuf->list)); + } + + if (pxmitbuf != NULL) { + pxmitpriv->free_xmitbuf_cnt--; +#ifdef DBG_XMIT_BUF + RTW_INFO("DBG_XMIT_BUF ALLOC no=%d, free_xmitbuf_cnt=%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt); +#endif + /* RTW_INFO("alloc, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt); */ + + pxmitbuf->priv_data = NULL; + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxmitbuf->len = 0; + pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead; + pxmitbuf->agg_num = 0; + pxmitbuf->pg_num = 0; +#endif +#ifdef CONFIG_PCI_HCI + pxmitbuf->len = 0; +#ifdef CONFIG_TRX_BD_ARCH + /*pxmitbuf->buf_desc = NULL;*/ +#else + pxmitbuf->desc = NULL; +#endif +#endif + + if (pxmitbuf->sctx) { + RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC); + } + } +#ifdef DBG_XMIT_BUF + else + RTW_INFO("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n"); +#endif + + _exit_critical(&pfree_xmitbuf_queue->lock, &irqL); + + + return pxmitbuf; +} + +s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf) +{ + _irqL irqL; + _queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue; + + + /* RTW_INFO("+rtw_free_xmitbuf\n"); */ + + if (pxmitbuf == NULL) + return _FAIL; + + if (pxmitbuf->sctx) { + RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__); + rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE); + } + + if (pxmitbuf->buf_tag == XMITBUF_CMD) { + } else if (pxmitbuf->buf_tag == XMITBUF_MGNT) + rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf); + else { + _enter_critical(&pfree_xmitbuf_queue->lock, &irqL); + + rtw_list_delete(&pxmitbuf->list); + + rtw_list_insert_tail(&(pxmitbuf->list), get_list_head(pfree_xmitbuf_queue)); + + pxmitpriv->free_xmitbuf_cnt++; + /* RTW_INFO("FREE, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt); */ +#ifdef DBG_XMIT_BUF + RTW_INFO("DBG_XMIT_BUF FREE no=%d, free_xmitbuf_cnt=%d\n", pxmitbuf->no , pxmitpriv->free_xmitbuf_cnt); +#endif + _exit_critical(&pfree_xmitbuf_queue->lock, &irqL); + } + + + return _SUCCESS; +} + +void rtw_init_xmitframe(struct xmit_frame *pxframe) +{ + if (pxframe != NULL) { /* default value setting */ + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + _rtw_memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib)); + /* pxframe->attrib.psta = NULL; */ + + pxframe->frame_tag = DATA_FRAMETAG; + +#ifdef CONFIG_USB_HCI + pxframe->pkt = NULL; +#ifdef USB_PACKET_OFFSET_SZ + pxframe->pkt_offset = (PACKET_OFFSET_SZ / 8); +#else + pxframe->pkt_offset = 1;/* default use pkt_offset to fill tx desc */ +#endif + +#ifdef CONFIG_USB_TX_AGGREGATION + pxframe->agg_num = 1; +#endif + +#endif /* #ifdef CONFIG_USB_HCI */ + +#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI) + pxframe->pg_num = 1; + pxframe->agg_num = 1; +#endif + +#ifdef CONFIG_XMIT_ACK + pxframe->ack_report = 0; +#endif + + } +} + +/* +Calling context: +1. OS_TXENTRY +2. RXENTRY (rx_thread or RX_ISR/RX_CallBack) + +If we turn on USE_RXTHREAD, then, no need for critical section. +Otherwise, we must use _enter/_exit critical to protect free_xmit_queue... + +Must be very very cautious... + +*/ +struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv, u16 os_qid) +{ + /* + Please remember to use all the osdep_service api, + and lock/unlock or _enter/_exit critical to protect + pfree_xmit_queue + */ + + _irqL irqL; + struct xmit_frame *pxframe = NULL; + _list *plist, *phead; + _queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue; + + _enter_critical_bh(&pfree_xmit_queue->lock, &irqL); + + if (_rtw_queue_empty(pfree_xmit_queue) == _TRUE) { + pxframe = NULL; + } else { + phead = get_list_head(pfree_xmit_queue); + + plist = get_next(phead); + + pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + rtw_list_delete(&(pxframe->list)); + pxmitpriv->free_xmitframe_cnt--; + pxframe->os_qid = os_qid; + } + + _exit_critical_bh(&pfree_xmit_queue->lock, &irqL); + + if (pxframe) + rtw_os_check_stop_queue(pxmitpriv->adapter, os_qid); + + rtw_init_xmitframe(pxframe); + + + return pxframe; +} + +struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv) +{ + _irqL irqL; + struct xmit_frame *pxframe = NULL; + _list *plist, *phead; + _queue *queue = &pxmitpriv->free_xframe_ext_queue; + + + _enter_critical_bh(&queue->lock, &irqL); + + if (_rtw_queue_empty(queue) == _TRUE) { + pxframe = NULL; + } else { + phead = get_list_head(queue); + plist = get_next(phead); + pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + rtw_list_delete(&(pxframe->list)); + pxmitpriv->free_xframe_ext_cnt--; + } + + _exit_critical_bh(&queue->lock, &irqL); + + rtw_init_xmitframe(pxframe); + + + return pxframe; +} + +struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv) +{ + struct xmit_frame *pxframe = NULL; + u8 *alloc_addr; + + alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4); + + if (alloc_addr == NULL) + goto exit; + + pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4); + pxframe->alloc_addr = alloc_addr; + + pxframe->padapter = pxmitpriv->adapter; + pxframe->frame_tag = NULL_FRAMETAG; + + pxframe->pkt = NULL; + + pxframe->buf_addr = NULL; + pxframe->pxmitbuf = NULL; + + rtw_init_xmitframe(pxframe); + + RTW_INFO("################## %s ##################\n", __func__); + +exit: + return pxframe; +} + +s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe) +{ + _irqL irqL; + _queue *queue = NULL; + _adapter *padapter = pxmitpriv->adapter; + _pkt *pndis_pkt = NULL; + + + if (pxmitframe == NULL) { + goto exit; + } + + if (pxmitframe->pkt) { + pndis_pkt = pxmitframe->pkt; + pxmitframe->pkt = NULL; + } + + if (pxmitframe->alloc_addr) { + RTW_INFO("################## %s with alloc_addr ##################\n", __func__); + rtw_mfree(pxmitframe->alloc_addr, sizeof(struct xmit_frame) + 4); + goto check_pkt_complete; + } + + if (pxmitframe->ext_tag == 0) + queue = &pxmitpriv->free_xmit_queue; + else if (pxmitframe->ext_tag == 1) + queue = &pxmitpriv->free_xframe_ext_queue; + else + rtw_warn_on(1); + + _enter_critical_bh(&queue->lock, &irqL); + + rtw_list_delete(&pxmitframe->list); + rtw_list_insert_tail(&pxmitframe->list, get_list_head(queue)); + if (pxmitframe->ext_tag == 0) { + pxmitpriv->free_xmitframe_cnt++; + } else if (pxmitframe->ext_tag == 1) { + pxmitpriv->free_xframe_ext_cnt++; + } else { + } + + _exit_critical_bh(&queue->lock, &irqL); + + if (queue == &pxmitpriv->free_xmit_queue) + rtw_os_check_wakup_queue(padapter, pxmitframe->os_qid); + +check_pkt_complete: + + if (pndis_pkt) + rtw_os_pkt_complete(padapter, pndis_pkt); + +exit: + + + return _SUCCESS; +} + +#ifdef CONFIG_RTW_MGMT_QUEUE +void rtw_free_mgmt_xmitframe_queue(struct xmit_priv *pxmitpriv, _queue *mgmt_queue) +{ + _irqL irqL; + _list *plist, *phead; + struct xmit_frame *pxmitframe; + + _enter_critical_bh(&(mgmt_queue->lock), &irqL); + + phead = get_list_head(mgmt_queue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + + pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + plist = get_next(plist); + + #ifdef DBG_MGMT_QUEUE + RTW_INFO("%s seq_num = %u\n", __func__, pxmitframe->attrib.seqnum); + #endif + + rtw_free_xmitbuf_ext(pxmitpriv, pxmitframe->pxmitbuf); + rtw_free_xmitframe(pxmitpriv, pxmitframe); + } + _exit_critical_bh(&(mgmt_queue->lock), &irqL); +} + +u8 rtw_mgmt_xmitframe_enqueue(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + struct sta_info *psta; + struct tx_servq *ptxservq; + struct pkt_attrib *pattrib = &(pxmitframe->attrib); + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct hw_xmit *phwxmits = pxmitpriv->hwxmits; + u8 mgmt_idx = pxmitpriv->hwxmit_entry - 1; + + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class); + + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_sta); + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return _FAIL; + } + + if (psta == NULL) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta); + RTW_INFO("rtw_xmit_classifier: psta == NULL\n"); + return _FAIL; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink); + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FAIL; + } + + ptxservq = &(psta->sta_xmitpriv.mgmt_q); + + if (rtw_is_list_empty(&ptxservq->tx_pending)) + rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(phwxmits[mgmt_idx].sta_queue)); + + rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending)); + ptxservq->qcnt++; + phwxmits[mgmt_idx].accnt++; + + return _SUCCESS; +} +#endif + +void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, _queue *pframequeue) +{ + _irqL irqL; + _list *plist, *phead; + struct xmit_frame *pxmitframe; + + + _enter_critical_bh(&(pframequeue->lock), &irqL); + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + + pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + plist = get_next(plist); + + rtw_free_xmitframe(pxmitpriv, pxmitframe); + + } + _exit_critical_bh(&(pframequeue->lock), &irqL); + +} + +s32 rtw_xmitframe_enqueue(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue); + if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) { + /* pxmitframe->pkt = NULL; */ + return _FAIL; + } + + return _SUCCESS; +} + +static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, _queue *pframe_queue) +{ + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + + xmitframe_phead = get_list_head(pframe_queue); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + /* xmitframe_plist = get_next(xmitframe_plist); */ + + /*#ifdef RTK_DMP_PLATFORM + #ifdef CONFIG_USB_TX_AGGREGATION + if((ptxservq->qcnt>0) && (ptxservq->qcnt<=2)) + { + pxmitframe = NULL; + + tasklet_schedule(&pxmitpriv->xmit_tasklet); + + break; + } + #endif + #endif*/ + rtw_list_delete(&pxmitframe->list); + + ptxservq->qcnt--; + + /* rtw_list_insert_tail(&pxmitframe->list, &phwxmit->pending); */ + + /* ptxservq->qcnt--; */ + + break; + + /* pxmitframe = NULL; */ + + } + + return pxmitframe; +} + +static struct xmit_frame *get_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, _queue *pframe_queue) +{ + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + + xmitframe_phead = get_list_head(pframe_queue); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + break; + } + + return pxmitframe; +} + +struct xmit_frame *rtw_get_xframe(struct xmit_priv *pxmitpriv, int *num_frame) +{ + _irqL irqL0; + _list *sta_plist, *sta_phead; + struct hw_xmit *phwxmit_i = pxmitpriv->hwxmits; +#ifdef CONFIG_RTW_MGMT_QUEUE + /* This function gets xmit_frame from AC queue. */ + /* When mgmt queue is used, AC queue index is (hwxmit_entry - 1) */ + sint entry = pxmitpriv->hwxmit_entry - 1; +#else + sint entry = pxmitpriv->hwxmit_entry; +#endif + struct hw_xmit *phwxmit; + struct tx_servq *ptxservq = NULL; + _queue *pframe_queue = NULL; + struct xmit_frame *pxmitframe = NULL; + _adapter *padapter = pxmitpriv->adapter; + struct registry_priv *pregpriv = &padapter->registrypriv; + int i, inx[4]; + + inx[0] = 0; + inx[1] = 1; + inx[2] = 2; + inx[3] = 3; + + *num_frame = 0; + + /*No amsdu when wifi_spec on*/ + if (pregpriv->wifi_spec == 1) { + return NULL; + } + + _enter_critical_bh(&pxmitpriv->lock, &irqL0); + + for (i = 0; i < entry; i++) { + phwxmit = phwxmit_i + inx[i]; + + sta_phead = get_list_head(phwxmit->sta_queue); + sta_plist = get_next(sta_phead); + + while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == _FALSE) { + + ptxservq = LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending); + pframe_queue = &ptxservq->sta_pending; + + if(ptxservq->qcnt) + { + *num_frame = ptxservq->qcnt; + pxmitframe = get_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue); + goto exit; + } + sta_plist = get_next(sta_plist); + } + } + +exit: + + _exit_critical_bh(&pxmitpriv->lock, &irqL0); + + return pxmitframe; +} + +#ifdef CONFIG_RTW_MGMT_QUEUE +struct xmit_frame *rtw_dequeue_mgmt_xframe(struct xmit_priv *pxmitpriv) +{ + _irqL irqL0; + _list *sta_plist, *sta_phead; + struct hw_xmit *mgmt_hwxmit; + struct tx_servq *ptxservq = NULL; + _queue *pframe_queue = NULL; + struct xmit_frame *pxmitframe = NULL; + u8 mgmt_entry = pxmitpriv->hwxmit_entry - 1; + + _enter_critical_bh(&pxmitpriv->lock, &irqL0); + + /* management queue */ + mgmt_hwxmit = (pxmitpriv->hwxmits) + mgmt_entry; + + sta_phead = get_list_head(mgmt_hwxmit->sta_queue); + sta_plist = get_next(sta_phead); + + while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == _FALSE) { + + ptxservq = LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending); + + pframe_queue = &ptxservq->sta_pending; + + pxmitframe = dequeue_one_xmitframe(pxmitpriv, mgmt_hwxmit, ptxservq, pframe_queue); + + #ifdef DBG_MGMT_QUEUE + RTW_INFO("%s dequeue mgmt frame (seq_num = %u) to TX\n", __func__, pxmitframe->attrib.seqnum); + #endif + + if (pxmitframe) { + mgmt_hwxmit->accnt--; + + /* Remove sta node when there is no pending packets. */ + if (_rtw_queue_empty(pframe_queue)) /* must be done after get_next and before break */ + rtw_list_delete(&ptxservq->tx_pending); + + goto exit; + } + sta_plist = get_next(sta_plist); + } +exit: + _exit_critical_bh(&pxmitpriv->lock, &irqL0); + + return pxmitframe; +} +#endif + +struct xmit_frame *rtw_dequeue_xframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, sint entry) +{ + _irqL irqL0; + _list *sta_plist, *sta_phead; + struct hw_xmit *phwxmit; + struct tx_servq *ptxservq = NULL; + _queue *pframe_queue = NULL; + struct xmit_frame *pxmitframe = NULL; + _adapter *padapter = pxmitpriv->adapter; + struct registry_priv *pregpriv = &padapter->registrypriv; + int i, inx[4]; +#ifdef CONFIG_RTW_MGMT_QUEUE + /* This function gets xmit_frame from AC queue. */ + /* When mgmt queue is used, AC queue index is (hwxmit_entry - 1) */ + entry--; +#endif + inx[0] = 0; + inx[1] = 1; + inx[2] = 2; + inx[3] = 3; + + if (pregpriv->wifi_spec == 1) { + int j; +#if 0 + if (flags < XMIT_QUEUE_ENTRY) { + /* priority exchange according to the completed xmitbuf flags. */ + inx[flags] = 0; + inx[0] = flags; + } +#endif + +#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_PCI_HCI) + for (j = 0; j < 4; j++) + inx[j] = pxmitpriv->wmm_para_seq[j]; +#endif + } + + _enter_critical_bh(&pxmitpriv->lock, &irqL0); + + for (i = 0; i < entry; i++) { + phwxmit = phwxmit_i + inx[i]; + + /* _enter_critical_ex(&phwxmit->sta_queue->lock, &irqL0); */ + + sta_phead = get_list_head(phwxmit->sta_queue); + sta_plist = get_next(sta_phead); + + while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == _FALSE) { + + ptxservq = LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending); + + pframe_queue = &ptxservq->sta_pending; + + pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue); + + if (pxmitframe) { + phwxmit->accnt--; + + /* Remove sta node when there is no pending packets. */ + if (_rtw_queue_empty(pframe_queue)) /* must be done after get_next and before break */ + rtw_list_delete(&ptxservq->tx_pending); + + /* _exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0); */ + + goto exit; + } + + sta_plist = get_next(sta_plist); + + } + + /* _exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0); */ + + } + +exit: + + _exit_critical_bh(&pxmitpriv->lock, &irqL0); + + return pxmitframe; +} + +struct tx_servq *rtw_get_sta_pending(_adapter *padapter, struct sta_info *psta, sint up, u8 *ac) +{ + struct tx_servq *ptxservq = NULL; + + + switch (up) { + case 1: + case 2: + ptxservq = &(psta->sta_xmitpriv.bk_q); + *(ac) = 3; + break; + + case 4: + case 5: + ptxservq = &(psta->sta_xmitpriv.vi_q); + *(ac) = 1; + break; + + case 6: + case 7: + ptxservq = &(psta->sta_xmitpriv.vo_q); + *(ac) = 0; + break; + + case 0: + case 3: + default: + ptxservq = &(psta->sta_xmitpriv.be_q); + *(ac) = 2; + break; + + } + + + return ptxservq; +} + +/* + * Will enqueue pxmitframe to the proper queue, + * and indicate it to xx_pending list..... + */ +s32 rtw_xmit_classifier(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + /* _irqL irqL0; */ + u8 ac_index; + struct sta_info *psta; + struct tx_servq *ptxservq; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits; + sint res = _SUCCESS; + + + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class); + + /* + if (pattrib->psta) { + psta = pattrib->psta; + } else { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + psta = rtw_get_stainfo(pstapriv, pattrib->ra); + } + */ + + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_sta); + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return _FAIL; + } + + if (psta == NULL) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta); + res = _FAIL; + RTW_INFO("rtw_xmit_classifier: psta == NULL\n"); + goto exit; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink); + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FAIL; + } + + ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index)); + + /* _enter_critical(&pstapending->lock, &irqL0); */ + + if (rtw_is_list_empty(&ptxservq->tx_pending)) + rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue)); + + /* _enter_critical(&ptxservq->sta_pending.lock, &irqL1); */ + + rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending)); + ptxservq->qcnt++; + phwxmits[ac_index].accnt++; + + /* _exit_critical(&ptxservq->sta_pending.lock, &irqL1); */ + + /* _exit_critical(&pstapending->lock, &irqL0); */ + +exit: + + + return res; +} + +void rtw_alloc_hwxmits(_adapter *padapter) +{ + struct hw_xmit *hwxmits; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + pxmitpriv->hwxmit_entry = HWXMIT_ENTRY; + + pxmitpriv->hwxmits = NULL; + + pxmitpriv->hwxmits = (struct hw_xmit *)rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry); + + if (pxmitpriv->hwxmits == NULL) { + RTW_INFO("alloc hwxmits fail!...\n"); + return; + } + + hwxmits = pxmitpriv->hwxmits; + + rtw_warn_on(pxmitpriv->hwxmit_entry < 4); + + /* pxmitpriv->vo_txqueue.head = 0; */ + /* hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue; */ + hwxmits[0].sta_queue = &pxmitpriv->vo_pending; + + /* pxmitpriv->vi_txqueue.head = 0; */ + /* hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue; */ + hwxmits[1].sta_queue = &pxmitpriv->vi_pending; + + /* pxmitpriv->be_txqueue.head = 0; */ + /* hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue; */ + hwxmits[2].sta_queue = &pxmitpriv->be_pending; + + /* pxmitpriv->bk_txqueue.head = 0; */ + /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */ + hwxmits[3].sta_queue = &pxmitpriv->bk_pending; + +#ifdef CONFIG_RTW_MGMT_QUEUE + hwxmits[4].sta_queue = &pxmitpriv->mgmt_pending; +#endif + +} + +void rtw_free_hwxmits(_adapter *padapter) +{ + struct hw_xmit *hwxmits; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + hwxmits = pxmitpriv->hwxmits; + if (hwxmits) + rtw_mfree((u8 *)hwxmits, (sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry)); +} + +void rtw_init_hwxmits(struct hw_xmit *phwxmit, sint entry) +{ + sint i; + for (i = 0; i < entry; i++, phwxmit++) { + /* _rtw_spinlock_init(&phwxmit->xmit_lock); */ + /* _rtw_init_listhead(&phwxmit->pending); */ + /* phwxmit->txcmdcnt = 0; */ + phwxmit->accnt = 0; + } +} + +#ifdef CONFIG_BR_EXT +int rtw_br_client_tx(_adapter *padapter, struct sk_buff **pskb) +{ + struct sk_buff *skb = *pskb; + _irqL irqL; + /* if(check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE) */ + { + void dhcp_flag_bcast(_adapter *priv, struct sk_buff *skb); + int res, is_vlan_tag = 0, i, do_nat25 = 1; + unsigned short vlan_hdr = 0; + void *br_port = NULL; + + /* mac_clone_handle_frame(priv, skb); */ + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) + br_port = padapter->pnetdev->br_port; +#else /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */ + rcu_read_lock(); + br_port = rcu_dereference(padapter->pnetdev->rx_handler_data); + rcu_read_unlock(); +#endif /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */ + _enter_critical_bh(&padapter->br_ext_lock, &irqL); + if (!(skb->data[0] & 1) && + br_port && + memcmp(skb->data + MACADDRLEN, padapter->br_mac, MACADDRLEN) && + *((unsigned short *)(skb->data + MACADDRLEN * 2)) != __constant_htons(ETH_P_8021Q) && + *((unsigned short *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_IP) && + !memcmp(padapter->scdb_mac, skb->data + MACADDRLEN, MACADDRLEN) && padapter->scdb_entry) { + memcpy(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN); + padapter->scdb_entry->ageing_timer = jiffies; + _exit_critical_bh(&padapter->br_ext_lock, &irqL); + } else + /* if (!priv->pmib->ethBrExtInfo.nat25_disable) */ + { + /* if (priv->dev->br_port && + * !memcmp(skb->data+MACADDRLEN, priv->br_mac, MACADDRLEN)) { */ +#if 1 + if (*((unsigned short *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_8021Q)) { + is_vlan_tag = 1; + vlan_hdr = *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2)); + for (i = 0; i < 6; i++) + *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + MACADDRLEN * 2 - 2 - i * 2)); + skb_pull(skb, 4); + } + /* if SA == br_mac && skb== IP => copy SIP to br_ip ?? why */ + if (!memcmp(skb->data + MACADDRLEN, padapter->br_mac, MACADDRLEN) && + (*((unsigned short *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_IP))) + memcpy(padapter->br_ip, skb->data + WLAN_ETHHDR_LEN + 12, 4); + + if (*((unsigned short *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_IP)) { + if (memcmp(padapter->scdb_mac, skb->data + MACADDRLEN, MACADDRLEN)) { + void *scdb_findEntry(_adapter *priv, unsigned char *macAddr, unsigned char *ipAddr); + + padapter->scdb_entry = (struct nat25_network_db_entry *)scdb_findEntry(padapter, + skb->data + MACADDRLEN, skb->data + WLAN_ETHHDR_LEN + 12); + if (padapter->scdb_entry != NULL) { + memcpy(padapter->scdb_mac, skb->data + MACADDRLEN, MACADDRLEN); + memcpy(padapter->scdb_ip, skb->data + WLAN_ETHHDR_LEN + 12, 4); + padapter->scdb_entry->ageing_timer = jiffies; + do_nat25 = 0; + } + } else { + if (padapter->scdb_entry) { + padapter->scdb_entry->ageing_timer = jiffies; + do_nat25 = 0; + } else { + memset(padapter->scdb_mac, 0, MACADDRLEN); + memset(padapter->scdb_ip, 0, 4); + } + } + } + _exit_critical_bh(&padapter->br_ext_lock, &irqL); +#endif /* 1 */ + if (do_nat25) { + int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method); + if (nat25_db_handle(padapter, skb, NAT25_CHECK) == 0) { + struct sk_buff *newskb; + + if (is_vlan_tag) { + skb_push(skb, 4); + for (i = 0; i < 6; i++) + *((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2)); + *((unsigned short *)(skb->data + MACADDRLEN * 2)) = __constant_htons(ETH_P_8021Q); + *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2)) = vlan_hdr; + } + + newskb = rtw_skb_copy(skb); + if (newskb == NULL) { + /* priv->ext_stats.tx_drops++; */ + DEBUG_ERR("TX DROP: rtw_skb_copy fail!\n"); + /* goto stop_proc; */ + return -1; + } + rtw_skb_free(skb); + + *pskb = skb = newskb; + if (is_vlan_tag) { + vlan_hdr = *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2)); + for (i = 0; i < 6; i++) + *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + MACADDRLEN * 2 - 2 - i * 2)); + skb_pull(skb, 4); + } + } + + if (skb_is_nonlinear(skb)) + DEBUG_ERR("%s(): skb_is_nonlinear!!\n", __FUNCTION__); + + +#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)) + res = skb_linearize(skb, GFP_ATOMIC); +#else /* (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)) */ + res = skb_linearize(skb); +#endif /* (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)) */ + if (res < 0) { + DEBUG_ERR("TX DROP: skb_linearize fail!\n"); + /* goto free_and_stop; */ + return -1; + } + + res = nat25_db_handle(padapter, skb, NAT25_INSERT); + if (res < 0) { + if (res == -2) { + /* priv->ext_stats.tx_drops++; */ + DEBUG_ERR("TX DROP: nat25_db_handle fail!\n"); + /* goto free_and_stop; */ + return -1; + + } + /* we just print warning message and let it go */ + /* DEBUG_WARN("%s()-%d: nat25_db_handle INSERT Warning!\n", __FUNCTION__, __LINE__); */ + /* return -1; */ /* return -1 will cause system crash on 2011/08/30! */ + return 0; + } + } + + memcpy(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN); + + dhcp_flag_bcast(padapter, skb); + + if (is_vlan_tag) { + skb_push(skb, 4); + for (i = 0; i < 6; i++) + *((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2)); + *((unsigned short *)(skb->data + MACADDRLEN * 2)) = __constant_htons(ETH_P_8021Q); + *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2)) = vlan_hdr; + } + } +#if 0 + else { + if (*((unsigned short *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_8021Q)) + is_vlan_tag = 1; + + if (is_vlan_tag) { + if (ICMPV6_MCAST_MAC(skb->data) && ICMPV6_PROTO1A_VALN(skb->data)) + memcpy(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN); + } else { + if (ICMPV6_MCAST_MAC(skb->data) && ICMPV6_PROTO1A(skb->data)) + memcpy(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN); + } + } +#endif /* 0 */ + + /* check if SA is equal to our MAC */ + if (memcmp(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN)) { + /* priv->ext_stats.tx_drops++; */ + DEBUG_ERR("TX DROP: untransformed frame SA:%02X%02X%02X%02X%02X%02X!\n", + skb->data[6], skb->data[7], skb->data[8], skb->data[9], skb->data[10], skb->data[11]); + /* goto free_and_stop; */ + return -1; + } + } + return 0; +} +#endif /* CONFIG_BR_EXT */ + +u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe) +{ + u32 addr; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + + switch (pattrib->qsel) { + case 0: + case 3: + addr = BE_QUEUE_INX; + break; + case 1: + case 2: + addr = BK_QUEUE_INX; + break; + case 4: + case 5: + addr = VI_QUEUE_INX; + break; + case 6: + case 7: + addr = VO_QUEUE_INX; + break; + case 0x10: + addr = BCN_QUEUE_INX; + break; + case 0x11: /* BC/MC in PS (HIQ) */ + addr = HIGH_QUEUE_INX; + break; + case 0x13: + addr = TXCMD_QUEUE_INX; + break; + case 0x12: + default: + addr = MGT_QUEUE_INX; + break; + + } + + return addr; + +} + +static void do_queue_select(_adapter *padapter, struct pkt_attrib *pattrib) +{ + u8 qsel; + + qsel = pattrib->priority; + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(padapter)) { + /* Under MCC */ + if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_NEED_MCC)) { + if (padapter->mcc_adapterpriv.role == MCC_ROLE_GO + || padapter->mcc_adapterpriv.role == MCC_ROLE_AP) { + pattrib->qsel = QSLT_VO; /* AP interface VO queue */ + pattrib->priority = QSLT_VO; + } else { + pattrib->qsel = QSLT_BE; /* STA interface BE queue */ + pattrib->priority = QSLT_BE; + } + } else + /* Not Under MCC */ + pattrib->qsel = qsel; + } else + /* Not enable MCC */ + pattrib->qsel = qsel; +#else /* !CONFIG_MCC_MODE */ + pattrib->qsel = qsel; +#endif /* CONFIG_MCC_MODE */ + + /* high priority packet */ + if (pattrib->hipriority_pkt) { + pattrib->qsel = QSLT_VO; + pattrib->priority = QSLT_VO; + } +} + +/* + * The main transmit(tx) entry + * + * Return + * 1 enqueue + * 0 success, hardware will handle this xmit frame(packet) + * <0 fail + */ + #if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)) +s32 rtw_monitor_xmit_entry(struct sk_buff *skb, struct net_device *ndev) +{ + u16 frame_ctl; + struct ieee80211_radiotap_header rtap_hdr; + _adapter *padapter = (_adapter *)rtw_netdev_priv(ndev); + struct pkt_file pktfile; + struct rtw_ieee80211_hdr *pwlanhdr; + struct pkt_attrib *pattrib; + struct xmit_frame *pmgntframe; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + unsigned char *pframe; + u8 dummybuf[32]; + int len = skb->len, rtap_len; + + + rtw_mstat_update(MSTAT_TYPE_SKB, MSTAT_ALLOC_SUCCESS, skb->truesize); + +#ifndef CONFIG_CUSTOMER_ALIBABA_GENERAL + if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header))) + goto fail; + + _rtw_open_pktfile((_pkt *)skb, &pktfile); + _rtw_pktfile_read(&pktfile, (u8 *)(&rtap_hdr), sizeof(struct ieee80211_radiotap_header)); + rtap_len = ieee80211_get_radiotap_len((u8 *)(&rtap_hdr)); + if (unlikely(rtap_hdr.it_version)) + goto fail; + + if (unlikely(skb->len < rtap_len)) + goto fail; + +#ifndef CONFIG_MONITOR_MODE_XMIT + if (rtap_len != 12) { + RTW_INFO("radiotap len (should be 14): %d\n", rtap_len); + goto fail; + } +#endif /* CONFIG_MONITOR_MODE_XMIT */ + _rtw_pktfile_read(&pktfile, dummybuf, rtap_len-sizeof(struct ieee80211_radiotap_header)); + len = len - rtap_len; +#endif + pmgntframe = alloc_mgtxmitframe(pxmitpriv); + if (pmgntframe == NULL) { + rtw_udelay_os(500); + goto fail; + } + + _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; +// _rtw_memcpy(pframe, (void *)checking, len); + _rtw_pktfile_read(&pktfile, pframe, len); + + + /* Check DATA/MGNT frames */ + pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; + frame_ctl = le16_to_cpu(pwlanhdr->frame_ctl); + if ((frame_ctl & RTW_IEEE80211_FCTL_FTYPE) == RTW_IEEE80211_FTYPE_DATA) { + + pattrib = &pmgntframe->attrib; + update_monitor_frame_attrib(padapter, pattrib); + + if (is_broadcast_mac_addr(pwlanhdr->addr3) || is_broadcast_mac_addr(pwlanhdr->addr1)) + pattrib->rate = MGN_24M; + + } else { + + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib(padapter, pattrib); + + } + pattrib->retry_ctrl = _FALSE; + pattrib->pktlen = len; + pmlmeext->mgnt_seq = GetSequence(pwlanhdr); + pattrib->seqnum = pmlmeext->mgnt_seq; + pmlmeext->mgnt_seq++; + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe(padapter, pmgntframe); + +fail: + rtw_skb_free(skb); + return 0; +} +#endif + +/* + * + * Return _TRUE when frame has been put to queue, otherwise return _FALSE. + */ +static u8 xmit_enqueue(struct _ADAPTER *a, struct xmit_frame *frame) +{ + struct sta_info *sta = NULL; + struct pkt_attrib *attrib = NULL; + _irqL irqL; + _list *head; + u8 ret = _TRUE; + + + attrib = &frame->attrib; + sta = attrib->psta; + if (!sta) + return _FALSE; + + _enter_critical_bh(&sta->tx_queue.lock, &irqL); + + head = get_list_head(&sta->tx_queue); + + if ((rtw_is_list_empty(head) == _TRUE) && (!sta->tx_q_enable)) { + ret = _FALSE; + goto exit; + } + + rtw_list_insert_tail(&frame->list, head); + RTW_INFO(FUNC_ADPT_FMT ": en-queue tx pkt for macid=%d\n", + FUNC_ADPT_ARG(a), sta->cmn.mac_id); + +exit: + _exit_critical_bh(&sta->tx_queue.lock, &irqL); + + return ret; +} + +static void xmit_dequeue(struct sta_info *sta) +{ + struct _ADAPTER *a; + _irqL irqL; + _list *head, *list; + struct xmit_frame *frame; + + + a = sta->padapter; + + _enter_critical_bh(&sta->tx_queue.lock, &irqL); + + head = get_list_head(&sta->tx_queue); + + do { + if (rtw_is_list_empty(head) == _TRUE) + break; + + list = get_next(head); + rtw_list_delete(list); + frame = LIST_CONTAINOR(list, struct xmit_frame, list); + RTW_INFO(FUNC_ADPT_FMT ": de-queue tx frame of macid=%d\n", + FUNC_ADPT_ARG(a), sta->cmn.mac_id); + + rtw_hal_xmit(a, frame); + } while (1); + + _exit_critical_bh(&sta->tx_queue.lock, &irqL); +} + +void rtw_xmit_dequeue_callback(_workitem *work) +{ + struct sta_info *sta; + + + sta = container_of(work, struct sta_info, tx_q_work); + xmit_dequeue(sta); +} + +void rtw_xmit_queue_set(struct sta_info *sta) +{ + _irqL irqL; + + _enter_critical_bh(&sta->tx_queue.lock, &irqL); + + if (sta->tx_q_enable) { + RTW_WARN(FUNC_ADPT_FMT ": duplicated set!\n", + FUNC_ADPT_ARG(sta->padapter)); + goto exit; + } + sta->tx_q_enable = 1; + RTW_INFO(FUNC_ADPT_FMT ": enable queue TX for macid=%d\n", + FUNC_ADPT_ARG(sta->padapter), sta->cmn.mac_id); + +exit: + _exit_critical_bh(&sta->tx_queue.lock, &irqL); +} + +void rtw_xmit_queue_clear(struct sta_info *sta) +{ + _irqL irqL; + + _enter_critical_bh(&sta->tx_queue.lock, &irqL); + + if (!sta->tx_q_enable) { + RTW_WARN(FUNC_ADPT_FMT ": tx queue for macid=%d " + "not be enabled!\n", + FUNC_ADPT_ARG(sta->padapter), sta->cmn.mac_id); + goto exit; + } + + sta->tx_q_enable = 0; + RTW_INFO(FUNC_ADPT_FMT ": disable queue TX for macid=%d\n", + FUNC_ADPT_ARG(sta->padapter), sta->cmn.mac_id); + + _set_workitem(&sta->tx_q_work); + +exit: + _exit_critical_bh(&sta->tx_queue.lock, &irqL); +} + +/* + * The main transmit(tx) entry post handle + * + * Return + * 1 enqueue + * 0 success, hardware will handle this xmit frame(packet) + * <0 fail + */ +s32 rtw_xmit_posthandle(_adapter *padapter, struct xmit_frame *pxmitframe, _pkt *pkt) +{ +#ifdef CONFIG_AP_MODE + _irqL irqL0; +#endif + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + s32 res; + + res = update_attrib(padapter, pkt, &pxmitframe->attrib); + +#ifdef CONFIG_MCC_MODE + /* record data kernel TX to driver to check MCC concurrent TX */ + rtw_hal_mcc_calc_tx_bytes_from_kernel(padapter, pxmitframe->attrib.pktlen); +#endif /* CONFIG_MCC_MODE */ + +#ifdef CONFIG_WAPI_SUPPORT + if (pxmitframe->attrib.ether_type != 0x88B4) { + if (rtw_wapi_drop_for_key_absent(padapter, pxmitframe->attrib.ra)) { + WAPI_TRACE(WAPI_RX, "drop for key absend when tx\n"); + res = _FAIL; + } + } +#endif + if (res == _FAIL) { + /*RTW_INFO("%s-"ADPT_FMT" update attrib fail\n", __func__, ADPT_ARG(padapter));*/ +#ifdef DBG_TX_DROP_FRAME + RTW_INFO("DBG_TX_DROP_FRAME %s update attrib fail\n", __FUNCTION__); +#endif + rtw_free_xmitframe(pxmitpriv, pxmitframe); + return -1; + } + pxmitframe->pkt = pkt; + + rtw_led_tx_control(padapter, pxmitframe->attrib.dst); + + do_queue_select(padapter, &pxmitframe->attrib); + +#ifdef CONFIG_AP_MODE + _enter_critical_bh(&pxmitpriv->lock, &irqL0); + if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == _TRUE) { + _exit_critical_bh(&pxmitpriv->lock, &irqL0); + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue); + return 1; + } + _exit_critical_bh(&pxmitpriv->lock, &irqL0); +#endif + + if (xmit_enqueue(padapter, pxmitframe) == _TRUE) + return 1; + + /* pre_xmitframe */ + if (rtw_hal_xmit(padapter, pxmitframe) == _FALSE) + return 1; + + return 0; +} + +/* + * The main transmit(tx) entry + * + * Return + * 1 enqueue + * 0 success, hardware will handle this xmit frame(packet) + * <0 fail + */ +s32 rtw_xmit(_adapter *padapter, _pkt **ppkt, u16 os_qid) +{ + static systime start = 0; + static u32 drop_cnt = 0; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_frame *pxmitframe = NULL; + s32 res; +#ifdef CONFIG_LAYER2_ROAMING + struct mlme_priv *pmlmepriv = &(padapter->mlmepriv); + struct sk_buff *skb = (struct sk_buff *)(*ppkt); + _irqL irqL; +#endif + + DBG_COUNTER(padapter->tx_logs.core_tx); + + if (IS_CH_WAITING(adapter_to_rfctl(padapter))) + return -1; + + if ((rtw_linked_check(padapter) == _FALSE) +#ifdef CONFIG_LAYER2_ROAMING + &&(!padapter->mlmepriv.roam_network) +#endif + ) + return -1; + + if (start == 0) + start = rtw_get_current_time(); + + pxmitframe = rtw_alloc_xmitframe(pxmitpriv, os_qid); + + if (rtw_get_passing_time_ms(start) > 2000) { + if (drop_cnt) + RTW_INFO("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __FUNCTION__, drop_cnt); + start = rtw_get_current_time(); + drop_cnt = 0; + } + + if (pxmitframe == NULL) { + drop_cnt++; + /*RTW_INFO("%s-"ADPT_FMT" no more xmitframe\n", __func__, ADPT_ARG(padapter));*/ + DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe); + return -1; + } + +#ifdef CONFIG_BR_EXT + if (!adapter_use_wds(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE) == _TRUE) { + void *br_port = NULL; + + #if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) + br_port = padapter->pnetdev->br_port; + #else + rcu_read_lock(); + br_port = rcu_dereference(padapter->pnetdev->rx_handler_data); + rcu_read_unlock(); + #endif + + if (br_port) { + res = rtw_br_client_tx(padapter, ppkt); + if (res == -1) { + rtw_free_xmitframe(pxmitpriv, pxmitframe); + DBG_COUNTER(padapter->tx_logs.core_tx_err_brtx); + return -1; + } + } + } +#endif /* CONFIG_BR_EXT */ +#ifdef CONFIG_LAYER2_ROAMING + if ((pmlmepriv->roam_network) && (skb->protocol != htons(0x888e))) { /* eapol never enqueue.*/ + pxmitframe->pkt = *ppkt; + rtw_list_delete(&pxmitframe->list); + _enter_critical_bh(&pxmitpriv->rpkt_queue.lock, &irqL); + rtw_list_insert_tail(&(pxmitframe->list), get_list_head(&(pxmitpriv->rpkt_queue))); + _exit_critical_bh(&pxmitpriv->rpkt_queue.lock, &irqL); + return 1; + } +#endif + +#if defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) + if (MLME_STATE(padapter) & (WIFI_AP_STATE | WIFI_MESH_STATE)) { + _list b2u_list; + + #ifdef CONFIG_RTW_MESH + if (MLME_IS_MESH(padapter)) + res = rtw_mesh_addr_resolve(padapter, os_qid, pxmitframe, *ppkt, &b2u_list); + else + #endif + res = rtw_ap_addr_resolve(padapter, os_qid, pxmitframe, *ppkt, &b2u_list); + if (res == RTW_RA_RESOLVING) + return 1; + if (res == _FAIL) + return -1; + + #if CONFIG_RTW_DATA_BMC_TO_UC + if (!rtw_is_list_empty(&b2u_list)) { + _list *list = get_next(&b2u_list); + struct xmit_frame *b2uframe; + + while ((rtw_end_of_queue_search(&b2u_list, list)) == _FALSE) { + b2uframe = LIST_CONTAINOR(list, struct xmit_frame, list); + list = get_next(list); + rtw_list_delete(&b2uframe->list); + + b2uframe->pkt = rtw_os_pkt_copy(*ppkt); + if (!b2uframe->pkt) { + if (res == RTW_BMC_NO_NEED) + res = _SUCCESS; + rtw_free_xmitframe(pxmitpriv, b2uframe); + continue; + } + + rtw_xmit_posthandle(padapter, b2uframe, b2uframe->pkt); + } + } + #endif + + if (res == RTW_BMC_NO_NEED) { + rtw_free_xmitframe(&padapter->xmitpriv, pxmitframe); + return 0; + } + } +#endif /* defined(CONFIG_AP_MODE) || defined(CONFIG_RTW_MESH) */ + + pxmitframe->pkt = NULL; /* let rtw_xmit_posthandle not to free pkt inside */ + res = rtw_xmit_posthandle(padapter, pxmitframe, *ppkt); + + return res; +} + +#ifdef CONFIG_TDLS +sint xmitframe_enqueue_for_tdls_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + sint ret = _FALSE; + + _irqL irqL; + struct sta_info *ptdls_sta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); + int i; + + ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst); + if (ptdls_sta == NULL) + return ret; + else if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) { + + if (pattrib->triggered == 1) { + ret = _TRUE; + return ret; + } + + _enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL); + + if (ptdls_sta->state & WIFI_SLEEP_STATE) { + rtw_list_delete(&pxmitframe->list); + + /* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */ + + rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptdls_sta->sleep_q)); + + ptdls_sta->sleepq_len++; + ptdls_sta->sleepq_ac_len++; + + /* indicate 4-AC queue bit in TDLS peer traffic indication */ + switch (pattrib->priority) { + case 1: + case 2: + ptdls_sta->uapsd_bk |= BIT(1); + break; + case 4: + case 5: + ptdls_sta->uapsd_vi |= BIT(1); + break; + case 6: + case 7: + ptdls_sta->uapsd_vo |= BIT(1); + break; + case 0: + case 3: + default: + ptdls_sta->uapsd_be |= BIT(1); + break; + } + + /* Transmit TDLS PTI via AP */ + if (ptdls_sta->sleepq_len == 1) + rtw_tdls_cmd(padapter, ptdls_sta->cmn.mac_addr, TDLS_ISSUE_PTI); + + ret = _TRUE; + } + + _exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL); + } + + return ret; + +} +#endif /* CONFIG_TDLS */ + +#define RTW_HIQ_FILTER_ALLOW_ALL 0 +#define RTW_HIQ_FILTER_ALLOW_SPECIAL 1 +#define RTW_HIQ_FILTER_DENY_ALL 2 + +inline bool xmitframe_hiq_filter(struct xmit_frame *xmitframe) +{ + bool allow = _FALSE; + _adapter *adapter = xmitframe->padapter; + struct registry_priv *registry = &adapter->registrypriv; + + if (adapter->registrypriv.wifi_spec == 1) + allow = _TRUE; + else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) { + + struct pkt_attrib *attrib = &xmitframe->attrib; + + if (attrib->ether_type == 0x0806 + || attrib->ether_type == 0x888e +#ifdef CONFIG_WAPI_SUPPORT + || attrib->ether_type == 0x88B4 +#endif + || attrib->dhcp_pkt + ) { + if (0) + RTW_INFO(FUNC_ADPT_FMT" ether_type:0x%04x%s\n", FUNC_ADPT_ARG(xmitframe->padapter) + , attrib->ether_type, attrib->dhcp_pkt ? " DHCP" : ""); + allow = _TRUE; + } + } else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_ALL) + allow = _TRUE; + else if (registry->hiq_filter == RTW_HIQ_FILTER_DENY_ALL) + allow = _FALSE; + else + rtw_warn_on(1); + + return allow; +} + +#if defined(CONFIG_AP_MODE) || defined(CONFIG_TDLS) +#ifdef CONFIG_RTW_MGMT_QUEUE +u8 mgmt_xmitframe_enqueue_for_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + _irqL irqL; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + struct sta_info *psta = pattrib->psta; + struct sta_priv *pstapriv = &padapter->stapriv; + bool update_tim = _FALSE; + u8 ret = _TRUE; + + if (is_broadcast_mac_addr(pattrib->ra) || pattrib->ps_dontq) + return _FALSE; + + if (psta == NULL) { + RTW_INFO("%s, psta==NUL, pattrib->ra:"MAC_FMT"\n", + __func__, MAC_ARG(pattrib->ra)); + return _FALSE; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link); + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FALSE; + } + + _enter_critical_bh(&psta->mgmt_sleep_q.lock, &irqL); + + if (psta->state & WIFI_SLEEP_STATE && + rtw_tim_map_is_set(padapter, pstapriv->sta_dz_bitmap, psta->cmn.aid)) { + + rtw_list_delete(&pxmitframe->list); + rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->mgmt_sleep_q)); + psta->mgmt_sleepq_len++; + + #ifdef DBG_MGMT_QUEUE + RTW_INFO("%s attrib->ra:"MAC_FMT" seq_num = %u, subtype = 0x%x\n", + __func__, MAC_ARG(pattrib->ra), pattrib->seqnum, pattrib->subtype); + #endif + + if (!(rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid))) + update_tim = _TRUE; + + rtw_tim_map_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + /* upate BCN for TIM IE */ + if (update_tim == _TRUE) + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "buffer mgmt frame"); + + ret = RTW_QUEUE_MGMT; + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast); + } + + _exit_critical_bh(&psta->mgmt_sleep_q.lock, &irqL); + + return ret; +} + +static void dequeue_mgmt_xmitframe_to_sleepq(_adapter *padapter, struct sta_info *psta, _queue *pframequeue) +{ + sint ret; + _list *plist, *phead; + struct tx_servq *ptxservq; + struct pkt_attrib *pattrib; + struct xmit_frame *pxmitframe; + struct xmit_priv *pxmitpriv = &(padapter->xmitpriv); + struct hw_xmit *phwxmits = pxmitpriv->hwxmits; + u8 mgmt_idx = pxmitpriv->hwxmit_entry - 1; + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + plist = get_next(plist); + + pattrib = &pxmitframe->attrib; + pattrib->triggered = 0; + + ret = mgmt_xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe); + + if (ret == RTW_QUEUE_MGMT) { + ptxservq = &(psta->sta_xmitpriv.mgmt_q); + ptxservq->qcnt--; + phwxmits[mgmt_idx].accnt--; + } else { + /* RTW_INFO("xmitframe_enqueue_for_sleeping_sta return _FALSE\n"); */ + } + } +} +#endif + +sint xmitframe_enqueue_for_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe) +{ + _irqL irqL; + sint ret = _FALSE; + struct sta_info *psta = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct pkt_attrib *pattrib = &pxmitframe->attrib; + sint bmcst = IS_MCAST(pattrib->ra); + bool update_tim = _FALSE; +#ifdef CONFIG_TDLS + + if (padapter->tdlsinfo.link_established == _TRUE) + ret = xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pxmitframe); +#endif /* CONFIG_TDLS */ + + if (!MLME_IS_AP(padapter) && !MLME_IS_MESH(padapter)) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_fwstate); + return ret; + } + /* + if(pattrib->psta) + { + psta = pattrib->psta; + } + else + { + RTW_INFO("%s, call rtw_get_stainfo()\n", __func__); + psta=rtw_get_stainfo(pstapriv, pattrib->ra); + } + */ + psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra); + if (pattrib->psta != psta) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_sta); + RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta); + return _FALSE; + } + + if (psta == NULL) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_nosta); + RTW_INFO("%s, psta==NUL\n", __func__); + return _FALSE; + } + + if (!(psta->state & WIFI_ASOC_STATE)) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link); + RTW_INFO("%s, psta->state(0x%x) != WIFI_ASOC_STATE\n", __func__, psta->state); + return _FALSE; + } + + if (pattrib->triggered == 1) { + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_trigger); + /* RTW_INFO("directly xmit pspoll_triggered packet\n"); */ + + /* pattrib->triggered=0; */ + if (bmcst && xmitframe_hiq_filter(pxmitframe) == _TRUE) + pattrib->qsel = QSLT_HIGH;/* HIQ */ + + return ret; + } + + + if (bmcst) { + _enter_critical_bh(&psta->sleep_q.lock, &irqL); + + if (rtw_tim_map_anyone_be_set(padapter, pstapriv->sta_dz_bitmap)) { /* if anyone sta is in ps mode */ + /* pattrib->qsel = QSLT_HIGH; */ /* HIQ */ + + rtw_list_delete(&pxmitframe->list); + + /*_enter_critical_bh(&psta->sleep_q.lock, &irqL);*/ + + rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->sleep_q)); + + psta->sleepq_len++; + + if (!(rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, 0))) + update_tim = _TRUE; + + rtw_tim_map_set(padapter, pstapriv->tim_bitmap, 0); + rtw_tim_map_set(padapter, pstapriv->sta_dz_bitmap, 0); + + /* RTW_INFO("enqueue, sq_len=%d\n", psta->sleepq_len); */ + /* RTW_INFO_DUMP("enqueue, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + if (update_tim == _TRUE) { + if (is_broadcast_mac_addr(pattrib->ra)) + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "buffer BC"); + else + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "buffer MC"); + } else + chk_bmc_sleepq_cmd(padapter); + + /*_exit_critical_bh(&psta->sleep_q.lock, &irqL);*/ + + ret = _TRUE; + + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast); + } + + _exit_critical_bh(&psta->sleep_q.lock, &irqL); + + return ret; + + } + + + _enter_critical_bh(&psta->sleep_q.lock, &irqL); + + if (psta->state & WIFI_SLEEP_STATE) { + u8 wmmps_ac = 0; + + if (rtw_tim_map_is_set(padapter, pstapriv->sta_dz_bitmap, psta->cmn.aid)) { + rtw_list_delete(&pxmitframe->list); + + /* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */ + + rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->sleep_q)); + + psta->sleepq_len++; + + switch (pattrib->priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk & BIT(0); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi & BIT(0); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo & BIT(0); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be & BIT(0); + break; + } + + if (wmmps_ac) + psta->sleepq_ac_len++; + + if (((psta->has_legacy_ac) && (!wmmps_ac)) || ((!psta->has_legacy_ac) && (wmmps_ac))) { + if (!(rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid))) + update_tim = _TRUE; + + rtw_tim_map_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + /* RTW_INFO("enqueue, sq_len=%d\n", psta->sleepq_len); */ + /* RTW_INFO_DUMP("enqueue, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + + if (update_tim == _TRUE) { + /* RTW_INFO("sleepq_len==1, update BCNTIM\n"); */ + /* upate BCN for TIM IE */ + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "buffer UC"); + } + } + + /* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */ + + /* if(psta->sleepq_len > (NR_XMITFRAME>>3)) */ + /* { */ + /* wakeup_sta_to_xmit(padapter, psta); */ + /* } */ + + ret = _TRUE; + + DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast); + } + + } + + _exit_critical_bh(&psta->sleep_q.lock, &irqL); + + return ret; + +} + +static void dequeue_xmitframes_to_sleeping_queue(_adapter *padapter, struct sta_info *psta, _queue *pframequeue) +{ + sint ret; + _list *plist, *phead; + u8 ac_index; + struct tx_servq *ptxservq; + struct pkt_attrib *pattrib; + struct xmit_frame *pxmitframe; + struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits; + + phead = get_list_head(pframequeue); + plist = get_next(phead); + + while (rtw_end_of_queue_search(phead, plist) == _FALSE) { + pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list); + + plist = get_next(plist); + + pattrib = &pxmitframe->attrib; + + pattrib->triggered = 0; + + ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe); + + if (_TRUE == ret) { + ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index)); + + ptxservq->qcnt--; + phwxmits[ac_index].accnt--; + } else { + /* RTW_INFO("xmitframe_enqueue_for_sleeping_sta return _FALSE\n"); */ + } + + } + +} + +void stop_sta_xmit(_adapter *padapter, struct sta_info *psta) +{ + _irqL irqL0; + struct sta_info *psta_bmc; + struct sta_xmit_priv *pstaxmitpriv; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + pstaxmitpriv = &psta->sta_xmitpriv; + + /* for BC/MC Frames */ + psta_bmc = rtw_get_bcmc_stainfo(padapter); + if (!psta_bmc) + rtw_warn_on(1); + + _enter_critical_bh(&pxmitpriv->lock, &irqL0); + + psta->state |= WIFI_SLEEP_STATE; + +#ifdef CONFIG_TDLS + if (!(psta->tdls_sta_state & TDLS_LINKED_STATE)) +#endif /* CONFIG_TDLS */ + rtw_tim_map_set(padapter, pstapriv->sta_dz_bitmap, psta->cmn.aid); + +#ifdef CONFIG_RTW_MGMT_QUEUE + dequeue_mgmt_xmitframe_to_sleepq(padapter, psta, &pstaxmitpriv->mgmt_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->mgmt_q.tx_pending)); +#endif + + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending)); + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending)); + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending)); + dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending)); + + if (psta_bmc != NULL + #ifdef CONFIG_TDLS + && !(psta->tdls_sta_state & TDLS_LINKED_STATE) + #endif + ) + { + /* for BC/MC Frames */ + pstaxmitpriv = &psta_bmc->sta_xmitpriv; + dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->vo_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending)); + dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->vi_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending)); + dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending)); + dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->bk_q.sta_pending); + rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending)); + } + _exit_critical_bh(&pxmitpriv->lock, &irqL0); + + +} + +void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta) +{ + _irqL irqL; + u8 update_mask = 0, wmmps_ac = 0; + struct sta_info *psta_bmc; + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + psta_bmc = rtw_get_bcmc_stainfo(padapter); + + + /* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */ + _enter_critical_bh(&pxmitpriv->lock, &irqL); + +#ifdef CONFIG_RTW_MGMT_QUEUE + /* management queue */ + xmitframe_phead = get_list_head(&psta->mgmt_sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + rtw_list_delete(&pxmitframe->list); + + #ifdef DBG_MGMT_QUEUE + RTW_INFO("%s seq_num = %u, subtype = 0x%x\n", + __func__, pxmitframe->attrib.seqnum, pxmitframe->attrib.subtype); + #endif + + psta->mgmt_sleepq_len--; + + pxmitframe->attrib.triggered = 1; + + rtw_hal_mgmt_xmitframe_enqueue(padapter, pxmitframe); + } +#endif /* CONFIG_RTW_MGMT_QUEUE */ + + /* AC queue */ + xmitframe_phead = get_list_head(&psta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + rtw_list_delete(&pxmitframe->list); + + switch (pxmitframe->attrib.priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk & BIT(1); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi & BIT(1); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo & BIT(1); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be & BIT(1); + break; + } + + psta->sleepq_len--; + if (psta->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + if (wmmps_ac) { + psta->sleepq_ac_len--; + if (psta->sleepq_ac_len > 0) { + pxmitframe->attrib.mdata = 1; + pxmitframe->attrib.eosp = 0; + } else { + pxmitframe->attrib.mdata = 0; + pxmitframe->attrib.eosp = 1; + } + } + + pxmitframe->attrib.triggered = 1; + + /* + _exit_critical_bh(&psta->sleep_q.lock, &irqL); + if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE) + { + rtw_os_xmit_complete(padapter, pxmitframe); + } + _enter_critical_bh(&psta->sleep_q.lock, &irqL); + */ + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + + } + + if (psta->sleepq_len == 0 +#ifdef CONFIG_RTW_MGMT_QUEUE + && psta->mgmt_sleepq_len == 0 +#endif + ) { +#ifdef CONFIG_TDLS + if (psta->tdls_sta_state & TDLS_LINKED_STATE) { + if (psta->state & WIFI_SLEEP_STATE) + psta->state ^= WIFI_SLEEP_STATE; + + _exit_critical_bh(&pxmitpriv->lock, &irqL); + return; + } +#endif /* CONFIG_TDLS */ + + if (rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, psta->cmn.aid)) { + /* RTW_INFO("wakeup to xmit, qlen==0\n"); */ + /* RTW_INFO_DUMP("update_BCNTIM, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_mask = BIT(0); + } + + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + if (psta->state & WIFI_SLEEP_STATE) + psta->state ^= WIFI_SLEEP_STATE; + + if (psta->state & WIFI_STA_ALIVE_CHK_STATE) { + RTW_INFO("%s alive check\n", __func__); + psta->expire_to = pstapriv->expire_to; + psta->state ^= WIFI_STA_ALIVE_CHK_STATE; + } + + rtw_tim_map_clear(padapter, pstapriv->sta_dz_bitmap, psta->cmn.aid); + } + + /* for BC/MC Frames */ + if (!psta_bmc) + goto _exit; + + if (!(rtw_tim_map_anyone_be_set_exclude_aid0(padapter, pstapriv->sta_dz_bitmap))) { /* no any sta in ps mode */ + xmitframe_phead = get_list_head(&psta_bmc->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + rtw_list_delete(&pxmitframe->list); + + psta_bmc->sleepq_len--; + if (psta_bmc->sleepq_len > 0) + pxmitframe->attrib.mdata = 1; + else + pxmitframe->attrib.mdata = 0; + + + pxmitframe->attrib.triggered = 1; + /* + _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL); + if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE) + { + rtw_os_xmit_complete(padapter, pxmitframe); + } + _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL); + + */ + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + } + + if (psta_bmc->sleepq_len == 0) { + if (rtw_tim_map_is_set(padapter, pstapriv->tim_bitmap, 0)) { + /* RTW_INFO("wakeup to xmit, qlen==0\n"); */ + /* RTW_INFO_DUMP("update_BCNTIM, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_mask |= BIT(1); + } + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, 0); + rtw_tim_map_clear(padapter, pstapriv->sta_dz_bitmap, 0); + } + + } + +_exit: + + /* _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL); */ + _exit_critical_bh(&pxmitpriv->lock, &irqL); + + if (update_mask) { + /* update_BCNTIM(padapter); */ + if ((update_mask & (BIT(0) | BIT(1))) == (BIT(0) | BIT(1))) + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "clear UC&BMC"); + else if ((update_mask & BIT(1)) == BIT(1)) + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "clear BMC"); + else + _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0, "clear UC"); + } +} + +void xmit_delivery_enabled_frames(_adapter *padapter, struct sta_info *psta) +{ + _irqL irqL; + u8 wmmps_ac = 0; + _list *xmitframe_plist, *xmitframe_phead; + struct xmit_frame *pxmitframe = NULL; + struct sta_priv *pstapriv = &padapter->stapriv; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + + /* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */ + _enter_critical_bh(&pxmitpriv->lock, &irqL); + + xmitframe_phead = get_list_head(&psta->sleep_q); + xmitframe_plist = get_next(xmitframe_phead); + + while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE) { + pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list); + + xmitframe_plist = get_next(xmitframe_plist); + + switch (pxmitframe->attrib.priority) { + case 1: + case 2: + wmmps_ac = psta->uapsd_bk & BIT(1); + break; + case 4: + case 5: + wmmps_ac = psta->uapsd_vi & BIT(1); + break; + case 6: + case 7: + wmmps_ac = psta->uapsd_vo & BIT(1); + break; + case 0: + case 3: + default: + wmmps_ac = psta->uapsd_be & BIT(1); + break; + } + + if (!wmmps_ac) + continue; + + rtw_list_delete(&pxmitframe->list); + + psta->sleepq_len--; + psta->sleepq_ac_len--; + + if (psta->sleepq_ac_len > 0) { + pxmitframe->attrib.mdata = 1; + pxmitframe->attrib.eosp = 0; + } else { + pxmitframe->attrib.mdata = 0; + pxmitframe->attrib.eosp = 1; + } + + pxmitframe->attrib.triggered = 1; + rtw_hal_xmitframe_enqueue(padapter, pxmitframe); + + if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) { +#ifdef CONFIG_TDLS + if (psta->tdls_sta_state & TDLS_LINKED_STATE) { + /* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */ + goto exit; + } +#endif /* CONFIG_TDLS */ + rtw_tim_map_clear(padapter, pstapriv->tim_bitmap, psta->cmn.aid); + + /* RTW_INFO("wakeup to xmit, qlen==0\n"); */ + /* RTW_INFO_DUMP("update_BCNTIM, tim=", pstapriv->tim_bitmap, pstapriv->aid_bmp_len); */ + /* upate BCN for TIM IE */ + /* update_BCNTIM(padapter); */ + update_beacon(padapter, _TIM_IE_, NULL, _TRUE, 0); + /* update_mask = BIT(0); */ + } + + } + +#ifdef CONFIG_TDLS +exit: +#endif + /* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */ + _exit_critical_bh(&pxmitpriv->lock, &irqL); + + return; +} + +#endif /* defined(CONFIG_AP_MODE) || defined(CONFIG_TDLS) */ + +#ifdef CONFIG_XMIT_THREAD_MODE +void enqueue_pending_xmitbuf( + struct xmit_priv *pxmitpriv, + struct xmit_buf *pxmitbuf) +{ + _irqL irql; + _queue *pqueue; + _adapter *pri_adapter = pxmitpriv->adapter; + + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + _enter_critical_bh(&pqueue->lock, &irql); + rtw_list_delete(&pxmitbuf->list); + rtw_list_insert_tail(&pxmitbuf->list, get_list_head(pqueue)); + _exit_critical_bh(&pqueue->lock, &irql); + +#if defined(CONFIG_SDIO_HCI) && defined(CONFIG_CONCURRENT_MODE) + pri_adapter = GET_PRIMARY_ADAPTER(pri_adapter); +#endif /*SDIO_HCI + CONCURRENT*/ + _rtw_up_sema(&(pri_adapter->xmitpriv.xmit_sema)); +} + +void enqueue_pending_xmitbuf_to_head( + struct xmit_priv *pxmitpriv, + struct xmit_buf *pxmitbuf) +{ + _irqL irql; + _queue *pqueue = &pxmitpriv->pending_xmitbuf_queue; + + _enter_critical_bh(&pqueue->lock, &irql); + rtw_list_delete(&pxmitbuf->list); + rtw_list_insert_head(&pxmitbuf->list, get_list_head(pqueue)); + _exit_critical_bh(&pqueue->lock, &irql); +} + +struct xmit_buf *dequeue_pending_xmitbuf( + struct xmit_priv *pxmitpriv) +{ + _irqL irql; + struct xmit_buf *pxmitbuf; + _queue *pqueue; + + + pxmitbuf = NULL; + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + _enter_critical_bh(&pqueue->lock, &irql); + + if (_rtw_queue_empty(pqueue) == _FALSE) { + _list *plist, *phead; + + phead = get_list_head(pqueue); + plist = get_next(phead); + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + rtw_list_delete(&pxmitbuf->list); + } + + _exit_critical_bh(&pqueue->lock, &irql); + + return pxmitbuf; +} + +static struct xmit_buf *dequeue_pending_xmitbuf_ext( + struct xmit_priv *pxmitpriv) +{ + _irqL irql; + struct xmit_buf *pxmitbuf; + _queue *pqueue; + + pxmitbuf = NULL; + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + _enter_critical_bh(&pqueue->lock, &irql); + + if (_rtw_queue_empty(pqueue) == _FALSE) { + _list *plist, *phead; + + phead = get_list_head(pqueue); + plist = phead; + do { + plist = get_next(plist); + if (plist == phead) + break; + + pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list); + + if (pxmitbuf->buf_tag == XMITBUF_MGNT) { + rtw_list_delete(&pxmitbuf->list); + break; + } + pxmitbuf = NULL; + } while (1); + } + + _exit_critical_bh(&pqueue->lock, &irql); + + return pxmitbuf; +} + +struct xmit_buf *select_and_dequeue_pending_xmitbuf(_adapter *padapter) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct xmit_buf *pxmitbuf = NULL; + + if (_TRUE == rtw_is_xmit_blocked(padapter)) + return pxmitbuf; + + pxmitbuf = dequeue_pending_xmitbuf_ext(pxmitpriv); + if (pxmitbuf == NULL && rtw_xmit_ac_blocked(padapter) != _TRUE) + pxmitbuf = dequeue_pending_xmitbuf(pxmitpriv); + + return pxmitbuf; +} + +sint check_pending_xmitbuf( + struct xmit_priv *pxmitpriv) +{ + _irqL irql; + _queue *pqueue; + sint ret = _FALSE; + + pqueue = &pxmitpriv->pending_xmitbuf_queue; + + _enter_critical_bh(&pqueue->lock, &irql); + + if (_rtw_queue_empty(pqueue) == _FALSE) + ret = _TRUE; + + _exit_critical_bh(&pqueue->lock, &irql); + + return ret; +} + +thread_return rtw_xmit_thread(thread_context context) +{ + s32 err; + PADAPTER padapter; +#ifdef RTW_XMIT_THREAD_HIGH_PRIORITY +#ifdef PLATFORM_LINUX + struct sched_param param = { .sched_priority = 1 }; + + sched_setscheduler(current, SCHED_FIFO, ¶m); +#endif /* PLATFORM_LINUX */ +#endif /* RTW_XMIT_THREAD_HIGH_PRIORITY */ + + err = _SUCCESS; + padapter = (PADAPTER)context; + + thread_enter("RTW_XMIT_THREAD"); + + do { + err = rtw_hal_xmit_thread_handler(padapter); + flush_signals_thread(); + } while (_SUCCESS == err); + + RTW_INFO(FUNC_ADPT_FMT " Exit\n", FUNC_ADPT_ARG(padapter)); + + rtw_thread_wait_stop(); + + return 0; +} +#endif + +#ifdef DBG_XMIT_BLOCK +void dump_xmit_block(void *sel, _adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + RTW_PRINT_SEL(sel, "[XMIT-BLOCK] xmit_block :0x%02x\n", dvobj->xmit_block); + if (dvobj->xmit_block & XMIT_BLOCK_REDLMEM) + RTW_PRINT_SEL(sel, "Reason:%s\n", "XMIT_BLOCK_REDLMEM"); + if (dvobj->xmit_block & XMIT_BLOCK_SUSPEND) + RTW_PRINT_SEL(sel, "Reason:%s\n", "XMIT_BLOCK_SUSPEND"); + if (dvobj->xmit_block == XMIT_BLOCK_NONE) + RTW_PRINT_SEL(sel, "Reason:%s\n", "XMIT_BLOCK_NONE"); +} +void dump_xmit_block_info(void *sel, const char *fun_name, _adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + RTW_INFO("\n"ADPT_FMT" call %s\n", ADPT_ARG(padapter), fun_name); + dump_xmit_block(sel, padapter); +} +#define DBG_XMIT_BLOCK_DUMP(adapter) dump_xmit_block_info(RTW_DBGDUMP, __func__, adapter) +#endif + +void rtw_set_xmit_block(_adapter *padapter, enum XMIT_BLOCK_REASON reason) +{ + _irqL irqL; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + _enter_critical_bh(&dvobj->xmit_block_lock, &irqL); + dvobj->xmit_block |= reason; + _exit_critical_bh(&dvobj->xmit_block_lock, &irqL); + + #ifdef DBG_XMIT_BLOCK + DBG_XMIT_BLOCK_DUMP(padapter); + #endif +} + +void rtw_clr_xmit_block(_adapter *padapter, enum XMIT_BLOCK_REASON reason) +{ + _irqL irqL; + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + _enter_critical_bh(&dvobj->xmit_block_lock, &irqL); + dvobj->xmit_block &= ~reason; + _exit_critical_bh(&dvobj->xmit_block_lock, &irqL); + + #ifdef DBG_XMIT_BLOCK + DBG_XMIT_BLOCK_DUMP(padapter); + #endif +} +bool rtw_is_xmit_blocked(_adapter *padapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(padapter); + + #ifdef DBG_XMIT_BLOCK + DBG_XMIT_BLOCK_DUMP(padapter); + #endif + return ((dvobj->xmit_block) ? _TRUE : _FALSE); +} + +bool rtw_xmit_ac_blocked(_adapter *adapter) +{ + struct dvobj_priv *dvobj = adapter_to_dvobj(adapter); + struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter); + _adapter *iface; + struct mlme_ext_priv *mlmeext; + bool blocked = _FALSE; + int i; +#ifdef DBG_CONFIG_ERROR_DETECT +#ifdef DBG_CONFIG_ERROR_RESET +#ifdef CONFIG_USB_HCI + if (rtw_hal_sreset_inprogress(adapter) == _TRUE) { + blocked = _TRUE; + goto exit; + } +#endif/* #ifdef CONFIG_USB_HCI */ +#endif/* #ifdef DBG_CONFIG_ERROR_RESET */ +#endif/* #ifdef DBG_CONFIG_ERROR_DETECT */ + + if (rfctl->offch_state != OFFCHS_NONE + #if CONFIG_DFS + || IS_RADAR_DETECTED(rfctl) || rfctl->csa_ch + #endif + ) { + blocked = _TRUE; + goto exit; + } + + for (i = 0; i < dvobj->iface_nums; i++) { + iface = dvobj->padapters[i]; + mlmeext = &iface->mlmeextpriv; + + /* check scan state */ + if (mlmeext_scan_state(mlmeext) != SCAN_DISABLE + && mlmeext_scan_state(mlmeext) != SCAN_BACK_OP + ) { + blocked = _TRUE; + goto exit; + } + + if (mlmeext_scan_state(mlmeext) == SCAN_BACK_OP + && !mlmeext_chk_scan_backop_flags(mlmeext, SS_BACKOP_TX_RESUME) + ) { + blocked = _TRUE; + goto exit; + } + } + +#ifdef CONFIG_MCC_MODE + if (MCC_EN(adapter)) { + if (rtw_hal_check_mcc_status(adapter, MCC_STATUS_DOING_MCC)) { + if (MCC_STOP(adapter)) { + blocked = _TRUE; + goto exit; + } + } + } +#endif /* CONFIG_MCC_MODE */ + +exit: + return blocked; +} + +#ifdef CONFIG_TX_AMSDU +void rtw_amsdu_vo_timeout_handler(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + + adapter->xmitpriv.amsdu_vo_timeout = RTW_AMSDU_TIMER_TIMEOUT; + + tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet); +} + +void rtw_amsdu_vi_timeout_handler(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + + adapter->xmitpriv.amsdu_vi_timeout = RTW_AMSDU_TIMER_TIMEOUT; + + tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet); +} + +void rtw_amsdu_be_timeout_handler(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + + adapter->xmitpriv.amsdu_be_timeout = RTW_AMSDU_TIMER_TIMEOUT; + + if (printk_ratelimit()) + RTW_INFO("%s Timeout!\n",__FUNCTION__); + + tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet); +} + +void rtw_amsdu_bk_timeout_handler(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + + adapter->xmitpriv.amsdu_bk_timeout = RTW_AMSDU_TIMER_TIMEOUT; + + tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet); +} + +u8 rtw_amsdu_get_timer_status(_adapter *padapter, u8 priority) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + u8 status = RTW_AMSDU_TIMER_UNSET; + + switch(priority) + { + case 1: + case 2: + status = pxmitpriv->amsdu_bk_timeout; + break; + case 4: + case 5: + status = pxmitpriv->amsdu_vi_timeout; + break; + case 6: + case 7: + status = pxmitpriv->amsdu_vo_timeout; + break; + case 0: + case 3: + default: + status = pxmitpriv->amsdu_be_timeout; + break; + } + return status; +} + +void rtw_amsdu_set_timer_status(_adapter *padapter, u8 priority, u8 status) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + switch(priority) + { + case 1: + case 2: + pxmitpriv->amsdu_bk_timeout = status; + break; + case 4: + case 5: + pxmitpriv->amsdu_vi_timeout = status; + break; + case 6: + case 7: + pxmitpriv->amsdu_vo_timeout = status; + break; + case 0: + case 3: + default: + pxmitpriv->amsdu_be_timeout = status; + break; + } +} + +void rtw_amsdu_set_timer(_adapter *padapter, u8 priority) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + + _timer* amsdu_timer = NULL; + + switch(priority) + { + case 1: + case 2: + amsdu_timer = &pxmitpriv->amsdu_bk_timer; + break; + case 4: + case 5: + amsdu_timer = &pxmitpriv->amsdu_vi_timer; + break; + case 6: + case 7: + amsdu_timer = &pxmitpriv->amsdu_vo_timer; + break; + case 0: + case 3: + default: + amsdu_timer = &pxmitpriv->amsdu_be_timer; + break; + } + _set_timer(amsdu_timer, 1); +} + +void rtw_amsdu_cancel_timer(_adapter *padapter, u8 priority) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _timer* amsdu_timer = NULL; + + switch(priority) + { + case 1: + case 2: + amsdu_timer = &pxmitpriv->amsdu_bk_timer; + break; + case 4: + case 5: + amsdu_timer = &pxmitpriv->amsdu_vi_timer; + break; + case 6: + case 7: + amsdu_timer = &pxmitpriv->amsdu_vo_timer; + break; + case 0: + case 3: + default: + amsdu_timer = &pxmitpriv->amsdu_be_timer; + break; + } + _cancel_timer_ex(amsdu_timer); +} +#endif /* CONFIG_TX_AMSDU */ + +#ifdef DBG_TXBD_DESC_DUMP +static struct rtw_tx_desc_backup tx_backup[HW_QUEUE_ENTRY][TX_BAK_FRMAE_CNT]; +static u8 backup_idx[HW_QUEUE_ENTRY]; + +void rtw_tx_desc_backup(_adapter *padapter, struct xmit_frame *pxmitframe, u8 desc_size, u8 hwq) +{ + u32 tmp32; + u8 *pxmit_buf; + + if (rtw_get_hw_init_completed(padapter) == _FALSE) + return; + + pxmit_buf = pxmitframe->pxmitbuf->pbuf; + + _rtw_memcpy(tx_backup[hwq][backup_idx[hwq]].tx_bak_desc, pxmit_buf, desc_size); + _rtw_memcpy(tx_backup[hwq][backup_idx[hwq]].tx_bak_data_hdr, pxmit_buf+desc_size, TX_BAK_DATA_LEN); + + tmp32 = rtw_read32(padapter, get_txbd_rw_reg(hwq)); + + tx_backup[hwq][backup_idx[hwq]].tx_bak_rp = (tmp32>>16)&0xfff; + tx_backup[hwq][backup_idx[hwq]].tx_bak_wp = tmp32&0xfff; + + tx_backup[hwq][backup_idx[hwq]].tx_desc_size = desc_size; + + backup_idx[hwq] = (backup_idx[hwq] + 1) % TX_BAK_FRMAE_CNT; +} + +void rtw_tx_desc_backup_reset(void) +{ + int i, j; + + for (i = 0; i < HW_QUEUE_ENTRY; i++) { + for (j = 0; j < TX_BAK_FRMAE_CNT; j++) + _rtw_memset(&tx_backup[i][j], 0, sizeof(struct rtw_tx_desc_backup)); + + backup_idx[i] = 0; + } +} + +u8 rtw_get_tx_desc_backup(_adapter *padapter, u8 hwq, struct rtw_tx_desc_backup **pbak) +{ + *pbak = &tx_backup[hwq][0]; + + return backup_idx[hwq]; +} +#endif + +#ifdef CONFIG_PCI_TX_POLLING +void rtw_tx_poll_init(_adapter *padapter) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _timer* timer = &pxmitpriv->tx_poll_timer; + + if (!is_primary_adapter(padapter)) + return; + + if (timer->function != NULL) { + RTW_INFO("tx polling timer has been init.\n"); + return; + } + + rtw_init_timer(timer, padapter, rtw_tx_poll_timeout_handler, padapter); + rtw_tx_poll_timer_set(padapter, 1); + RTW_INFO("Tx poll timer init!\n"); +} + +void rtw_tx_poll_timeout_handler(void *FunctionContext) +{ + _adapter *adapter = (_adapter *)FunctionContext; + + rtw_tx_poll_timer_set(adapter, 1); + + if (adapter->hal_func.tx_poll_handler) + adapter->hal_func.tx_poll_handler(adapter); + else + RTW_WARN("hal ops: tx_poll_handler is NULL\n"); +} + +void rtw_tx_poll_timer_set(_adapter *padapter, u32 delay) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _timer* timer = NULL; + + timer = &pxmitpriv->tx_poll_timer; + _set_timer(timer, delay); +} + +void rtw_tx_poll_timer_cancel(_adapter *padapter) +{ + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + _timer* timer = NULL; + + if (!is_primary_adapter(padapter)) + return; + + timer = &pxmitpriv->tx_poll_timer; + _cancel_timer_ex(timer); + timer->function = NULL; + RTW_INFO("Tx poll timer cancel !\n"); +} +#endif /* CONFIG_PCI_TX_POLLING */ + +void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms) +{ + sctx->timeout_ms = timeout_ms; + sctx->submit_time = rtw_get_current_time(); +#ifdef PLATFORM_LINUX /* TODO: add condition wating interface for other os */ + init_completion(&sctx->done); +#endif + sctx->status = RTW_SCTX_SUBMITTED; +} + +int rtw_sctx_wait(struct submit_ctx *sctx, const char *msg) +{ + int ret = _FAIL; + unsigned long expire; + int status = 0; + +#ifdef PLATFORM_LINUX + expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT; + if (!wait_for_completion_timeout(&sctx->done, expire)) { + /* timeout, do something?? */ + status = RTW_SCTX_DONE_TIMEOUT; + RTW_INFO("%s timeout: %s\n", __func__, msg); + } else + status = sctx->status; +#endif + + if (status == RTW_SCTX_DONE_SUCCESS) + ret = _SUCCESS; + + return ret; +} + +bool rtw_sctx_chk_waring_status(int status) +{ + switch (status) { + case RTW_SCTX_DONE_UNKNOWN: + case RTW_SCTX_DONE_BUF_ALLOC: + case RTW_SCTX_DONE_BUF_FREE: + + case RTW_SCTX_DONE_DRV_STOP: + case RTW_SCTX_DONE_DEV_REMOVE: + return _TRUE; + default: + return _FALSE; + } +} + +void rtw_sctx_done_err(struct submit_ctx **sctx, int status) +{ + if (*sctx) { + if (rtw_sctx_chk_waring_status(status)) + RTW_INFO("%s status:%d\n", __func__, status); + (*sctx)->status = status; +#ifdef PLATFORM_LINUX + complete(&((*sctx)->done)); +#endif + *sctx = NULL; + } +} + +void rtw_sctx_done(struct submit_ctx **sctx) +{ + rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS); +} + +#ifdef CONFIG_XMIT_ACK +int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms) +{ + struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops; + + pack_tx_ops->submit_time = rtw_get_current_time(); + pack_tx_ops->timeout_ms = timeout_ms; + pack_tx_ops->status = RTW_SCTX_SUBMITTED; + + return rtw_sctx_wait(pack_tx_ops, __func__); +} + +void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status) +{ + struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops; + + if (pxmitpriv->ack_tx) + rtw_sctx_done_err(&pack_tx_ops, status); + else + RTW_INFO("%s ack_tx not set\n", __func__); +} +#endif /* CONFIG_XMIT_ACK */ diff --git a/core/wds/rtw_wds.c b/core/wds/rtw_wds.c new file mode 100644 index 0000000..fd88b1c --- /dev/null +++ b/core/wds/rtw_wds.c @@ -0,0 +1,786 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2017 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#define _RTW_WDS_C_ + +#include + +#if defined(CONFIG_RTW_WDS) +#include + +#if defined(CONFIG_AP_MODE) + +#ifdef PLATFORM_LINUX +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 0, 0)) +static void rtw_wpath_free_rcu(struct rtw_wds_path *wpath) +{ + kfree_rcu(wpath, rcu); + rtw_mstat_update(MSTAT_TYPE_PHY, MSTAT_FREE, sizeof(struct rtw_wds_path)); +} +#else +static void rtw_wpath_free_rcu_callback(rtw_rcu_head *head) +{ + struct rtw_wds_path *wpath; + + wpath = container_of(head, struct rtw_wds_path, rcu); + rtw_mfree(wpath, sizeof(struct rtw_wds_path)); +} + +static void rtw_wpath_free_rcu(struct rtw_wds_path *wpath) +{ + call_rcu(&wpath->rcu, rtw_wpath_free_rcu_callback); +} +#endif +#endif /* PLATFORM_LINUX */ + +static void rtw_wds_path_free_rcu(struct rtw_wds_table *tbl, struct rtw_wds_path *wpath); + +static u32 rtw_wds_table_hash(const void *addr, u32 len, u32 seed) +{ + /* Use last four bytes of hw addr as hash index */ + return jhash_1word(*(u32 *)(addr+2), seed); +} + +static const rtw_rhashtable_params rtw_wds_rht_params = { + .nelem_hint = 2, + .automatic_shrinking = true, + .key_len = ETH_ALEN, + .key_offset = offsetof(struct rtw_wds_path, dst), + .head_offset = offsetof(struct rtw_wds_path, rhash), + .hashfn = rtw_wds_table_hash, +}; + +static void rtw_wds_path_rht_free(void *ptr, void *tblptr) +{ + struct rtw_wds_path *wpath = ptr; + struct rtw_wds_table *tbl = tblptr; + + rtw_wds_path_free_rcu(tbl, wpath); +} + +static struct rtw_wds_table *rtw_wds_table_alloc(void) +{ + struct rtw_wds_table *newtbl; + + newtbl = rtw_malloc(sizeof(struct rtw_wds_table)); + if (!newtbl) + return NULL; + + return newtbl; +} + +static void rtw_wds_table_free(struct rtw_wds_table *tbl) +{ + rtw_rhashtable_free_and_destroy(&tbl->rhead, + rtw_wds_path_rht_free, tbl); + rtw_mfree(tbl, sizeof(struct rtw_wds_table)); +} + +void rtw_wds_path_assign_nexthop(struct rtw_wds_path *wpath, struct sta_info *sta) +{ + rtw_rcu_assign_pointer(wpath->next_hop, sta); +} + +static struct rtw_wds_path *rtw_wpath_lookup(struct rtw_wds_table *tbl, const u8 *dst) +{ + struct rtw_wds_path *wpath; + + if (!tbl) + return NULL; + + wpath = rtw_rhashtable_lookup_fast(&tbl->rhead, dst, rtw_wds_rht_params); + + return wpath; +} + +struct rtw_wds_path *rtw_wds_path_lookup(_adapter *adapter, const u8 *dst) +{ + return rtw_wpath_lookup(adapter->wds_paths, dst); +} + +static struct rtw_wds_path * +__rtw_wds_path_lookup_by_idx(struct rtw_wds_table *tbl, int idx) +{ + int i = 0, ret; + struct rtw_wds_path *wpath = NULL; + rtw_rhashtable_iter iter; + + if (!tbl) + return NULL; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return NULL; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto err; + + while ((wpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(wpath) && PTR_ERR(wpath) == -EAGAIN) + continue; + if (IS_ERR(wpath)) + break; + if (i++ == idx) + break; + } +err: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); + + if (IS_ERR(wpath) || !wpath) + return NULL; + + return wpath; +} + +/** + * Locking: must be called within a read rcu section. + */ +struct rtw_wds_path * +rtw_wds_path_lookup_by_idx(_adapter *adapter, int idx) +{ + return __rtw_wds_path_lookup_by_idx(adapter->wds_paths, idx); +} + +void dump_wpath(void *sel, _adapter *adapter) +{ + struct rtw_wds_path *wpath; + int idx = 0; + char dst[ETH_ALEN]; + char next_hop[ETH_ALEN]; + u32 age_ms; + + RTW_PRINT_SEL(sel, "num:%d\n", ATOMIC_READ(&adapter->wds_path_num)); + RTW_PRINT_SEL(sel, "%-17s %-17s %-6s\n" + , "dst", "next_hop", "age" + ); + + do { + rtw_rcu_read_lock(); + + wpath = rtw_wds_path_lookup_by_idx(adapter, idx); + if (wpath) { + _rtw_memcpy(dst, wpath->dst, ETH_ALEN); + _rtw_memcpy(next_hop, wpath->next_hop->cmn.mac_addr, ETH_ALEN); + age_ms = rtw_get_passing_time_ms(wpath->last_update); + } + + rtw_rcu_read_unlock(); + + if (wpath) { + RTW_PRINT_SEL(sel, MAC_FMT" "MAC_FMT" %6u\n" + , MAC_ARG(dst), MAC_ARG(next_hop) + , age_ms < 999999 ? age_ms : 999999 + ); + } + + idx++; + } while (wpath); +} + +static +struct rtw_wds_path *rtw_wds_path_new(_adapter *adapter, + const u8 *dst) +{ + struct rtw_wds_path *new_wpath; + + new_wpath = rtw_zmalloc(sizeof(struct rtw_wds_path)); + if (!new_wpath) + return NULL; + + new_wpath->adapter = adapter; + _rtw_memcpy(new_wpath->dst, dst, ETH_ALEN); + new_wpath->last_update = rtw_get_current_time(); + + return new_wpath; +} + +/** + * Returns: 0 on success + * + * State: the initial state of the new path is set to 0 + */ +struct rtw_wds_path *rtw_wds_path_add(_adapter *adapter, + const u8 *dst, struct sta_info *next_hop) +{ + struct rtw_wds_table *tbl = adapter->wds_paths; + struct rtw_wds_path *wpath, *new_wpath; + int ret; + + if (!tbl) + return ERR_PTR(-ENOTSUPP); + + if (_rtw_memcmp(dst, adapter_mac_addr(adapter), ETH_ALEN) == _TRUE) + /* never add ourselves as neighbours */ + return ERR_PTR(-ENOTSUPP); + + if (IS_MCAST(dst)) + return ERR_PTR(-ENOTSUPP); + + if (ATOMIC_INC_UNLESS(&adapter->wds_path_num, RTW_WDS_MAX_PATHS) == 0) + return ERR_PTR(-ENOSPC); + + new_wpath = rtw_wds_path_new(adapter, dst); + if (!new_wpath) + return ERR_PTR(-ENOMEM); + + do { + ret = rtw_rhashtable_lookup_insert_fast(&tbl->rhead, + &new_wpath->rhash, + rtw_wds_rht_params); + + if (ret == -EEXIST) + wpath = rtw_rhashtable_lookup_fast(&tbl->rhead, + dst, + rtw_wds_rht_params); + + } while (unlikely(ret == -EEXIST && !wpath)); + + if (ret && ret != -EEXIST) + return ERR_PTR(ret); + + /* At this point either new_wpath was added, or we found a + * matching entry already in the table; in the latter case + * free the unnecessary new entry. + */ + if (ret == -EEXIST) { + rtw_mfree(new_wpath, sizeof(struct rtw_wds_path)); + new_wpath = wpath; + } + rtw_wds_path_assign_nexthop(new_wpath, next_hop); + + return new_wpath; +} + +static void rtw_wds_path_free_rcu(struct rtw_wds_table *tbl, + struct rtw_wds_path *wpath) +{ + _adapter *adapter = wpath->adapter; + + ATOMIC_DEC(&adapter->wds_path_num); + + rtw_wpath_free_rcu(wpath); +} + +static void __rtw_wds_path_del(struct rtw_wds_table *tbl, struct rtw_wds_path *wpath) +{ + rtw_rhashtable_remove_fast(&tbl->rhead, &wpath->rhash, rtw_wds_rht_params); + rtw_wds_path_free_rcu(tbl, wpath); +} + +void rtw_wds_path_flush_by_nexthop(struct sta_info *sta) +{ + _adapter *adapter = sta->padapter; + struct rtw_wds_table *tbl = adapter->wds_paths; + struct rtw_wds_path *wpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((wpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(wpath) && PTR_ERR(wpath) == -EAGAIN) + continue; + if (IS_ERR(wpath)) + break; + + if (rtw_rcu_access_pointer(wpath->next_hop) == sta) + __rtw_wds_path_del(tbl, wpath); + } +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +static void rtw_wds_table_flush_by_iface(struct rtw_wds_table *tbl) +{ + struct rtw_wds_path *wpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((wpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(wpath) && PTR_ERR(wpath) == -EAGAIN) + continue; + if (IS_ERR(wpath)) + break; + __rtw_wds_path_del(tbl, wpath); + } +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +void rtw_wds_path_flush_by_iface(_adapter *adapter) +{ + rtw_wds_table_flush_by_iface(adapter->wds_paths); +} + +static int rtw_wds_table_path_del(struct rtw_wds_table *tbl, + const u8 *addr) +{ + struct rtw_wds_path *wpath; + + if (!tbl) + return -ENXIO; + + rtw_rcu_read_lock(); + wpath = rtw_rhashtable_lookup_fast(&tbl->rhead, addr, rtw_wds_rht_params); + if (!wpath) { + rtw_rcu_read_unlock(); + return -ENXIO; + } + + __rtw_wds_path_del(tbl, wpath); + rtw_rcu_read_unlock(); + return 0; +} + +int rtw_wds_path_del(_adapter *adapter, const u8 *addr) +{ + int err; + + err = rtw_wds_table_path_del(adapter->wds_paths, addr); + return err; +} + +int rtw_wds_pathtbl_init(_adapter *adapter) +{ + struct rtw_wds_table *tbl_path; + int ret; + + tbl_path = rtw_wds_table_alloc(); + if (!tbl_path) + return -ENOMEM; + + rtw_rhashtable_init(&tbl_path->rhead, &rtw_wds_rht_params); + + ATOMIC_SET(&adapter->wds_path_num, 0); + adapter->wds_paths = tbl_path; + + return 0; +} + +static +void rtw_wds_path_tbl_expire(_adapter *adapter, + struct rtw_wds_table *tbl) +{ + struct rtw_wds_path *wpath; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((wpath = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(wpath) && PTR_ERR(wpath) == -EAGAIN) + continue; + if (IS_ERR(wpath)) + break; + if (rtw_time_after(rtw_get_current_time(), wpath->last_update + RTW_WDS_PATH_EXPIRE)) + __rtw_wds_path_del(tbl, wpath); + } + +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +void rtw_wds_path_expire(_adapter *adapter) +{ + rtw_wds_path_tbl_expire(adapter, adapter->wds_paths); +} + +void rtw_wds_pathtbl_unregister(_adapter *adapter) +{ + if (adapter->wds_paths) { + rtw_wds_table_free(adapter->wds_paths); + adapter->wds_paths = NULL; + } +} + +int rtw_wds_nexthop_lookup(_adapter *adapter, const u8 *da, u8 *ra) +{ + struct rtw_wds_path *wpath; + struct sta_info *next_hop; + int err = -ENOENT; + + rtw_rcu_read_lock(); + wpath = rtw_wds_path_lookup(adapter, da); + + if (!wpath) + goto endlookup; + + next_hop = rtw_rcu_dereference(wpath->next_hop); + if (next_hop) { + _rtw_memcpy(ra, next_hop->cmn.mac_addr, ETH_ALEN); + err = 0; + } + +endlookup: + rtw_rcu_read_unlock(); + return err; +} + +#endif /* defined(CONFIG_AP_MODE) */ + +/* WDS group adddressed proxy TX record */ +struct rtw_wds_gptr { + u8 src[ETH_ALEN]; + systime last_update; + rtw_rhash_head rhash; + _adapter *adapter; + rtw_rcu_head rcu; +}; + +#define RTW_WDS_GPTR_EXPIRE (2 * HZ) + +/* Maximum number of gptrs per interface */ +#define RTW_WDS_MAX_GPTRS 1024 + +#ifdef PLATFORM_LINUX +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 0, 0)) +static void rtw_wgptr_free_rcu(struct rtw_wds_gptr *wgptr) +{ + kfree_rcu(wgptr, rcu); + rtw_mstat_update(MSTAT_TYPE_PHY, MSTAT_FREE, sizeof(struct rtw_wds_gptr)); +} +#else +static void rtw_wgptr_free_rcu_callback(rtw_rcu_head *head) +{ + struct rtw_wds_gptr *wgptr; + + wgptr = container_of(head, struct rtw_wds_gptr, rcu); + rtw_mfree(wgptr, sizeof(struct rtw_wds_gptr)); +} + +static void rtw_wgptr_free_rcu(struct rtw_wds_gptr *wgptr) +{ + call_rcu(&wgptr->rcu, rtw_wgptr_free_rcu_callback); +} +#endif +#endif /* PLATFORM_LINUX */ + +static void rtw_wds_gptr_free_rcu(struct rtw_wds_gptr_table *tbl, struct rtw_wds_gptr *wgptr) +{ + _adapter *adapter = wgptr->adapter; + + ATOMIC_DEC(&adapter->wds_gpt_record_num); + + rtw_wgptr_free_rcu(wgptr); +} + +static u32 rtw_wds_gptr_table_hash(const void *addr, u32 len, u32 seed) +{ + /* Use last four bytes of hw addr as hash index */ + return jhash_1word(*(u32 *)(addr+2), seed); +} + +static const rtw_rhashtable_params rtw_wds_gptr_rht_params = { + .nelem_hint = 2, + .automatic_shrinking = true, + .key_len = ETH_ALEN, + .key_offset = offsetof(struct rtw_wds_gptr, src), + .head_offset = offsetof(struct rtw_wds_gptr, rhash), + .hashfn = rtw_wds_gptr_table_hash, +}; + +static void rtw_wds_gptr_rht_free(void *ptr, void *tblptr) +{ + struct rtw_wds_gptr *wgptr = ptr; + struct rtw_wds_gptr_table *tbl = tblptr; + + rtw_wds_gptr_free_rcu(tbl, wgptr); +} + +static struct rtw_wds_gptr_table *rtw_wds_gptr_table_alloc(void) +{ + struct rtw_wds_gptr_table *newtbl; + + newtbl = rtw_malloc(sizeof(struct rtw_wds_gptr_table)); + if (!newtbl) + return NULL; + + return newtbl; +} + +static void rtw_wds_gptr_table_free(struct rtw_wds_gptr_table *tbl) +{ + rtw_rhashtable_free_and_destroy(&tbl->rhead, + rtw_wds_gptr_rht_free, tbl); + rtw_mfree(tbl, sizeof(struct rtw_wds_gptr_table)); +} + +static struct rtw_wds_gptr *rtw_wds_gptr_lookup(_adapter *adapter, const u8 *src) +{ + struct rtw_wds_gptr_table *tbl = adapter->wds_gpt_records; + + if (!tbl) + return NULL; + + return rtw_rhashtable_lookup_fast(&tbl->rhead, src, rtw_wds_gptr_rht_params); +} + +/** + * Locking: must be called within a read rcu section. + */ +static struct rtw_wds_gptr *rtw_wds_gptr_lookup_by_idx(_adapter *adapter, int idx) +{ + int i = 0, ret; + struct rtw_wds_gptr_table *tbl = adapter->wds_gpt_records; + struct rtw_wds_gptr *wgptr = NULL; + rtw_rhashtable_iter iter; + + if (!tbl) + return NULL; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return NULL; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto err; + + while ((wgptr = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(wgptr) && PTR_ERR(wgptr) == -EAGAIN) + continue; + if (IS_ERR(wgptr)) + break; + if (i++ == idx) + break; + } +err: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); + + if (IS_ERR(wgptr) || !wgptr) + return NULL; + + return wgptr; +} + +void dump_wgptr(void *sel, _adapter *adapter) +{ + struct rtw_wds_gptr *wgptr; + int idx = 0; + char src[ETH_ALEN]; + u32 age_ms; + + RTW_PRINT_SEL(sel, "num:%d\n", ATOMIC_READ(&adapter->wds_gpt_record_num)); + RTW_PRINT_SEL(sel, "%-17s %-6s\n" + , "src", "age" + ); + + do { + rtw_rcu_read_lock(); + + wgptr = rtw_wds_gptr_lookup_by_idx(adapter, idx); + if (wgptr) { + _rtw_memcpy(src, wgptr->src, ETH_ALEN); + age_ms = rtw_get_passing_time_ms(wgptr->last_update); + } + + rtw_rcu_read_unlock(); + + if (wgptr) { + RTW_PRINT_SEL(sel, MAC_FMT" %6u\n" + , MAC_ARG(src) + , age_ms < 999999 ? age_ms : 999999 + ); + } + + idx++; + } while (wgptr); +} + +static struct rtw_wds_gptr *rtw_wds_gptr_new(_adapter *adapter, const u8 *src) +{ + struct rtw_wds_gptr *new_wgptr; + + new_wgptr = rtw_zmalloc(sizeof(struct rtw_wds_gptr)); + if (!new_wgptr) + return NULL; + + new_wgptr->adapter = adapter; + _rtw_memcpy(new_wgptr->src, src, ETH_ALEN); + new_wgptr->last_update = rtw_get_current_time(); + + return new_wgptr; +} + +static struct rtw_wds_gptr *rtw_wds_gptr_add(_adapter *adapter, const u8 *src) +{ + struct rtw_wds_gptr_table *tbl = adapter->wds_gpt_records; + struct rtw_wds_gptr *wgptr, *new_wgptr; + int ret; + + if (!tbl) + return ERR_PTR(-ENOTSUPP); + + if (ATOMIC_INC_UNLESS(&adapter->wds_gpt_record_num, RTW_WDS_MAX_PATHS) == 0) + return ERR_PTR(-ENOSPC); + + new_wgptr = rtw_wds_gptr_new(adapter, src); + if (!new_wgptr) + return ERR_PTR(-ENOMEM); + + do { + ret = rtw_rhashtable_lookup_insert_fast(&tbl->rhead, + &new_wgptr->rhash, + rtw_wds_gptr_rht_params); + + if (ret == -EEXIST) + wgptr = rtw_rhashtable_lookup_fast(&tbl->rhead, + src, + rtw_wds_gptr_rht_params); + + } while (unlikely(ret == -EEXIST && !wgptr)); + + if (ret && ret != -EEXIST) + return ERR_PTR(ret); + + /* At this point either new_wgptr was added, or we found a + * matching entry already in the table; in the latter case + * free the unnecessary new entry. + */ + if (ret == -EEXIST) { + rtw_mfree(new_wgptr, sizeof(struct rtw_wds_gptr)); + new_wgptr = wgptr; + } + + return new_wgptr; +} + +bool rtw_rx_wds_gptr_check(_adapter *adapter, const u8 *src) +{ + struct rtw_wds_gptr *wgptr; + bool ret = 0; + + rtw_rcu_read_lock(); + + wgptr = rtw_wds_gptr_lookup(adapter, src); + if (wgptr) + ret = rtw_time_after(wgptr->last_update + RTW_WDS_GPTR_EXPIRE, rtw_get_current_time()); + + rtw_rcu_read_unlock(); + + return ret; +} + +void rtw_tx_wds_gptr_update(_adapter *adapter, const u8 *src) +{ + struct rtw_wds_gptr *wgptr; + + rtw_rcu_read_lock(); + wgptr = rtw_wds_gptr_lookup(adapter, src); + if (!wgptr) + rtw_wds_gptr_add(adapter, src); + else + wgptr->last_update = rtw_get_current_time(); + rtw_rcu_read_unlock(); +} + +static void __rtw_wds_gptr_del(struct rtw_wds_gptr_table *tbl, struct rtw_wds_gptr *wgptr) +{ + rtw_rhashtable_remove_fast(&tbl->rhead, &wgptr->rhash, rtw_wds_gptr_rht_params); + rtw_wds_gptr_free_rcu(tbl, wgptr); +} + +void rtw_wds_gptr_expire(_adapter *adapter) +{ + struct rtw_wds_gptr_table *tbl = adapter->wds_gpt_records; + struct rtw_wds_gptr *wgptr; + rtw_rhashtable_iter iter; + int ret; + + if (!tbl) + return; + + ret = rtw_rhashtable_walk_enter(&tbl->rhead, &iter); + if (ret) + return; + + ret = rtw_rhashtable_walk_start(&iter); + if (ret && ret != -EAGAIN) + goto out; + + while ((wgptr = rtw_rhashtable_walk_next(&iter))) { + if (IS_ERR(wgptr) && PTR_ERR(wgptr) == -EAGAIN) + continue; + if (IS_ERR(wgptr)) + break; + if (rtw_time_after(rtw_get_current_time(), wgptr->last_update + RTW_WDS_GPTR_EXPIRE)) + __rtw_wds_gptr_del(tbl, wgptr); + } + +out: + rtw_rhashtable_walk_stop(&iter); + rtw_rhashtable_walk_exit(&iter); +} + +int rtw_wds_gptr_tbl_init(_adapter *adapter) +{ + struct rtw_wds_gptr_table *tbl; + int ret; + + tbl = rtw_wds_gptr_table_alloc(); + if (!tbl) + return -ENOMEM; + + rtw_rhashtable_init(&tbl->rhead, &rtw_wds_gptr_rht_params); + + ATOMIC_SET(&adapter->wds_gpt_record_num, 0); + adapter->wds_gpt_records = tbl; + + return 0; +} + +void rtw_wds_gptr_tbl_unregister(_adapter *adapter) +{ + if (adapter->wds_gpt_records) { + rtw_wds_gptr_table_free(adapter->wds_gpt_records); + adapter->wds_gpt_records = NULL; + } +} +#endif /* defined(CONFIG_RTW_WDS) */ + diff --git a/core/wds/rtw_wds.h b/core/wds/rtw_wds.h new file mode 100644 index 0000000..f1312c7 --- /dev/null +++ b/core/wds/rtw_wds.h @@ -0,0 +1,65 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2019 Realtek Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of version 2 of the GNU General Public License as + * published by the Free Software Foundation. + * + * 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. + * + *****************************************************************************/ +#ifndef __RTW_WDS_H_ +#define __RTW_WDS_H_ + +#ifdef CONFIG_AP_MODE +struct rtw_wds_path { + u8 dst[ETH_ALEN]; + rtw_rhash_head rhash; + _adapter *adapter; + struct sta_info __rcu *next_hop; + rtw_rcu_head rcu; + systime last_update; +}; + +struct rtw_wds_table { + rtw_rhashtable rhead; +}; + +#define RTW_WDS_PATH_EXPIRE (600 * HZ) + +/* Maximum number of paths per interface */ +#define RTW_WDS_MAX_PATHS 1024 + +int rtw_wds_nexthop_lookup(_adapter *adapter, const u8 *da, u8 *ra); + +struct rtw_wds_path *rtw_wds_path_lookup(_adapter *adapter, const u8 *dst); +void dump_wpath(void *sel, _adapter *adapter); + +void rtw_wds_path_expire(_adapter *adapter); + +struct rtw_wds_path *rtw_wds_path_add(_adapter *adapter, const u8 *dst, struct sta_info *next_hop); +void rtw_wds_path_assign_nexthop(struct rtw_wds_path *path, struct sta_info *sta); + +int rtw_wds_pathtbl_init(_adapter *adapter); +void rtw_wds_pathtbl_unregister(_adapter *adapter); + +void rtw_wds_path_flush_by_nexthop(struct sta_info *sta); +#endif /* CONFIG_AP_MODE */ + +struct rtw_wds_gptr_table { + rtw_rhashtable rhead; +}; + +void dump_wgptr(void *sel, _adapter *adapter); +bool rtw_rx_wds_gptr_check(_adapter *adapter, const u8 *src); +void rtw_tx_wds_gptr_update(_adapter *adapter, const u8 *src); +void rtw_wds_gptr_expire(_adapter *adapter); +int rtw_wds_gptr_tbl_init(_adapter *adapter); +void rtw_wds_gptr_tbl_unregister(_adapter *adapter); + +#endif /* __RTW_WDSH_ */ +