diff options
Diffstat (limited to 'net')
49 files changed, 4823 insertions, 960 deletions
diff --git a/net/6lowpan/iphc.c b/net/6lowpan/iphc.c index aced97db62f0..32ffec6ef164 100644 --- a/net/6lowpan/iphc.c +++ b/net/6lowpan/iphc.c @@ -15,9 +15,6 @@ * 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-1301 USA. */ /* Jon's code is based on 6lowpan implementation for Contiki which is: diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig index 5e97a8ff850b..29bcafc41adf 100644 --- a/net/bluetooth/Kconfig +++ b/net/bluetooth/Kconfig @@ -10,6 +10,7 @@ menuconfig BT select CRYPTO select CRYPTO_BLKCIPHER select CRYPTO_AES + select CRYPTO_CMAC select CRYPTO_ECB select CRYPTO_SHA256 help diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile index 886e9aa3ecf1..a5432a6a0ae6 100644 --- a/net/bluetooth/Makefile +++ b/net/bluetooth/Makefile @@ -13,6 +13,6 @@ bluetooth_6lowpan-y := 6lowpan.o bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \ hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \ - a2mp.o amp.o + a2mp.o amp.o ecc.o subdir-ccflags-y += -D__CHECK_ENDIAN__ diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c index 0a7cc565f93e..012e3b03589d 100644 --- a/net/bluetooth/af_bluetooth.c +++ b/net/bluetooth/af_bluetooth.c @@ -31,7 +31,7 @@ #include <net/bluetooth/bluetooth.h> #include <linux/proc_fs.h> -#define VERSION "2.19" +#define VERSION "2.20" /* Bluetooth sockets */ #define BT_MAX_PROTO 8 diff --git a/net/bluetooth/ecc.c b/net/bluetooth/ecc.c new file mode 100644 index 000000000000..e1709f8467ac --- /dev/null +++ b/net/bluetooth/ecc.c @@ -0,0 +1,816 @@ +/* + * Copyright (c) 2013, Kenneth MacKay + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "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 COPYRIGHT + * HOLDER 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. + */ + +#include <linux/random.h> + +#include "ecc.h" + +/* 256-bit curve */ +#define ECC_BYTES 32 + +#define MAX_TRIES 16 + +/* Number of u64's needed */ +#define NUM_ECC_DIGITS (ECC_BYTES / 8) + +struct ecc_point { + u64 x[NUM_ECC_DIGITS]; + u64 y[NUM_ECC_DIGITS]; +}; + +typedef struct { + u64 m_low; + u64 m_high; +} uint128_t; + +#define CURVE_P_32 { 0xFFFFFFFFFFFFFFFFull, 0x00000000FFFFFFFFull, \ + 0x0000000000000000ull, 0xFFFFFFFF00000001ull } + +#define CURVE_G_32 { \ + { 0xF4A13945D898C296ull, 0x77037D812DEB33A0ull, \ + 0xF8BCE6E563A440F2ull, 0x6B17D1F2E12C4247ull }, \ + { 0xCBB6406837BF51F5ull, 0x2BCE33576B315ECEull, \ + 0x8EE7EB4A7C0F9E16ull, 0x4FE342E2FE1A7F9Bull } \ +} + +#define CURVE_N_32 { 0xF3B9CAC2FC632551ull, 0xBCE6FAADA7179E84ull, \ + 0xFFFFFFFFFFFFFFFFull, 0xFFFFFFFF00000000ull } + +static u64 curve_p[NUM_ECC_DIGITS] = CURVE_P_32; +static struct ecc_point curve_g = CURVE_G_32; +static u64 curve_n[NUM_ECC_DIGITS] = CURVE_N_32; + +static void vli_clear(u64 *vli) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) + vli[i] = 0; +} + +/* Returns true if vli == 0, false otherwise. */ +static bool vli_is_zero(const u64 *vli) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + if (vli[i]) + return false; + } + + return true; +} + +/* Returns nonzero if bit bit of vli is set. */ +static u64 vli_test_bit(const u64 *vli, unsigned int bit) +{ + return (vli[bit / 64] & ((u64) 1 << (bit % 64))); +} + +/* Counts the number of 64-bit "digits" in vli. */ +static unsigned int vli_num_digits(const u64 *vli) +{ + int i; + + /* Search from the end until we find a non-zero digit. + * We do it in reverse because we expect that most digits will + * be nonzero. + */ + for (i = NUM_ECC_DIGITS - 1; i >= 0 && vli[i] == 0; i--); + + return (i + 1); +} + +/* Counts the number of bits required for vli. */ +static unsigned int vli_num_bits(const u64 *vli) +{ + unsigned int i, num_digits; + u64 digit; + + num_digits = vli_num_digits(vli); + if (num_digits == 0) + return 0; + + digit = vli[num_digits - 1]; + for (i = 0; digit; i++) + digit >>= 1; + + return ((num_digits - 1) * 64 + i); +} + +/* Sets dest = src. */ +static void vli_set(u64 *dest, const u64 *src) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) + dest[i] = src[i]; +} + +/* Returns sign of left - right. */ +static int vli_cmp(const u64 *left, const u64 *right) +{ + int i; + + for (i = NUM_ECC_DIGITS - 1; i >= 0; i--) { + if (left[i] > right[i]) + return 1; + else if (left[i] < right[i]) + return -1; + } + + return 0; +} + +/* Computes result = in << c, returning carry. Can modify in place + * (if result == in). 0 < shift < 64. + */ +static u64 vli_lshift(u64 *result, const u64 *in, + unsigned int shift) +{ + u64 carry = 0; + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u64 temp = in[i]; + + result[i] = (temp << shift) | carry; + carry = temp >> (64 - shift); + } + + return carry; +} + +/* Computes vli = vli >> 1. */ +static void vli_rshift1(u64 *vli) +{ + u64 *end = vli; + u64 carry = 0; + + vli += NUM_ECC_DIGITS; + + while (vli-- > end) { + u64 temp = *vli; + *vli = (temp >> 1) | carry; + carry = temp << 63; + } +} + +/* Computes result = left + right, returning carry. Can modify in place. */ +static u64 vli_add(u64 *result, const u64 *left, + const u64 *right) +{ + u64 carry = 0; + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u64 sum; + + sum = left[i] + right[i] + carry; + if (sum != left[i]) + carry = (sum < left[i]); + + result[i] = sum; + } + + return carry; +} + +/* Computes result = left - right, returning borrow. Can modify in place. */ +static u64 vli_sub(u64 *result, const u64 *left, const u64 *right) +{ + u64 borrow = 0; + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u64 diff; + + diff = left[i] - right[i] - borrow; + if (diff != left[i]) + borrow = (diff > left[i]); + + result[i] = diff; + } + + return borrow; +} + +static uint128_t mul_64_64(u64 left, u64 right) +{ + u64 a0 = left & 0xffffffffull; + u64 a1 = left >> 32; + u64 b0 = right & 0xffffffffull; + u64 b1 = right >> 32; + u64 m0 = a0 * b0; + u64 m1 = a0 * b1; + u64 m2 = a1 * b0; + u64 m3 = a1 * b1; + uint128_t result; + + m2 += (m0 >> 32); + m2 += m1; + + /* Overflow */ + if (m2 < m1) + m3 += 0x100000000ull; + + result.m_low = (m0 & 0xffffffffull) | (m2 << 32); + result.m_high = m3 + (m2 >> 32); + + return result; +} + +static uint128_t add_128_128(uint128_t a, uint128_t b) +{ + uint128_t result; + + result.m_low = a.m_low + b.m_low; + result.m_high = a.m_high + b.m_high + (result.m_low < a.m_low); + + return result; +} + +static void vli_mult(u64 *result, const u64 *left, const u64 *right) +{ + uint128_t r01 = { 0, 0 }; + u64 r2 = 0; + unsigned int i, k; + + /* Compute each digit of result in sequence, maintaining the + * carries. + */ + for (k = 0; k < NUM_ECC_DIGITS * 2 - 1; k++) { + unsigned int min; + + if (k < NUM_ECC_DIGITS) + min = 0; + else + min = (k + 1) - NUM_ECC_DIGITS; + + for (i = min; i <= k && i < NUM_ECC_DIGITS; i++) { + uint128_t product; + + product = mul_64_64(left[i], right[k - i]); + + r01 = add_128_128(r01, product); + r2 += (r01.m_high < product.m_high); + } + + result[k] = r01.m_low; + r01.m_low = r01.m_high; + r01.m_high = r2; + r2 = 0; + } + + result[NUM_ECC_DIGITS * 2 - 1] = r01.m_low; +} + +static void vli_square(u64 *result, const u64 *left) +{ + uint128_t r01 = { 0, 0 }; + u64 r2 = 0; + int i, k; + + for (k = 0; k < NUM_ECC_DIGITS * 2 - 1; k++) { + unsigned int min; + + if (k < NUM_ECC_DIGITS) + min = 0; + else + min = (k + 1) - NUM_ECC_DIGITS; + + for (i = min; i <= k && i <= k - i; i++) { + uint128_t product; + + product = mul_64_64(left[i], left[k - i]); + + if (i < k - i) { + r2 += product.m_high >> 63; + product.m_high = (product.m_high << 1) | + (product.m_low >> 63); + product.m_low <<= 1; + } + + r01 = add_128_128(r01, product); + r2 += (r01.m_high < product.m_high); + } + + result[k] = r01.m_low; + r01.m_low = r01.m_high; + r01.m_high = r2; + r2 = 0; + } + + result[NUM_ECC_DIGITS * 2 - 1] = r01.m_low; +} + +/* Computes result = (left + right) % mod. + * Assumes that left < mod and right < mod, result != mod. + */ +static void vli_mod_add(u64 *result, const u64 *left, const u64 *right, + const u64 *mod) +{ + u64 carry; + + carry = vli_add(result, left, right); + + /* result > mod (result = mod + remainder), so subtract mod to + * get remainder. + */ + if (carry || vli_cmp(result, mod) >= 0) + vli_sub(result, result, mod); +} + +/* Computes result = (left - right) % mod. + * Assumes that left < mod and right < mod, result != mod. + */ +static void vli_mod_sub(u64 *result, const u64 *left, const u64 *right, + const u64 *mod) +{ + u64 borrow = vli_sub(result, left, right); + + /* In this case, p_result == -diff == (max int) - diff. + * Since -x % d == d - x, we can get the correct result from + * result + mod (with overflow). + */ + if (borrow) + vli_add(result, result, mod); +} + +/* Computes result = product % curve_p + from http://www.nsa.gov/ia/_files/nist-routines.pdf */ +static void vli_mmod_fast(u64 *result, const u64 *product) +{ + u64 tmp[NUM_ECC_DIGITS]; + int carry; + + /* t */ + vli_set(result, product); + + /* s1 */ + tmp[0] = 0; + tmp[1] = product[5] & 0xffffffff00000000ull; + tmp[2] = product[6]; + tmp[3] = product[7]; + carry = vli_lshift(tmp, tmp, 1); + carry += vli_add(result, result, tmp); + + /* s2 */ + tmp[1] = product[6] << 32; + tmp[2] = (product[6] >> 32) | (product[7] << 32); + tmp[3] = product[7] >> 32; + carry += vli_lshift(tmp, tmp, 1); + carry += vli_add(result, result, tmp); + + /* s3 */ + tmp[0] = product[4]; + tmp[1] = product[5] & 0xffffffff; + tmp[2] = 0; + tmp[3] = product[7]; + carry += vli_add(result, result, tmp); + + /* s4 */ + tmp[0] = (product[4] >> 32) | (product[5] << 32); + tmp[1] = (product[5] >> 32) | (product[6] & 0xffffffff00000000ull); + tmp[2] = product[7]; + tmp[3] = (product[6] >> 32) | (product[4] << 32); + carry += vli_add(result, result, tmp); + + /* d1 */ + tmp[0] = (product[5] >> 32) | (product[6] << 32); + tmp[1] = (product[6] >> 32); + tmp[2] = 0; + tmp[3] = (product[4] & 0xffffffff) | (product[5] << 32); + carry -= vli_sub(result, result, tmp); + + /* d2 */ + tmp[0] = product[6]; + tmp[1] = product[7]; + tmp[2] = 0; + tmp[3] = (product[4] >> 32) | (product[5] & 0xffffffff00000000ull); + carry -= vli_sub(result, result, tmp); + + /* d3 */ + tmp[0] = (product[6] >> 32) | (product[7] << 32); + tmp[1] = (product[7] >> 32) | (product[4] << 32); + tmp[2] = (product[4] >> 32) | (product[5] << 32); + tmp[3] = (product[6] << 32); + carry -= vli_sub(result, result, tmp); + + /* d4 */ + tmp[0] = product[7]; + tmp[1] = product[4] & 0xffffffff00000000ull; + tmp[2] = product[5]; + tmp[3] = product[6] & 0xffffffff00000000ull; + carry -= vli_sub(result, result, tmp); + + if (carry < 0) { + do { + carry += vli_add(result, result, curve_p); + } while (carry < 0); + } else { + while (carry || vli_cmp(curve_p, result) != 1) + carry -= vli_sub(result, result, curve_p); + } +} + +/* Computes result = (left * right) % curve_p. */ +static void vli_mod_mult_fast(u64 *result, const u64 *left, const u64 *right) +{ + u64 product[2 * NUM_ECC_DIGITS]; + + vli_mult(product, left, right); + vli_mmod_fast(result, product); +} + +/* Computes result = left^2 % curve_p. */ +static void vli_mod_square_fast(u64 *result, const u64 *left) +{ + u64 product[2 * NUM_ECC_DIGITS]; + + vli_square(product, left); + vli_mmod_fast(result, product); +} + +#define EVEN(vli) (!(vli[0] & 1)) +/* Computes result = (1 / p_input) % mod. All VLIs are the same size. + * See "From Euclid's GCD to Montgomery Multiplication to the Great Divide" + * https://labs.oracle.com/techrep/2001/smli_tr-2001-95.pdf + */ +static void vli_mod_inv(u64 *result, const u64 *input, const u64 *mod) +{ + u64 a[NUM_ECC_DIGITS], b[NUM_ECC_DIGITS]; + u64 u[NUM_ECC_DIGITS], v[NUM_ECC_DIGITS]; + u64 carry; + int cmp_result; + + if (vli_is_zero(input)) { + vli_clear(result); + return; + } + + vli_set(a, input); + vli_set(b, mod); + vli_clear(u); + u[0] = 1; + vli_clear(v); + + while ((cmp_result = vli_cmp(a, b)) != 0) { + carry = 0; + + if (EVEN(a)) { + vli_rshift1(a); + + if (!EVEN(u)) + carry = vli_add(u, u, mod); + + vli_rshift1(u); + if (carry) + u[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } else if (EVEN(b)) { + vli_rshift1(b); + + if (!EVEN(v)) + carry = vli_add(v, v, mod); + + vli_rshift1(v); + if (carry) + v[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } else if (cmp_result > 0) { + vli_sub(a, a, b); + vli_rshift1(a); + + if (vli_cmp(u, v) < 0) + vli_add(u, u, mod); + + vli_sub(u, u, v); + if (!EVEN(u)) + carry = vli_add(u, u, mod); + + vli_rshift1(u); + if (carry) + u[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } else { + vli_sub(b, b, a); + vli_rshift1(b); + + if (vli_cmp(v, u) < 0) + vli_add(v, v, mod); + + vli_sub(v, v, u); + if (!EVEN(v)) + carry = vli_add(v, v, mod); + + vli_rshift1(v); + if (carry) + v[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } + } + + vli_set(result, u); +} + +/* ------ Point operations ------ */ + +/* Returns true if p_point is the point at infinity, false otherwise. */ +static bool ecc_point_is_zero(const struct ecc_point *point) +{ + return (vli_is_zero(point->x) && vli_is_zero(point->y)); +} + +/* Point multiplication algorithm using Montgomery's ladder with co-Z + * coordinates. From http://eprint.iacr.org/2011/338.pdf + */ + +/* Double in place */ +static void ecc_point_double_jacobian(u64 *x1, u64 *y1, u64 *z1) +{ + /* t1 = x, t2 = y, t3 = z */ + u64 t4[NUM_ECC_DIGITS]; + u64 t5[NUM_ECC_DIGITS]; + + if (vli_is_zero(z1)) + return; + + vli_mod_square_fast(t4, y1); /* t4 = y1^2 */ + vli_mod_mult_fast(t5, x1, t4); /* t5 = x1*y1^2 = A */ + vli_mod_square_fast(t4, t4); /* t4 = y1^4 */ + vli_mod_mult_fast(y1, y1, z1); /* t2 = y1*z1 = z3 */ + vli_mod_square_fast(z1, z1); /* t3 = z1^2 */ + + vli_mod_add(x1, x1, z1, curve_p); /* t1 = x1 + z1^2 */ + vli_mod_add(z1, z1, z1, curve_p); /* t3 = 2*z1^2 */ + vli_mod_sub(z1, x1, z1, curve_p); /* t3 = x1 - z1^2 */ + vli_mod_mult_fast(x1, x1, z1); /* t1 = x1^2 - z1^4 */ + + vli_mod_add(z1, x1, x1, curve_p); /* t3 = 2*(x1^2 - z1^4) */ + vli_mod_add(x1, x1, z1, curve_p); /* t1 = 3*(x1^2 - z1^4) */ + if (vli_test_bit(x1, 0)) { + u64 carry = vli_add(x1, x1, curve_p); + vli_rshift1(x1); + x1[NUM_ECC_DIGITS - 1] |= carry << 63; + } else { + vli_rshift1(x1); + } + /* t1 = 3/2*(x1^2 - z1^4) = B */ + + vli_mod_square_fast(z1, x1); /* t3 = B^2 */ + vli_mod_sub(z1, z1, t5, curve_p); /* t3 = B^2 - A */ + vli_mod_sub(z1, z1, t5, curve_p); /* t3 = B^2 - 2A = x3 */ + vli_mod_sub(t5, t5, z1, curve_p); /* t5 = A - x3 */ + vli_mod_mult_fast(x1, x1, t5); /* t1 = B * (A - x3) */ + vli_mod_sub(t4, x1, t4, curve_p); /* t4 = B * (A - x3) - y1^4 = y3 */ + + vli_set(x1, z1); + vli_set(z1, y1); + vli_set(y1, t4); +} + +/* Modify (x1, y1) => (x1 * z^2, y1 * z^3) */ +static void apply_z(u64 *x1, u64 *y1, u64 *z) +{ + u64 t1[NUM_ECC_DIGITS]; + + vli_mod_square_fast(t1, z); /* z^2 */ + vli_mod_mult_fast(x1, x1, t1); /* x1 * z^2 */ + vli_mod_mult_fast(t1, t1, z); /* z^3 */ + vli_mod_mult_fast(y1, y1, t1); /* y1 * z^3 */ +} + +/* P = (x1, y1) => 2P, (x2, y2) => P' */ +static void xycz_initial_double(u64 *x1, u64 *y1, u64 *x2, u64 *y2, + u64 *p_initial_z) +{ + u64 z[NUM_ECC_DIGITS]; + + vli_set(x2, x1); + vli_set(y2, y1); + + vli_clear(z); + z[0] = 1; + + if (p_initial_z) + vli_set(z, p_initial_z); + + apply_z(x1, y1, z); + + ecc_point_double_jacobian(x1, y1, z); + + apply_z(x2, y2, z); +} + +/* Input P = (x1, y1, Z), Q = (x2, y2, Z) + * Output P' = (x1', y1', Z3), P + Q = (x3, y3, Z3) + * or P => P', Q => P + Q + */ +static void xycz_add(u64 *x1, u64 *y1, u64 *x2, u64 *y2) +{ + /* t1 = X1, t2 = Y1, t3 = X2, t4 = Y2 */ + u64 t5[NUM_ECC_DIGITS]; + + vli_mod_sub(t5, x2, x1, curve_p); /* t5 = x2 - x1 */ + vli_mod_square_fast(t5, t5); /* t5 = (x2 - x1)^2 = A */ + vli_mod_mult_fast(x1, x1, t5); /* t1 = x1*A = B */ + vli_mod_mult_fast(x2, x2, t5); /* t3 = x2*A = C */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y2 - y1 */ + vli_mod_square_fast(t5, y2); /* t5 = (y2 - y1)^2 = D */ + + vli_mod_sub(t5, t5, x1, curve_p); /* t5 = D - B */ + vli_mod_sub(t5, t5, x2, curve_p); /* t5 = D - B - C = x3 */ + vli_mod_sub(x2, x2, x1, curve_p); /* t3 = C - B */ + vli_mod_mult_fast(y1, y1, x2); /* t2 = y1*(C - B) */ + vli_mod_sub(x2, x1, t5, curve_p); /* t3 = B - x3 */ + vli_mod_mult_fast(y2, y2, x2); /* t4 = (y2 - y1)*(B - x3) */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y3 */ + + vli_set(x2, t5); +} + +/* Input P = (x1, y1, Z), Q = (x2, y2, Z) + * Output P + Q = (x3, y3, Z3), P - Q = (x3', y3', Z3) + * or P => P - Q, Q => P + Q + */ +static void xycz_add_c(u64 *x1, u64 *y1, u64 *x2, u64 *y2) +{ + /* t1 = X1, t2 = Y1, t3 = X2, t4 = Y2 */ + u64 t5[NUM_ECC_DIGITS]; + u64 t6[NUM_ECC_DIGITS]; + u64 t7[NUM_ECC_DIGITS]; + + vli_mod_sub(t5, x2, x1, curve_p); /* t5 = x2 - x1 */ + vli_mod_square_fast(t5, t5); /* t5 = (x2 - x1)^2 = A */ + vli_mod_mult_fast(x1, x1, t5); /* t1 = x1*A = B */ + vli_mod_mult_fast(x2, x2, t5); /* t3 = x2*A = C */ + vli_mod_add(t5, y2, y1, curve_p); /* t4 = y2 + y1 */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y2 - y1 */ + + vli_mod_sub(t6, x2, x1, curve_p); /* t6 = C - B */ + vli_mod_mult_fast(y1, y1, t6); /* t2 = y1 * (C - B) */ + vli_mod_add(t6, x1, x2, curve_p); /* t6 = B + C */ + vli_mod_square_fast(x2, y2); /* t3 = (y2 - y1)^2 */ + vli_mod_sub(x2, x2, t6, curve_p); /* t3 = x3 */ + + vli_mod_sub(t7, x1, x2, curve_p); /* t7 = B - x3 */ + vli_mod_mult_fast(y2, y2, t7); /* t4 = (y2 - y1)*(B - x3) */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y3 */ + + vli_mod_square_fast(t7, t5); /* t7 = (y2 + y1)^2 = F */ + vli_mod_sub(t7, t7, t6, curve_p); /* t7 = x3' */ + vli_mod_sub(t6, t7, x1, curve_p); /* t6 = x3' - B */ + vli_mod_mult_fast(t6, t6, t5); /* t6 = (y2 + y1)*(x3' - B) */ + vli_mod_sub(y1, t6, y1, curve_p); /* t2 = y3' */ + + vli_set(x1, t7); +} + +static void ecc_point_mult(struct ecc_point *result, + const struct ecc_point *point, u64 *scalar, + u64 *initial_z, int num_bits) +{ + /* R0 and R1 */ + u64 rx[2][NUM_ECC_DIGITS]; + u64 ry[2][NUM_ECC_DIGITS]; + u64 z[NUM_ECC_DIGITS]; + int i, nb; + + vli_set(rx[1], point->x); + vli_set(ry[1], point->y); + + xycz_initial_double(rx[1], ry[1], rx[0], ry[0], initial_z); + + for (i = num_bits - 2; i > 0; i--) { + nb = !vli_test_bit(scalar, i); + xycz_add_c(rx[1 - nb], ry[1 - nb], rx[nb], ry[nb]); + xycz_add(rx[nb], ry[nb], rx[1 - nb], ry[1 - nb]); + } + + nb = !vli_test_bit(scalar, 0); + xycz_add_c(rx[1 - nb], ry[1 - nb], rx[nb], ry[nb]); + + /* Find final 1/Z value. */ + vli_mod_sub(z, rx[1], rx[0], curve_p); /* X1 - X0 */ + vli_mod_mult_fast(z, z, ry[1 - nb]); /* Yb * (X1 - X0) */ + vli_mod_mult_fast(z, z, point->x); /* xP * Yb * (X1 - X0) */ + vli_mod_inv(z, z, curve_p); /* 1 / (xP * Yb * (X1 - X0)) */ + vli_mod_mult_fast(z, z, point->y); /* yP / (xP * Yb * (X1 - X0)) */ + vli_mod_mult_fast(z, z, rx[1 - nb]); /* Xb * yP / (xP * Yb * (X1 - X0)) */ + /* End 1/Z calculation */ + + xycz_add(rx[nb], ry[nb], rx[1 - nb], ry[1 - nb]); + + apply_z(rx[0], ry[0], z); + + vli_set(result->x, rx[0]); + vli_set(result->y, ry[0]); +} + +static void ecc_bytes2native(const u8 bytes[ECC_BYTES], + u64 native[NUM_ECC_DIGITS]) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + const u8 *digit = bytes + 8 * (NUM_ECC_DIGITS - 1 - i); + + native[NUM_ECC_DIGITS - 1 - i] = + ((u64) digit[0] << 0) | + ((u64) digit[1] << 8) | + ((u64) digit[2] << 16) | + ((u64) digit[3] << 24) | + ((u64) digit[4] << 32) | + ((u64) digit[5] << 40) | + ((u64) digit[6] << 48) | + ((u64) digit[7] << 56); + } +} + +static void ecc_native2bytes(const u64 native[NUM_ECC_DIGITS], + u8 bytes[ECC_BYTES]) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u8 *digit = bytes + 8 * (NUM_ECC_DIGITS - 1 - i); + + digit[0] = native[NUM_ECC_DIGITS - 1 - i] >> 0; + digit[1] = native[NUM_ECC_DIGITS - 1 - i] >> 8; + digit[2] = native[NUM_ECC_DIGITS - 1 - i] >> 16; + digit[3] = native[NUM_ECC_DIGITS - 1 - i] >> 24; + digit[4] = native[NUM_ECC_DIGITS - 1 - i] >> 32; + digit[5] = native[NUM_ECC_DIGITS - 1 - i] >> 40; + digit[6] = native[NUM_ECC_DIGITS - 1 - i] >> 48; + digit[7] = native[NUM_ECC_DIGITS - 1 - i] >> 56; + } +} + +bool ecc_make_key(u8 public_key[64], u8 private_key[32]) +{ + struct ecc_point pk; + u64 priv[NUM_ECC_DIGITS]; + unsigned int tries = 0; + + do { + if (tries++ >= MAX_TRIES) + return false; + + get_random_bytes(priv, ECC_BYTES); + + if (vli_is_zero(priv)) + continue; + + /* Make sure the private key is in the range [1, n-1]. */ + if (vli_cmp(curve_n, priv) != 1) + continue; + + ecc_point_mult(&pk, &curve_g, priv, NULL, vli_num_bits(priv)); + } while (ecc_point_is_zero(&pk)); + + ecc_native2bytes(priv, private_key); + ecc_native2bytes(pk.x, public_key); + ecc_native2bytes(pk.y, &public_key[32]); + + return true; +} + +bool ecdh_shared_secret(const u8 public_key[64], const u8 private_key[32], + u8 secret[32]) +{ + u64 priv[NUM_ECC_DIGITS]; + u64 rand[NUM_ECC_DIGITS]; + struct ecc_point product, pk; + + get_random_bytes(rand, ECC_BYTES); + + ecc_bytes2native(public_key, pk.x); + ecc_bytes2native(&public_key[32], pk.y); + ecc_bytes2native(private_key, priv); + + ecc_point_mult(&product, &pk, priv, rand, vli_num_bits(priv)); + + ecc_native2bytes(product.x, secret); + + return !ecc_point_is_zero(&product); +} diff --git a/net/bluetooth/ecc.h b/net/bluetooth/ecc.h new file mode 100644 index 000000000000..8d6a2f4d1905 --- /dev/null +++ b/net/bluetooth/ecc.h @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2013, Kenneth MacKay + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "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 COPYRIGHT + * HOLDER 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. + */ + +/* Create a public/private key pair. + * Outputs: + * public_key - Will be filled in with the public key. + * private_key - Will be filled in with the private key. + * + * Returns true if the key pair was generated successfully, false + * if an error occurred. The keys are with the LSB first. + */ +bool ecc_make_key(u8 public_key[64], u8 private_key[32]); + +/* Compute a shared secret given your secret key and someone else's + * public key. + * Note: It is recommended that you hash the result of ecdh_shared_secret + * before using it for symmetric encryption or HMAC. + * + * Inputs: + * public_key - The public key of the remote party + * private_key - Your private key. + * + * Outputs: + * secret - Will be filled in with the shared secret value. + * + * Returns true if the shared secret was generated successfully, false + * if an error occurred. Both input and output parameters are with the + * LSB first. + */ +bool ecdh_shared_secret(const u8 public_key[64], const u8 private_key[32], + u8 secret[32]); diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c index 96887ae8375b..79d84b88b8f0 100644 --- a/net/bluetooth/hci_conn.c +++ b/net/bluetooth/hci_conn.c @@ -449,6 +449,7 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst, conn->io_capability = hdev->io_capability; conn->remote_auth = 0xff; conn->key_type = 0xff; + conn->rssi = HCI_RSSI_INVALID; conn->tx_power = HCI_TX_POWER_INVALID; conn->max_tx_power = HCI_TX_POWER_INVALID; diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index d786958a1dec..93f92a085506 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -274,15 +274,13 @@ static const struct file_operations inquiry_cache_fops = { static int link_keys_show(struct seq_file *f, void *ptr) { struct hci_dev *hdev = f->private; - struct list_head *p, *n; + struct link_key *key; - hci_dev_lock(hdev); - list_for_each_safe(p, n, &hdev->link_keys) { - struct link_key *key = list_entry(p, struct link_key, list); + rcu_read_lock(); + list_for_each_entry_rcu(key, &hdev->link_keys, list) seq_printf(f, "%pMR %u %*phN %u\n", &key->bdaddr, key->type, HCI_LINK_KEY_SIZE, key->val, key->pin_len); - } - hci_dev_unlock(hdev); + rcu_read_unlock(); return 0; } @@ -408,6 +406,49 @@ static const struct file_operations force_sc_support_fops = { .llseek = default_llseek, }; +static ssize_t force_lesc_support_read(struct file *file, char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct hci_dev *hdev = file->private_data; + char buf[3]; + + buf[0] = test_bit(HCI_FORCE_LESC, &hdev->dbg_flags) ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static ssize_t force_lesc_support_write(struct file *file, + const char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct hci_dev *hdev = file->private_data; + char buf[32]; + size_t buf_size = min(count, (sizeof(buf)-1)); + bool enable; + + if (copy_from_user(buf, user_buf, buf_size)) + return -EFAULT; + + buf[buf_size] = '\0'; + if (strtobool(buf, &enable)) + return -EINVAL; + + if (enable == test_bit(HCI_FORCE_LESC, &hdev->dbg_flags)) + return -EALREADY; + + change_bit(HCI_FORCE_LESC, &hdev->dbg_flags); + + return count; +} + +static const struct file_operations force_lesc_support_fops = { + .open = simple_open, + .read = force_lesc_support_read, + .write = force_lesc_support_write, + .llseek = default_llseek, +}; + static ssize_t sc_only_mode_read(struct file *file, char __user *user_buf, size_t count, loff_t *ppos) { @@ -1128,6 +1169,7 @@ struct sk_buff *__hci_cmd_sync_ev(struct hci_dev *hdev, u16 opcode, u32 plen, err = hci_req_run(&req, hci_req_sync_complete); if (err < 0) { remove_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_RUNNING); return ERR_PTR(err); } @@ -1196,6 +1238,7 @@ static int __hci_req_sync(struct hci_dev *hdev, hdev->req_status = 0; remove_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_RUNNING); /* ENODATA means the HCI request command queue is empty. * This can happen when a request with conditionals doesn't @@ -1692,6 +1735,28 @@ static void hci_init3_req(struct hci_request *req, unsigned long opt) * Parameter Request */ + /* If the controller supports Extended Scanner Filter + * Policies, enable the correspondig event. + */ + if (hdev->le_features[0] & HCI_LE_EXT_SCAN_POLICY) + events[1] |= 0x04; /* LE Direct Advertising + * Report + */ + + /* If the controller supports the LE Read Local P-256 + * Public Key command, enable the corresponding event. + */ + if (hdev->commands[34] & 0x02) + events[0] |= 0x80; /* LE Read Local P-256 + * Public Key Complete + */ + + /* If the controller supports the LE Generate DHKey + * command, enable the corresponding event. + */ + if (hdev->commands[34] & 0x04) + events[1] |= 0x01; /* LE Generate DHKey Complete */ + hci_req_add(req, HCI_OP_LE_SET_EVENT_MASK, sizeof(events), events); @@ -1734,9 +1799,7 @@ static void hci_init4_req(struct hci_request *req, unsigned long opt) hci_req_add(req, HCI_OP_READ_SYNC_TRAIN_PARAMS, 0, NULL); /* Enable Secure Connections if supported and configured */ - if ((lmp_sc_capable(hdev) || - test_bit(HCI_FORCE_SC, &hdev->dbg_flags)) && - test_bit(HCI_SC_ENABLED, &hdev->dev_flags)) { + if (bredr_sc_enabled(hdev)) { u8 support = 0x01; hci_req_add(req, HCI_OP_WRITE_SC_SUPPORT, sizeof(support), &support); @@ -1819,6 +1882,10 @@ static int __hci_init(struct hci_dev *hdev) hdev, &force_sc_support_fops); debugfs_create_file("sc_only_mode", 0444, hdev->debugfs, hdev, &sc_only_mode_fops); + if (lmp_le_capable(hdev)) + debugfs_create_file("force_lesc_support", 0644, + hdev->debugfs, hdev, + &force_lesc_support_fops); } if (lmp_sniff_capable(hdev)) { @@ -2115,7 +2182,7 @@ u32 hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data, BT_DBG("cache %p, %pMR", cache, &data->bdaddr); - hci_remove_remote_oob_data(hdev, &data->bdaddr); + hci_remove_remote_oob_data(hdev, &data->bdaddr, BDADDR_BREDR); if (!data->ssp_mode) flags |= MGMT_DEV_FOUND_LEGACY_PAIRING; @@ -3099,15 +3166,11 @@ void hci_uuids_clear(struct hci_dev *hdev) void hci_link_keys_clear(struct hci_dev *hdev) { - struct list_head *p, *n; - - list_for_each_safe(p, n, &hdev->link_keys) { - struct link_key *key; - - key = list_entry(p, struct link_key, list); + struct link_key *key; - list_del(p); - kfree(key); + list_for_each_entry_rcu(key, &hdev->link_keys, list) { + list_del_rcu(&key->list); + kfree_rcu(key, rcu); } } @@ -3135,9 +3198,14 @@ struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr) { struct link_key *k; - list_for_each_entry(k, &hdev->link_keys, list) - if (bacmp(bdaddr, &k->bdaddr) == 0) + rcu_read_lock(); + list_for_each_entry_rcu(k, &hdev->link_keys, list) { + if (bacmp(bdaddr, &k->bdaddr) == 0) { + rcu_read_unlock(); return k; + } + } + rcu_read_unlock(); return NULL; } @@ -3161,6 +3229,10 @@ static bool hci_persistent_key(struct hci_dev *hdev, struct hci_conn *conn, if (!conn) return true; + /* BR/EDR key derived using SC from an LE link */ + if (conn->type == LE_LINK) + return true; + /* Neither local nor remote side had no-bonding as requirement */ if (conn->auth_type > 0x01 && conn->remote_auth > 0x01) return true; @@ -3186,37 +3258,17 @@ static u8 ltk_role(u8 type) return HCI_ROLE_SLAVE; } -struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, __le16 ediv, __le64 rand, - u8 role) +struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type, u8 role) { struct smp_ltk *k; rcu_read_lock(); list_for_each_entry_rcu(k, &hdev->long_term_keys, list) { - if (k->ediv != ediv || k->rand != rand) - continue; - - if (ltk_role(k->type) != role) + if (addr_type != k->bdaddr_type || bacmp(bdaddr, &k->bdaddr)) continue; - rcu_read_unlock(); - return k; - } - rcu_read_unlock(); - - return NULL; -} - -struct smp_ltk *hci_find_ltk_by_addr(struct hci_dev *hdev, bdaddr_t *bdaddr, - u8 addr_type, u8 role) -{ - struct smp_ltk *k; - - rcu_read_lock(); - list_for_each_entry_rcu(k, &hdev->long_term_keys, list) { - if (addr_type == k->bdaddr_type && - bacmp(bdaddr, &k->bdaddr) == 0 && - ltk_role(k->type) == role) { + if (smp_ltk_is_sc(k) || ltk_role(k->type) == role) { rcu_read_unlock(); return k; } @@ -3288,7 +3340,7 @@ struct link_key *hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, key = kzalloc(sizeof(*key), GFP_KERNEL); if (!key) return NULL; - list_add(&key->list, &hdev->link_keys); + list_add_rcu(&key->list, &hdev->link_keys); } BT_DBG("%s key for %pMR type %u", hdev->name, bdaddr, type); @@ -3326,7 +3378,7 @@ struct smp_ltk *hci_add_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, struct smp_ltk *key, *old_key; u8 role = ltk_role(type); - old_key = hci_find_ltk_by_addr(hdev, bdaddr, addr_type, role); + old_key = hci_find_ltk(hdev, bdaddr, addr_type, role); if (old_key) key = old_key; else { @@ -3381,8 +3433,8 @@ int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr) BT_DBG("%s removing %pMR", hdev->name, bdaddr); - list_del(&key->list); - kfree(key); + list_del_rcu(&key->list); + kfree_rcu(key, rcu); return 0; } @@ -3441,26 +3493,31 @@ static void hci_cmd_timeout(struct work_struct *work) } struct oob_data *hci_find_remote_oob_data(struct hci_dev *hdev, - bdaddr_t *bdaddr) + bdaddr_t *bdaddr, u8 bdaddr_type) { struct oob_data *data; - list_for_each_entry(data, &hdev->remote_oob_data, list) - if (bacmp(bdaddr, &data->bdaddr) == 0) - return data; + list_for_each_entry(data, &hdev->remote_oob_data, list) { + if (bacmp(bdaddr, &data->bdaddr) != 0) + continue; + if (data->bdaddr_type != bdaddr_type) + continue; + return data; + } return NULL; } -int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr) +int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 bdaddr_type) { struct oob_data *data; - data = hci_find_remote_oob_data(hdev, bdaddr); + data = hci_find_remote_oob_data(hdev, bdaddr, bdaddr_type); if (!data) return -ENOENT; - BT_DBG("%s removing %pMR", hdev->name, bdaddr); + BT_DBG("%s removing %pMR (%u)", hdev->name, bdaddr, bdaddr_type); list_del(&data->list); kfree(data); @@ -3479,52 +3536,37 @@ void hci_remote_oob_data_clear(struct hci_dev *hdev) } int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, - u8 *hash, u8 *rand) + u8 bdaddr_type, u8 *hash192, u8 *rand192, + u8 *hash256, u8 *rand256) { struct oob_data *data; - data = hci_find_remote_oob_data(hdev, bdaddr); + data = hci_find_remote_oob_data(hdev, bdaddr, bdaddr_type); if (!data) { data = kmalloc(sizeof(*data), GFP_KERNEL); if (!data) return -ENOMEM; bacpy(&data->bdaddr, bdaddr); + data->bdaddr_type = bdaddr_type; list_add(&data->list, &hdev->remote_oob_data); } - memcpy(data->hash192, hash, sizeof(data->hash192)); - memcpy(data->rand192, rand, sizeof(data->rand192)); - - memset(data->hash256, 0, sizeof(data->hash256)); - memset(data->rand256, 0, sizeof(data->rand256)); - - BT_DBG("%s for %pMR", hdev->name, bdaddr); - - return 0; -} - -int hci_add_remote_oob_ext_data(struct hci_dev *hdev, bdaddr_t *bdaddr, - u8 *hash192, u8 *rand192, - u8 *hash256, u8 *rand256) -{ - struct oob_data *data; - - data = hci_find_remote_oob_data(hdev, bdaddr); - if (!data) { - data = kmalloc(sizeof(*data), GFP_KERNEL); - if (!data) - return -ENOMEM; - - bacpy(&data->bdaddr, bdaddr); - list_add(&data->list, &hdev->remote_oob_data); + if (hash192 && rand192) { + memcpy(data->hash192, hash192, sizeof(data->hash192)); + memcpy(data->rand192, rand192, sizeof(data->rand192)); + } else { + memset(data->hash192, 0, sizeof(data->hash192)); + memset(data->rand192, 0, sizeof(data->rand192)); } - memcpy(data->hash192, hash192, sizeof(data->hash192)); - memcpy(data->rand192, rand192, sizeof(data->rand192)); - - memcpy(data->hash256, hash256, sizeof(data->hash256)); - memcpy(data->rand256, rand256, sizeof(data->rand256)); + if (hash256 && rand256) { + memcpy(data->hash256, hash256, sizeof(data->hash256)); + memcpy(data->rand256, rand256, sizeof(data->rand256)); + } else { + memset(data->hash256, 0, sizeof(data->hash256)); + memset(data->rand256, 0, sizeof(data->rand256)); + } BT_DBG("%s for %pMR", hdev->name, bdaddr); @@ -4224,6 +4266,7 @@ void hci_unregister_dev(struct hci_dev *hdev) hci_remote_oob_data_clear(hdev); hci_bdaddr_list_clear(&hdev->le_white_list); hci_conn_params_clear_all(hdev); + hci_discovery_filter_clear(hdev); hci_dev_unlock(hdev); hci_dev_put(hdev); @@ -5596,6 +5639,19 @@ void hci_req_add_le_passive_scan(struct hci_request *req) */ filter_policy = update_white_list(req); + /* When the controller is using random resolvable addresses and + * with that having LE privacy enabled, then controllers with + * Extended Scanner Filter Policies support can now enable support + * for handling directed advertising. + * + * So instead of using filter polices 0x00 (no whitelist) + * and 0x01 (whitelist enabled) use the new filter policies + * 0x02 (no whitelist) and 0x03 (whitelist enabled). + */ + if (test_bit(HCI_PRIVACY, &hdev->dev_flags) && + (hdev->le_features[0] & HCI_LE_EXT_SCAN_POLICY)) + filter_policy |= 0x02; + memset(¶m_cp, 0, sizeof(param_cp)); param_cp.type = LE_SCAN_PASSIVE; param_cp.interval = cpu_to_le16(hdev->le_scan_interval); @@ -5647,6 +5703,15 @@ void hci_update_background_scan(struct hci_dev *hdev) if (hdev->discovery.state != DISCOVERY_STOPPED) return; + /* Reset RSSI and UUID filters when starting background scanning + * since these filters are meant for service discovery only. + * + * The Start Discovery and Start Service Discovery operations + * ensure to set proper values for RSSI threshold and UUID + * filter list. So it is safe to just reset them here. + */ + hci_discovery_filter_clear(hdev); + hci_req_init(&req, hdev); if (list_empty(&hdev->pend_le_conns) && diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 844f7d1ff1cd..322abbbbcef9 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -2043,13 +2043,14 @@ static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) data.pscan_mode = info->pscan_mode; memcpy(data.dev_class, info->dev_class, 3); data.clock_offset = info->clock_offset; - data.rssi = 0x00; + data.rssi = HCI_RSSI_INVALID; data.ssp_mode = 0x00; flags = hci_inquiry_cache_update(hdev, &data, false); mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, - info->dev_class, 0, flags, NULL, 0, NULL, 0); + info->dev_class, HCI_RSSI_INVALID, + flags, NULL, 0, NULL, 0); } hci_dev_unlock(hdev); @@ -3191,6 +3192,38 @@ unlock: hci_dev_unlock(hdev); } +static void conn_set_key(struct hci_conn *conn, u8 key_type, u8 pin_len) +{ + if (key_type == HCI_LK_CHANGED_COMBINATION) + return; + + conn->pin_length = pin_len; + conn->key_type = key_type; + + switch (key_type) { + case HCI_LK_LOCAL_UNIT: + case HCI_LK_REMOTE_UNIT: + case HCI_LK_DEBUG_COMBINATION: + return; + case HCI_LK_COMBINATION: + if (pin_len == 16) + conn->pending_sec_level = BT_SECURITY_HIGH; + else + conn->pending_sec_level = BT_SECURITY_MEDIUM; + break; + case HCI_LK_UNAUTH_COMBINATION_P192: + case HCI_LK_UNAUTH_COMBINATION_P256: + conn->pending_sec_level = BT_SECURITY_MEDIUM; + break; + case HCI_LK_AUTH_COMBINATION_P192: + conn->pending_sec_level = BT_SECURITY_HIGH; + break; + case HCI_LK_AUTH_COMBINATION_P256: + conn->pending_sec_level = BT_SECURITY_FIPS; + break; + } +} + static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_ev_link_key_req *ev = (void *) skb->data; @@ -3217,6 +3250,8 @@ static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); if (conn) { + clear_bit(HCI_CONN_NEW_LINK_KEY, &conn->flags); + if ((key->type == HCI_LK_UNAUTH_COMBINATION_P192 || key->type == HCI_LK_UNAUTH_COMBINATION_P256) && conn->auth_type != 0xff && (conn->auth_type & 0x01)) { @@ -3232,8 +3267,7 @@ static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) goto not_found; } - conn->key_type = key->type; - conn->pin_length = key->pin_len; + conn_set_key(conn, key->type, key->pin_len); } bacpy(&cp.bdaddr, &ev->bdaddr); @@ -3263,16 +3297,15 @@ static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_lock(hdev); conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); - if (conn) { - hci_conn_hold(conn); - conn->disc_timeout = HCI_DISCONN_TIMEOUT; - pin_len = conn->pin_length; + if (!conn) + goto unlock; - if (ev->key_type != HCI_LK_CHANGED_COMBINATION) - conn->key_type = ev->key_type; + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + hci_conn_drop(conn); - hci_conn_drop(conn); - } + set_bit(HCI_CONN_NEW_LINK_KEY, &conn->flags); + conn_set_key(conn, ev->key_type, conn->pin_length); if (!test_bit(HCI_MGMT, &hdev->dev_flags)) goto unlock; @@ -3282,6 +3315,12 @@ static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) if (!key) goto unlock; + /* Update connection information since adding the key will have + * fixed up the type in the case of changed combination keys. + */ + if (ev->key_type == HCI_LK_CHANGED_COMBINATION) + conn_set_key(conn, key->type, key->pin_len); + mgmt_new_link_key(hdev, key, persistent); /* Keep debug keys around only if the HCI_KEEP_DEBUG_KEYS flag @@ -3291,15 +3330,16 @@ static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) */ if (key->type == HCI_LK_DEBUG_COMBINATION && !test_bit(HCI_KEEP_DEBUG_KEYS, &hdev->dev_flags)) { - list_del(&key->list); - kfree(key); - } else if (conn) { - if (persistent) - clear_bit(HCI_CONN_FLUSH_KEY, &conn->flags); - else - set_bit(HCI_CONN_FLUSH_KEY, &conn->flags); + list_del_rcu(&key->list); + kfree_rcu(key, rcu); + goto unlock; } + if (persistent) + clear_bit(HCI_CONN_FLUSH_KEY, &conn->flags); + else + set_bit(HCI_CONN_FLUSH_KEY, &conn->flags); + unlock: hci_dev_unlock(hdev); } @@ -3734,7 +3774,7 @@ static void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb) cp.authentication = conn->auth_type; - if (hci_find_remote_oob_data(hdev, &conn->dst) && + if (hci_find_remote_oob_data(hdev, &conn->dst, BDADDR_BREDR) && (conn->out || test_bit(HCI_CONN_REMOTE_OOB, &conn->flags))) cp.oob_data = 0x01; else @@ -3989,9 +4029,9 @@ static void hci_remote_oob_data_request_evt(struct hci_dev *hdev, if (!test_bit(HCI_MGMT, &hdev->dev_flags)) goto unlock; - data = hci_find_remote_oob_data(hdev, &ev->bdaddr); + data = hci_find_remote_oob_data(hdev, &ev->bdaddr, BDADDR_BREDR); if (data) { - if (test_bit(HCI_SC_ENABLED, &hdev->dev_flags)) { + if (bredr_sc_enabled(hdev)) { struct hci_cp_remote_oob_ext_data_reply cp; bacpy(&cp.bdaddr, &ev->bdaddr); @@ -4386,7 +4426,8 @@ static struct hci_conn *check_pending_le_conn(struct hci_dev *hdev, } static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr, - u8 bdaddr_type, s8 rssi, u8 *data, u8 len) + u8 bdaddr_type, bdaddr_t *direct_addr, + u8 direct_addr_type, s8 rssi, u8 *data, u8 len) { struct discovery_state *d = &hdev->discovery; struct smp_irk *irk; @@ -4394,6 +4435,32 @@ static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr, bool match; u32 flags; + /* If the direct address is present, then this report is from + * a LE Direct Advertising Report event. In that case it is + * important to see if the address is matching the local + * controller address. + */ + if (direct_addr) { + /* Only resolvable random addresses are valid for these + * kind of reports and others can be ignored. + */ + if (!hci_bdaddr_is_rpa(direct_addr, direct_addr_type)) + return; + + /* If the controller is not using resolvable random + * addresses, then this report can be ignored. + */ + if (!test_bit(HCI_PRIVACY, &hdev->dev_flags)) + return; + + /* If the local IRK of the controller does not match + * with the resolvable random address provided, then + * this report can be ignored. + */ + if (!smp_irk_matches(hdev, hdev->irk, direct_addr)) + return; + } + /* Check if we need to convert to identity address */ irk = hci_get_irk(hdev, bdaddr, bdaddr_type); if (irk) { @@ -4530,7 +4597,8 @@ static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) rssi = ev->data[ev->length]; process_adv_report(hdev, ev->evt_type, &ev->bdaddr, - ev->bdaddr_type, rssi, ev->data, ev->length); + ev->bdaddr_type, NULL, 0, rssi, + ev->data, ev->length); ptr += sizeof(*ev) + ev->length + 1; } @@ -4554,10 +4622,20 @@ static void hci_le_ltk_request_evt(struct hci_dev *hdev, struct sk_buff *skb) if (conn == NULL) goto not_found; - ltk = hci_find_ltk(hdev, ev->ediv, ev->rand, conn->role); - if (ltk == NULL) + ltk = hci_find_ltk(hdev, &conn->dst, conn->dst_type, conn->role); + if (!ltk) goto not_found; + if (smp_ltk_is_sc(ltk)) { + /* With SC both EDiv and Rand are set to zero */ + if (ev->ediv || ev->rand) + goto not_found; + } else { + /* For non-SC keys check that EDiv and Rand match */ + if (ev->ediv != ltk->ediv || ev->rand != ltk->rand) + goto not_found; + } + memcpy(cp.ltk, ltk->val, sizeof(ltk->val)); cp.handle = cpu_to_le16(conn->handle); @@ -4661,6 +4739,27 @@ static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, hci_send_cmd(hdev, HCI_OP_LE_CONN_PARAM_REQ_REPLY, sizeof(cp), &cp); } +static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + u8 num_reports = skb->data[0]; + void *ptr = &skb->data[1]; + + hci_dev_lock(hdev); + + while (num_reports--) { + struct hci_ev_le_direct_adv_info *ev = ptr; + + process_adv_report(hdev, ev->evt_type, &ev->bdaddr, + ev->bdaddr_type, &ev->direct_addr, + ev->direct_addr_type, ev->rssi, NULL, 0); + + ptr += sizeof(*ev); + } + + hci_dev_unlock(hdev); +} + static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_ev_le_meta *le_ev = (void *) skb->data; @@ -4688,6 +4787,10 @@ static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_le_remote_conn_param_req_evt(hdev, skb); break; + case HCI_EV_LE_DIRECT_ADV_REPORT: + hci_le_direct_adv_report_evt(hdev, skb); + break; + default: break; } diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 8e1273173020..a8da7ea9c2c0 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -46,7 +46,6 @@ bool disable_ertm; static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD; -static u8 l2cap_fixed_chan[8] = { L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS, }; static LIST_HEAD(chan_list); static DEFINE_RWLOCK(chan_list_lock); @@ -840,7 +839,10 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, if (!skb) return; - if (lmp_no_flush_capable(conn->hcon->hdev)) + /* Use NO_FLUSH if supported or we have an LE link (which does + * not support auto-flushing packets) */ + if (lmp_no_flush_capable(conn->hcon->hdev) || + conn->hcon->type == LE_LINK) flags = ACL_START_NO_FLUSH; else flags = ACL_START; @@ -874,8 +876,13 @@ static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) return; } - if (!test_bit(FLAG_FLUSHABLE, &chan->flags) && - lmp_no_flush_capable(hcon->hdev)) + /* Use NO_FLUSH for LE links (where this is the only option) or + * if the BR/EDR link supports it and flushing has not been + * explicitly requested (through FLAG_FLUSHABLE). + */ + if (hcon->type == LE_LINK || + (!test_bit(FLAG_FLUSHABLE, &chan->flags) && + lmp_no_flush_capable(hcon->hdev))) flags = ACL_START_NO_FLUSH; else flags = ACL_START; @@ -1112,10 +1119,10 @@ static bool __amp_capable(struct l2cap_chan *chan) struct hci_dev *hdev; bool amp_available = false; - if (!conn->hs_enabled) + if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) return false; - if (!(conn->fixed_chan_mask & L2CAP_FC_A2MP)) + if (!(conn->remote_fixed_chan & L2CAP_FC_A2MP)) return false; read_lock(&hci_dev_list_lock); @@ -3088,12 +3095,14 @@ static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) static inline bool __l2cap_ews_supported(struct l2cap_conn *conn) { - return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_WINDOW; + return ((conn->local_fixed_chan & L2CAP_FC_A2MP) && + (conn->feat_mask & L2CAP_FEAT_EXT_WINDOW)); } static inline bool __l2cap_efs_supported(struct l2cap_conn *conn) { - return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW; + return ((conn->local_fixed_chan & L2CAP_FC_A2MP) && + (conn->feat_mask & L2CAP_FEAT_EXT_FLOW)); } static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan, @@ -3322,7 +3331,7 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) break; case L2CAP_CONF_EWS: - if (!chan->conn->hs_enabled) + if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP)) return -ECONNREFUSED; set_bit(FLAG_EXT_CTRL, &chan->flags); @@ -4326,7 +4335,7 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, if (!disable_ertm) feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING | L2CAP_FEAT_FCS; - if (conn->hs_enabled) + if (conn->local_fixed_chan & L2CAP_FC_A2MP) feat_mask |= L2CAP_FEAT_EXT_FLOW | L2CAP_FEAT_EXT_WINDOW; @@ -4337,14 +4346,10 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, u8 buf[12]; struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; - if (conn->hs_enabled) - l2cap_fixed_chan[0] |= L2CAP_FC_A2MP; - else - l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP; - rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); - memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan)); + rsp->data[0] = conn->local_fixed_chan; + memset(rsp->data + 1, 0, 7); l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), buf); } else { @@ -4410,7 +4415,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, break; case L2CAP_IT_FIXED_CHAN: - conn->fixed_chan_mask = rsp->data[0]; + conn->remote_fixed_chan = rsp->data[0]; conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; conn->info_ident = 0; @@ -4434,7 +4439,7 @@ static int l2cap_create_channel_req(struct l2cap_conn *conn, if (cmd_len != sizeof(*req)) return -EPROTO; - if (!conn->hs_enabled) + if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) return -EINVAL; psm = le16_to_cpu(req->psm); @@ -4864,7 +4869,7 @@ static inline int l2cap_move_channel_req(struct l2cap_conn *conn, BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id); - if (!conn->hs_enabled) + if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) return -EINVAL; chan = l2cap_get_chan_by_dcid(conn, icid); @@ -6956,9 +6961,15 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon) conn->feat_mask = 0; - if (hcon->type == ACL_LINK) - conn->hs_enabled = test_bit(HCI_HS_ENABLED, - &hcon->hdev->dev_flags); + conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS; + + if (hcon->type == ACL_LINK && + test_bit(HCI_HS_ENABLED, &hcon->hdev->dev_flags)) + conn->local_fixed_chan |= L2CAP_FC_A2MP; + + if (bredr_sc_enabled(hcon->hdev) && + test_bit(HCI_LE_ENABLED, &hcon->hdev->dev_flags)) + conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR; mutex_init(&conn->ident_lock); mutex_init(&conn->chan_lock); diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index f3e4a16fb157..7384f1161336 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -35,7 +35,7 @@ #include "smp.h" #define MGMT_VERSION 1 -#define MGMT_REVISION 7 +#define MGMT_REVISION 8 static const u16 mgmt_commands[] = { MGMT_OP_READ_INDEX_LIST, @@ -93,6 +93,7 @@ static const u16 mgmt_commands[] = { MGMT_OP_READ_CONFIG_INFO, MGMT_OP_SET_EXTERNAL_CONFIG, MGMT_OP_SET_PUBLIC_ADDRESS, + MGMT_OP_START_SERVICE_DISCOVERY, }; static const u16 mgmt_events[] = { @@ -134,8 +135,10 @@ struct pending_cmd { u16 opcode; int index; void *param; + size_t param_len; struct sock *sk; void *user_data; + void (*cmd_complete)(struct pending_cmd *cmd, u8 status); }; /* HCI to MGMT error code conversion table */ @@ -574,6 +577,7 @@ static u32 get_supported_settings(struct hci_dev *hdev) if (lmp_le_capable(hdev)) { settings |= MGMT_SETTING_LE; settings |= MGMT_SETTING_ADVERTISING; + settings |= MGMT_SETTING_SECURE_CONN; settings |= MGMT_SETTING_PRIVACY; } @@ -1202,14 +1206,13 @@ static struct pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, cmd->opcode = opcode; cmd->index = hdev->id; - cmd->param = kmalloc(len, GFP_KERNEL); + cmd->param = kmemdup(data, len, GFP_KERNEL); if (!cmd->param) { kfree(cmd); return NULL; } - if (data) - memcpy(cmd->param, data, len); + cmd->param_len = len; cmd->sk = sk; sock_hold(sk); @@ -1469,6 +1472,32 @@ static void cmd_status_rsp(struct pending_cmd *cmd, void *data) mgmt_pending_remove(cmd); } +static void cmd_complete_rsp(struct pending_cmd *cmd, void *data) +{ + if (cmd->cmd_complete) { + u8 *status = data; + + cmd->cmd_complete(cmd, *status); + mgmt_pending_remove(cmd); + + return; + } + + cmd_status_rsp(cmd, data); +} + +static void generic_cmd_complete(struct pending_cmd *cmd, u8 status) +{ + cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, cmd->param, + cmd->param_len); +} + +static void addr_cmd_complete(struct pending_cmd *cmd, u8 status) +{ + cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, cmd->param, + sizeof(struct mgmt_addr_info)); +} + static u8 mgmt_bredr_support(struct hci_dev *hdev) { if (!lmp_bredr_capable(hdev)) @@ -2792,6 +2821,8 @@ static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data, goto unlock; } + cmd->cmd_complete = addr_cmd_complete; + dc.handle = cpu_to_le16(conn->handle); dc.reason = 0x13; /* Remote User Terminated Connection */ err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc); @@ -2855,6 +2886,8 @@ static int disconnect(struct sock *sk, struct hci_dev *hdev, void *data, goto failed; } + cmd->cmd_complete = generic_cmd_complete; + err = hci_disconnect(conn, HCI_ERROR_REMOTE_USER_TERM); if (err < 0) mgmt_pending_remove(cmd); @@ -3007,6 +3040,8 @@ static int pin_code_reply(struct sock *sk, struct hci_dev *hdev, void *data, goto failed; } + cmd->cmd_complete = addr_cmd_complete; + bacpy(&reply.bdaddr, &cp->addr.bdaddr); reply.pin_len = cp->pin_len; memcpy(reply.pin_code, cp->pin_code, sizeof(reply.pin_code)); @@ -3096,7 +3131,7 @@ void mgmt_smp_complete(struct hci_conn *conn, bool complete) cmd = find_pairing(conn); if (cmd) - pairing_complete(cmd, status); + cmd->cmd_complete(cmd, status); } static void pairing_complete_cb(struct hci_conn *conn, u8 status) @@ -3109,7 +3144,7 @@ static void pairing_complete_cb(struct hci_conn *conn, u8 status) if (!cmd) BT_DBG("Unable to find a pending command"); else - pairing_complete(cmd, mgmt_status(status)); + cmd->cmd_complete(cmd, mgmt_status(status)); } static void le_pairing_complete_cb(struct hci_conn *conn, u8 status) @@ -3125,7 +3160,7 @@ static void le_pairing_complete_cb(struct hci_conn *conn, u8 status) if (!cmd) BT_DBG("Unable to find a pending command"); else - pairing_complete(cmd, mgmt_status(status)); + cmd->cmd_complete(cmd, mgmt_status(status)); } static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data, @@ -3222,6 +3257,8 @@ static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data, goto unlock; } + cmd->cmd_complete = pairing_complete; + /* For LE, just connecting isn't a proof that the pairing finished */ if (cp->addr.type == BDADDR_BREDR) { conn->connect_cfm_cb = pairing_complete_cb; @@ -3338,6 +3375,8 @@ static int user_pairing_resp(struct sock *sk, struct hci_dev *hdev, goto done; } + cmd->cmd_complete = addr_cmd_complete; + /* Continue with pairing via HCI */ if (hci_op == HCI_OP_USER_PASSKEY_REPLY) { struct hci_cp_user_passkey_reply cp; @@ -3562,7 +3601,7 @@ static int read_local_oob_data(struct sock *sk, struct hci_dev *hdev, goto unlock; } - if (test_bit(HCI_SC_ENABLED, &hdev->dev_flags)) + if (bredr_sc_enabled(hdev)) err = hci_send_cmd(hdev, HCI_OP_READ_LOCAL_OOB_EXT_DATA, 0, NULL); else @@ -3598,7 +3637,8 @@ static int add_remote_oob_data(struct sock *sk, struct hci_dev *hdev, } err = hci_add_remote_oob_data(hdev, &cp->addr.bdaddr, - cp->hash, cp->rand); + cp->addr.type, cp->hash, + cp->rand, NULL, NULL); if (err < 0) status = MGMT_STATUS_FAILED; else @@ -3608,6 +3648,7 @@ static int add_remote_oob_data(struct sock *sk, struct hci_dev *hdev, status, &cp->addr, sizeof(cp->addr)); } else if (len == MGMT_ADD_REMOTE_OOB_EXT_DATA_SIZE) { struct mgmt_cp_add_remote_oob_ext_data *cp = data; + u8 *rand192, *hash192; u8 status; if (cp->addr.type != BDADDR_BREDR) { @@ -3618,9 +3659,17 @@ static int add_remote_oob_data(struct sock *sk, struct hci_dev *hdev, goto unlock; } - err = hci_add_remote_oob_ext_data(hdev, &cp->addr.bdaddr, - cp->hash192, cp->rand192, - cp->hash256, cp->rand256); + if (bdaddr_type_is_le(cp->addr.type)) { + rand192 = NULL; + hash192 = NULL; + } else { + rand192 = cp->rand192; + hash192 = cp->hash192; + } + + err = hci_add_remote_oob_data(hdev, &cp->addr.bdaddr, + cp->addr.type, hash192, rand192, + cp->hash256, cp->rand256); if (err < 0) status = MGMT_STATUS_FAILED; else @@ -3661,7 +3710,7 @@ static int remove_remote_oob_data(struct sock *sk, struct hci_dev *hdev, goto done; } - err = hci_remove_remote_oob_data(hdev, &cp->addr.bdaddr); + err = hci_remove_remote_oob_data(hdev, &cp->addr.bdaddr, cp->addr.type); if (err < 0) status = MGMT_STATUS_INVALID_PARAMS; else @@ -3675,64 +3724,150 @@ done: return err; } -static int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status) +static bool trigger_discovery(struct hci_request *req, u8 *status) { - struct pending_cmd *cmd; - u8 type; + struct hci_dev *hdev = req->hdev; + struct hci_cp_le_set_scan_param param_cp; + struct hci_cp_le_set_scan_enable enable_cp; + struct hci_cp_inquiry inq_cp; + /* General inquiry access code (GIAC) */ + u8 lap[3] = { 0x33, 0x8b, 0x9e }; + u8 own_addr_type; int err; - hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + switch (hdev->discovery.type) { + case DISCOV_TYPE_BREDR: + *status = mgmt_bredr_support(hdev); + if (*status) + return false; - cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev); - if (!cmd) - return -ENOENT; + if (test_bit(HCI_INQUIRY, &hdev->flags)) { + *status = MGMT_STATUS_BUSY; + return false; + } - type = hdev->discovery.type; + hci_inquiry_cache_flush(hdev); - err = cmd_complete(cmd->sk, hdev->id, cmd->opcode, mgmt_status(status), - &type, sizeof(type)); - mgmt_pending_remove(cmd); + memset(&inq_cp, 0, sizeof(inq_cp)); + memcpy(&inq_cp.lap, lap, sizeof(inq_cp.lap)); + inq_cp.length = DISCOV_BREDR_INQUIRY_LEN; + hci_req_add(req, HCI_OP_INQUIRY, sizeof(inq_cp), &inq_cp); + break; - return err; + case DISCOV_TYPE_LE: + case DISCOV_TYPE_INTERLEAVED: + *status = mgmt_le_support(hdev); + if (*status) + return false; + + if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED && + !test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) { + *status = MGMT_STATUS_NOT_SUPPORTED; + return false; + } + + if (test_bit(HCI_LE_ADV, &hdev->dev_flags)) { + /* Don't let discovery abort an outgoing + * connection attempt that's using directed + * advertising. + */ + if (hci_conn_hash_lookup_state(hdev, LE_LINK, + BT_CONNECT)) { + *status = MGMT_STATUS_REJECTED; + return false; + } + + disable_advertising(req); + } + + /* If controller is scanning, it means the background scanning + * is running. Thus, we should temporarily stop it in order to + * set the discovery scanning parameters. + */ + if (test_bit(HCI_LE_SCAN, &hdev->dev_flags)) + hci_req_add_le_scan_disable(req); + + memset(¶m_cp, 0, sizeof(param_cp)); + + /* All active scans will be done with either a resolvable + * private address (when privacy feature has been enabled) + * or unresolvable private address. + */ + err = hci_update_random_address(req, true, &own_addr_type); + if (err < 0) { + *status = MGMT_STATUS_FAILED; + return false; + } + + param_cp.type = LE_SCAN_ACTIVE; + param_cp.interval = cpu_to_le16(DISCOV_LE_SCAN_INT); + param_cp.window = cpu_to_le16(DISCOV_LE_SCAN_WIN); + param_cp.own_address_type = own_addr_type; + hci_req_add(req, HCI_OP_LE_SET_SCAN_PARAM, sizeof(param_cp), + ¶m_cp); + + memset(&enable_cp, 0, sizeof(enable_cp)); + enable_cp.enable = LE_SCAN_ENABLE; + enable_cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE; + hci_req_add(req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(enable_cp), + &enable_cp); + break; + + default: + *status = MGMT_STATUS_INVALID_PARAMS; + return false; + } + + return true; } static void start_discovery_complete(struct hci_dev *hdev, u8 status) { - unsigned long timeout = 0; + struct pending_cmd *cmd; + unsigned long timeout; BT_DBG("status %d", status); + hci_dev_lock(hdev); + + cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev); + if (!cmd) + cmd = mgmt_pending_find(MGMT_OP_START_SERVICE_DISCOVERY, hdev); + + if (cmd) { + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + } + if (status) { - hci_dev_lock(hdev); - mgmt_start_discovery_failed(hdev, status); - hci_dev_unlock(hdev); - return; + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + goto unlock; } - hci_dev_lock(hdev); hci_discovery_set_state(hdev, DISCOVERY_FINDING); - hci_dev_unlock(hdev); switch (hdev->discovery.type) { case DISCOV_TYPE_LE: timeout = msecs_to_jiffies(DISCOV_LE_TIMEOUT); break; - case DISCOV_TYPE_INTERLEAVED: timeout = msecs_to_jiffies(hdev->discov_interleaved_timeout); break; - case DISCOV_TYPE_BREDR: + timeout = 0; break; - default: BT_ERR("Invalid discovery type %d", hdev->discovery.type); + timeout = 0; + break; } - if (!timeout) - return; + if (timeout) + queue_delayed_work(hdev->workqueue, + &hdev->le_scan_disable, timeout); - queue_delayed_work(hdev->workqueue, &hdev->le_scan_disable, timeout); +unlock: + hci_dev_unlock(hdev); } static int start_discovery(struct sock *sk, struct hci_dev *hdev, @@ -3740,13 +3875,8 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev, { struct mgmt_cp_start_discovery *cp = data; struct pending_cmd *cmd; - struct hci_cp_le_set_scan_param param_cp; - struct hci_cp_le_set_scan_enable enable_cp; - struct hci_cp_inquiry inq_cp; struct hci_request req; - /* General inquiry access code (GIAC) */ - u8 lap[3] = { 0x33, 0x8b, 0x9e }; - u8 status, own_addr_type; + u8 status; int err; BT_DBG("%s", hdev->name); @@ -3760,184 +3890,182 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev, goto failed; } - if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) { + if (hdev->discovery.state != DISCOVERY_STOPPED || + test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) { err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, MGMT_STATUS_BUSY, &cp->type, sizeof(cp->type)); goto failed; } - if (hdev->discovery.state != DISCOVERY_STOPPED) { - err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, - MGMT_STATUS_BUSY, &cp->type, - sizeof(cp->type)); - goto failed; - } - - cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, NULL, 0); + cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, data, len); if (!cmd) { err = -ENOMEM; goto failed; } + cmd->cmd_complete = generic_cmd_complete; + + /* Clear the discovery filter first to free any previously + * allocated memory for the UUID list. + */ + hci_discovery_filter_clear(hdev); + hdev->discovery.type = cp->type; + hdev->discovery.report_invalid_rssi = false; hci_req_init(&req, hdev); - switch (hdev->discovery.type) { - case DISCOV_TYPE_BREDR: - status = mgmt_bredr_support(hdev); - if (status) { - err = cmd_complete(sk, hdev->id, - MGMT_OP_START_DISCOVERY, status, - &cp->type, sizeof(cp->type)); - mgmt_pending_remove(cmd); - goto failed; - } + if (!trigger_discovery(&req, &status)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, + status, &cp->type, sizeof(cp->type)); + mgmt_pending_remove(cmd); + goto failed; + } - if (test_bit(HCI_INQUIRY, &hdev->flags)) { - err = cmd_complete(sk, hdev->id, - MGMT_OP_START_DISCOVERY, - MGMT_STATUS_BUSY, &cp->type, - sizeof(cp->type)); - mgmt_pending_remove(cmd); - goto failed; - } + err = hci_req_run(&req, start_discovery_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } - hci_inquiry_cache_flush(hdev); + hci_discovery_set_state(hdev, DISCOVERY_STARTING); - memset(&inq_cp, 0, sizeof(inq_cp)); - memcpy(&inq_cp.lap, lap, sizeof(inq_cp.lap)); - inq_cp.length = DISCOV_BREDR_INQUIRY_LEN; - hci_req_add(&req, HCI_OP_INQUIRY, sizeof(inq_cp), &inq_cp); - break; +failed: + hci_dev_unlock(hdev); + return err; +} - case DISCOV_TYPE_LE: - case DISCOV_TYPE_INTERLEAVED: - status = mgmt_le_support(hdev); - if (status) { - err = cmd_complete(sk, hdev->id, - MGMT_OP_START_DISCOVERY, status, - &cp->type, sizeof(cp->type)); - mgmt_pending_remove(cmd); - goto failed; - } +static void service_discovery_cmd_complete(struct pending_cmd *cmd, u8 status) +{ + cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, cmd->param, 1); +} - if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED && - !test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) { - err = cmd_complete(sk, hdev->id, - MGMT_OP_START_DISCOVERY, - MGMT_STATUS_NOT_SUPPORTED, - &cp->type, sizeof(cp->type)); - mgmt_pending_remove(cmd); - goto failed; - } +static int start_service_discovery(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_start_service_discovery *cp = data; + struct pending_cmd *cmd; + struct hci_request req; + const u16 max_uuid_count = ((U16_MAX - sizeof(*cp)) / 16); + u16 uuid_count, expected_len; + u8 status; + int err; - if (test_bit(HCI_LE_ADV, &hdev->dev_flags)) { - /* Don't let discovery abort an outgoing - * connection attempt that's using directed - * advertising. - */ - if (hci_conn_hash_lookup_state(hdev, LE_LINK, - BT_CONNECT)) { - err = cmd_complete(sk, hdev->id, - MGMT_OP_START_DISCOVERY, - MGMT_STATUS_REJECTED, - &cp->type, - sizeof(cp->type)); - mgmt_pending_remove(cmd); - goto failed; - } + BT_DBG("%s", hdev->name); - disable_advertising(&req); - } + hci_dev_lock(hdev); - /* If controller is scanning, it means the background scanning - * is running. Thus, we should temporarily stop it in order to - * set the discovery scanning parameters. - */ - if (test_bit(HCI_LE_SCAN, &hdev->dev_flags)) - hci_req_add_le_scan_disable(&req); + if (!hdev_is_powered(hdev)) { + err = cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_NOT_POWERED, + &cp->type, sizeof(cp->type)); + goto failed; + } - memset(¶m_cp, 0, sizeof(param_cp)); + if (hdev->discovery.state != DISCOVERY_STOPPED || + test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) { + err = cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_BUSY, &cp->type, + sizeof(cp->type)); + goto failed; + } - /* All active scans will be done with either a resolvable - * private address (when privacy feature has been enabled) - * or unresolvable private address. - */ - err = hci_update_random_address(&req, true, &own_addr_type); - if (err < 0) { + uuid_count = __le16_to_cpu(cp->uuid_count); + if (uuid_count > max_uuid_count) { + BT_ERR("service_discovery: too big uuid_count value %u", + uuid_count); + err = cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_INVALID_PARAMS, &cp->type, + sizeof(cp->type)); + goto failed; + } + + expected_len = sizeof(*cp) + uuid_count * 16; + if (expected_len != len) { + BT_ERR("service_discovery: expected %u bytes, got %u bytes", + expected_len, len); + err = cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_INVALID_PARAMS, &cp->type, + sizeof(cp->type)); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_START_SERVICE_DISCOVERY, + hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + cmd->cmd_complete = service_discovery_cmd_complete; + + /* Clear the discovery filter first to free any previously + * allocated memory for the UUID list. + */ + hci_discovery_filter_clear(hdev); + + hdev->discovery.type = cp->type; + hdev->discovery.rssi = cp->rssi; + hdev->discovery.uuid_count = uuid_count; + + if (uuid_count > 0) { + hdev->discovery.uuids = kmemdup(cp->uuids, uuid_count * 16, + GFP_KERNEL); + if (!hdev->discovery.uuids) { err = cmd_complete(sk, hdev->id, - MGMT_OP_START_DISCOVERY, + MGMT_OP_START_SERVICE_DISCOVERY, MGMT_STATUS_FAILED, &cp->type, sizeof(cp->type)); mgmt_pending_remove(cmd); goto failed; } + } - param_cp.type = LE_SCAN_ACTIVE; - param_cp.interval = cpu_to_le16(DISCOV_LE_SCAN_INT); - param_cp.window = cpu_to_le16(DISCOV_LE_SCAN_WIN); - param_cp.own_address_type = own_addr_type; - hci_req_add(&req, HCI_OP_LE_SET_SCAN_PARAM, sizeof(param_cp), - ¶m_cp); - - memset(&enable_cp, 0, sizeof(enable_cp)); - enable_cp.enable = LE_SCAN_ENABLE; - enable_cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE; - hci_req_add(&req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(enable_cp), - &enable_cp); - break; + hci_req_init(&req, hdev); - default: - err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, - MGMT_STATUS_INVALID_PARAMS, - &cp->type, sizeof(cp->type)); + if (!trigger_discovery(&req, &status)) { + err = cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + status, &cp->type, sizeof(cp->type)); mgmt_pending_remove(cmd); goto failed; } err = hci_req_run(&req, start_discovery_complete); - if (err < 0) + if (err < 0) { mgmt_pending_remove(cmd); - else - hci_discovery_set_state(hdev, DISCOVERY_STARTING); + goto failed; + } + + hci_discovery_set_state(hdev, DISCOVERY_STARTING); failed: hci_dev_unlock(hdev); return err; } -static int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status) +static void stop_discovery_complete(struct hci_dev *hdev, u8 status) { struct pending_cmd *cmd; - int err; - cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev); - if (!cmd) - return -ENOENT; - - err = cmd_complete(cmd->sk, hdev->id, cmd->opcode, mgmt_status(status), - &hdev->discovery.type, sizeof(hdev->discovery.type)); - mgmt_pending_remove(cmd); - - return err; -} - -static void stop_discovery_complete(struct hci_dev *hdev, u8 status) -{ BT_DBG("status %d", status); hci_dev_lock(hdev); - if (status) { - mgmt_stop_discovery_failed(hdev, status); - goto unlock; + cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev); + if (cmd) { + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); } - hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + if (!status) + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); -unlock: hci_dev_unlock(hdev); } @@ -3967,12 +4095,14 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data, goto unlock; } - cmd = mgmt_pending_add(sk, MGMT_OP_STOP_DISCOVERY, hdev, NULL, 0); + cmd = mgmt_pending_add(sk, MGMT_OP_STOP_DISCOVERY, hdev, data, len); if (!cmd) { err = -ENOMEM; goto unlock; } + cmd->cmd_complete = generic_cmd_complete; + hci_req_init(&req, hdev); hci_stop_discovery(&req); @@ -4572,18 +4702,13 @@ static int set_secure_conn(struct sock *sk, struct hci_dev *hdev, { struct mgmt_mode *cp = data; struct pending_cmd *cmd; - u8 val, status; + u8 val; int err; BT_DBG("request for %s", hdev->name); - status = mgmt_bredr_support(hdev); - if (status) - return cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN, - status); - - if (!lmp_sc_capable(hdev) && - !test_bit(HCI_FORCE_SC, &hdev->dbg_flags)) + if (!test_bit(HCI_LE_ENABLED, &hdev->dev_flags) && + !lmp_sc_capable(hdev) && !test_bit(HCI_FORCE_SC, &hdev->dbg_flags)) return cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN, MGMT_STATUS_NOT_SUPPORTED); @@ -4593,7 +4718,10 @@ static int set_secure_conn(struct sock *sk, struct hci_dev *hdev, hci_dev_lock(hdev); - if (!hdev_is_powered(hdev)) { + if (!hdev_is_powered(hdev) || + (!lmp_sc_capable(hdev) && + !test_bit(HCI_FORCE_SC, &hdev->dbg_flags)) || + !test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) { bool changed; if (cp->val) { @@ -4910,18 +5038,26 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev, else addr_type = ADDR_LE_DEV_RANDOM; - if (key->master) - type = SMP_LTK; - else - type = SMP_LTK_SLAVE; - switch (key->type) { case MGMT_LTK_UNAUTHENTICATED: authenticated = 0x00; + type = key->master ? SMP_LTK : SMP_LTK_SLAVE; break; case MGMT_LTK_AUTHENTICATED: authenticated = 0x01; + type = key->master ? SMP_LTK : SMP_LTK_SLAVE; + break; + case MGMT_LTK_P256_UNAUTH: + authenticated = 0x00; + type = SMP_LTK_P256; break; + case MGMT_LTK_P256_AUTH: + authenticated = 0x01; + type = SMP_LTK_P256; + break; + case MGMT_LTK_P256_DEBUG: + authenticated = 0x00; + type = SMP_LTK_P256_DEBUG; default: continue; } @@ -4939,67 +5075,42 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev, return err; } -struct cmd_conn_lookup { - struct hci_conn *conn; - bool valid_tx_power; - u8 mgmt_status; -}; - -static void get_conn_info_complete(struct pending_cmd *cmd, void *data) +static void conn_info_cmd_complete(struct pending_cmd *cmd, u8 status) { - struct cmd_conn_lookup *match = data; - struct mgmt_cp_get_conn_info *cp; - struct mgmt_rp_get_conn_info rp; struct hci_conn *conn = cmd->user_data; + struct mgmt_rp_get_conn_info rp; - if (conn != match->conn) - return; - - cp = (struct mgmt_cp_get_conn_info *) cmd->param; - - memset(&rp, 0, sizeof(rp)); - bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); - rp.addr.type = cp->addr.type; + memcpy(&rp.addr, cmd->param, sizeof(rp.addr)); - if (!match->mgmt_status) { + if (status == MGMT_STATUS_SUCCESS) { rp.rssi = conn->rssi; - - if (match->valid_tx_power) { - rp.tx_power = conn->tx_power; - rp.max_tx_power = conn->max_tx_power; - } else { - rp.tx_power = HCI_TX_POWER_INVALID; - rp.max_tx_power = HCI_TX_POWER_INVALID; - } + rp.tx_power = conn->tx_power; + rp.max_tx_power = conn->max_tx_power; + } else { + rp.rssi = HCI_RSSI_INVALID; + rp.tx_power = HCI_TX_POWER_INVALID; + rp.max_tx_power = HCI_TX_POWER_INVALID; } - cmd_complete(cmd->sk, cmd->index, MGMT_OP_GET_CONN_INFO, - match->mgmt_status, &rp, sizeof(rp)); + cmd_complete(cmd->sk, cmd->index, MGMT_OP_GET_CONN_INFO, status, + &rp, sizeof(rp)); hci_conn_drop(conn); hci_conn_put(conn); - - mgmt_pending_remove(cmd); } -static void conn_info_refresh_complete(struct hci_dev *hdev, u8 status) +static void conn_info_refresh_complete(struct hci_dev *hdev, u8 hci_status) { struct hci_cp_read_rssi *cp; + struct pending_cmd *cmd; struct hci_conn *conn; - struct cmd_conn_lookup match; u16 handle; + u8 status; - BT_DBG("status 0x%02x", status); + BT_DBG("status 0x%02x", hci_status); hci_dev_lock(hdev); - /* TX power data is valid in case request completed successfully, - * otherwise we assume it's not valid. At the moment we assume that - * either both or none of current and max values are valid to keep code - * simple. - */ - match.valid_tx_power = !status; - /* Commands sent in request are either Read RSSI or Read Transmit Power * Level so we check which one was last sent to retrieve connection * handle. Both commands have handle as first parameter so it's safe to @@ -5012,29 +5123,29 @@ static void conn_info_refresh_complete(struct hci_dev *hdev, u8 status) cp = hci_sent_cmd_data(hdev, HCI_OP_READ_RSSI); if (!cp) { cp = hci_sent_cmd_data(hdev, HCI_OP_READ_TX_POWER); - status = 0; + status = MGMT_STATUS_SUCCESS; + } else { + status = mgmt_status(hci_status); } if (!cp) { - BT_ERR("invalid sent_cmd in response"); + BT_ERR("invalid sent_cmd in conn_info response"); goto unlock; } handle = __le16_to_cpu(cp->handle); conn = hci_conn_hash_lookup_handle(hdev, handle); if (!conn) { - BT_ERR("unknown handle (%d) in response", handle); + BT_ERR("unknown handle (%d) in conn_info response", handle); goto unlock; } - match.conn = conn; - match.mgmt_status = mgmt_status(status); + cmd = mgmt_pending_find_data(MGMT_OP_GET_CONN_INFO, hdev, conn); + if (!cmd) + goto unlock; - /* Cache refresh is complete, now reply for mgmt request for given - * connection only. - */ - mgmt_pending_foreach(MGMT_OP_GET_CONN_INFO, hdev, - get_conn_info_complete, &match); + cmd->cmd_complete(cmd, status); + mgmt_pending_remove(cmd); unlock: hci_dev_unlock(hdev); @@ -5080,6 +5191,12 @@ static int get_conn_info(struct sock *sk, struct hci_dev *hdev, void *data, goto unlock; } + if (mgmt_pending_find_data(MGMT_OP_GET_CONN_INFO, hdev, conn)) { + err = cmd_complete(sk, hdev->id, MGMT_OP_GET_CONN_INFO, + MGMT_STATUS_BUSY, &rp, sizeof(rp)); + goto unlock; + } + /* To avoid client trying to guess when to poll again for information we * calculate conn info age as random value between min/max set in hdev. */ @@ -5135,6 +5252,7 @@ static int get_conn_info(struct sock *sk, struct hci_dev *hdev, void *data, hci_conn_hold(conn); cmd->user_data = hci_conn_get(conn); + cmd->cmd_complete = conn_info_cmd_complete; conn->conn_info_timestamp = jiffies; } else { @@ -5152,10 +5270,40 @@ unlock: return err; } -static void get_clock_info_complete(struct hci_dev *hdev, u8 status) +static void clock_info_cmd_complete(struct pending_cmd *cmd, u8 status) { - struct mgmt_cp_get_clock_info *cp; + struct hci_conn *conn = cmd->user_data; struct mgmt_rp_get_clock_info rp; + struct hci_dev *hdev; + + memset(&rp, 0, sizeof(rp)); + memcpy(&rp.addr, &cmd->param, sizeof(rp.addr)); + + if (status) + goto complete; + + hdev = hci_dev_get(cmd->index); + if (hdev) { + rp.local_clock = cpu_to_le32(hdev->clock); + hci_dev_put(hdev); + } + + if (conn) { + rp.piconet_clock = cpu_to_le32(conn->clock); + rp.accuracy = cpu_to_le16(conn->clock_accuracy); + } + +complete: + cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, &rp, sizeof(rp)); + + if (conn) { + hci_conn_drop(conn); + hci_conn_put(conn); + } +} + +static void get_clock_info_complete(struct hci_dev *hdev, u8 status) +{ struct hci_cp_read_clock *hci_cp; struct pending_cmd *cmd; struct hci_conn *conn; @@ -5179,29 +5327,8 @@ static void get_clock_info_complete(struct hci_dev *hdev, u8 status) if (!cmd) goto unlock; - cp = cmd->param; - - memset(&rp, 0, sizeof(rp)); - memcpy(&rp.addr, &cp->addr, sizeof(rp.addr)); - - if (status) - goto send_rsp; - - rp.local_clock = cpu_to_le32(hdev->clock); - - if (conn) { - rp.piconet_clock = cpu_to_le32(conn->clock); - rp.accuracy = cpu_to_le16(conn->clock_accuracy); - } - -send_rsp: - cmd_complete(cmd->sk, cmd->index, cmd->opcode, mgmt_status(status), - &rp, sizeof(rp)); + cmd->cmd_complete(cmd, mgmt_status(status)); mgmt_pending_remove(cmd); - if (conn) { - hci_conn_drop(conn); - hci_conn_put(conn); - } unlock: hci_dev_unlock(hdev); @@ -5257,6 +5384,8 @@ static int get_clock_info(struct sock *sk, struct hci_dev *hdev, void *data, goto unlock; } + cmd->cmd_complete = clock_info_cmd_complete; + hci_req_init(&req, hdev); memset(&hci_cp, 0, sizeof(hci_cp)); @@ -5746,6 +5875,7 @@ static const struct mgmt_handler { { read_config_info, false, MGMT_READ_CONFIG_INFO_SIZE }, { set_external_config, false, MGMT_SET_EXTERNAL_CONFIG_SIZE }, { set_public_address, false, MGMT_SET_PUBLIC_ADDRESS_SIZE }, + { start_service_discovery,true, MGMT_START_SERVICE_DISCOVERY_SIZE }, }; int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen) @@ -5882,7 +6012,7 @@ void mgmt_index_removed(struct hci_dev *hdev) if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) return; - mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status); + mgmt_pending_foreach(0, hdev, cmd_complete_rsp, &status); if (test_bit(HCI_UNCONFIGURED, &hdev->dev_flags)) mgmt_event(MGMT_EV_UNCONF_INDEX_REMOVED, hdev, NULL, 0, NULL); @@ -6017,7 +6147,7 @@ int mgmt_powered(struct hci_dev *hdev, u8 powered) } mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, settings_rsp, &match); - mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status_not_powered); + mgmt_pending_foreach(0, hdev, cmd_complete_rsp, &status_not_powered); if (memcmp(hdev->dev_class, zero_cod, sizeof(zero_cod)) != 0) mgmt_event(MGMT_EV_CLASS_OF_DEV_CHANGED, hdev, @@ -6101,8 +6231,19 @@ void mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key, static u8 mgmt_ltk_type(struct smp_ltk *ltk) { - if (ltk->authenticated) - return MGMT_LTK_AUTHENTICATED; + switch (ltk->type) { + case SMP_LTK: + case SMP_LTK_SLAVE: + if (ltk->authenticated) + return MGMT_LTK_AUTHENTICATED; + return MGMT_LTK_UNAUTHENTICATED; + case SMP_LTK_P256: + if (ltk->authenticated) + return MGMT_LTK_P256_AUTH; + return MGMT_LTK_P256_UNAUTH; + case SMP_LTK_P256_DEBUG: + return MGMT_LTK_P256_DEBUG; + } return MGMT_LTK_UNAUTHENTICATED; } @@ -6276,15 +6417,9 @@ void mgmt_device_connected(struct hci_dev *hdev, struct hci_conn *conn, static void disconnect_rsp(struct pending_cmd *cmd, void *data) { - struct mgmt_cp_disconnect *cp = cmd->param; struct sock **sk = data; - struct mgmt_rp_disconnect rp; - bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); - rp.addr.type = cp->addr.type; - - cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT, 0, &rp, - sizeof(rp)); + cmd->cmd_complete(cmd, 0); *sk = cmd->sk; sock_hold(*sk); @@ -6296,16 +6431,10 @@ static void unpair_device_rsp(struct pending_cmd *cmd, void *data) { struct hci_dev *hdev = data; struct mgmt_cp_unpair_device *cp = cmd->param; - struct mgmt_rp_unpair_device rp; - - memset(&rp, 0, sizeof(rp)); - bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); - rp.addr.type = cp->addr.type; device_unpaired(hdev, &cp->addr.bdaddr, cp->addr.type, cmd->sk); - cmd_complete(cmd->sk, cmd->index, cmd->opcode, 0, &rp, sizeof(rp)); - + cmd->cmd_complete(cmd, 0); mgmt_pending_remove(cmd); } @@ -6366,7 +6495,6 @@ void mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, { u8 bdaddr_type = link_to_bdaddr(link_type, addr_type); struct mgmt_cp_disconnect *cp; - struct mgmt_rp_disconnect rp; struct pending_cmd *cmd; mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp, @@ -6384,12 +6512,7 @@ void mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, if (cp->addr.type != bdaddr_type) return; - bacpy(&rp.addr.bdaddr, bdaddr); - rp.addr.type = bdaddr_type; - - cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT, - mgmt_status(status), &rp, sizeof(rp)); - + cmd->cmd_complete(cmd, mgmt_status(status)); mgmt_pending_remove(cmd); } @@ -6428,18 +6551,12 @@ void mgmt_pin_code_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 status) { struct pending_cmd *cmd; - struct mgmt_rp_pin_code_reply rp; cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_REPLY, hdev); if (!cmd) return; - bacpy(&rp.addr.bdaddr, bdaddr); - rp.addr.type = BDADDR_BREDR; - - cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, - mgmt_status(status), &rp, sizeof(rp)); - + cmd->cmd_complete(cmd, mgmt_status(status)); mgmt_pending_remove(cmd); } @@ -6447,18 +6564,12 @@ void mgmt_pin_code_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 status) { struct pending_cmd *cmd; - struct mgmt_rp_pin_code_reply rp; cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_NEG_REPLY, hdev); if (!cmd) return; - bacpy(&rp.addr.bdaddr, bdaddr); - rp.addr.type = BDADDR_BREDR; - - cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_NEG_REPLY, - mgmt_status(status), &rp, sizeof(rp)); - + cmd->cmd_complete(cmd, mgmt_status(status)); mgmt_pending_remove(cmd); } @@ -6498,21 +6609,15 @@ static int user_pairing_resp_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 opcode) { struct pending_cmd *cmd; - struct mgmt_rp_user_confirm_reply rp; - int err; cmd = mgmt_pending_find(opcode, hdev); if (!cmd) return -ENOENT; - bacpy(&rp.addr.bdaddr, bdaddr); - rp.addr.type = link_to_bdaddr(link_type, addr_type); - err = cmd_complete(cmd->sk, hdev->id, opcode, mgmt_status(status), - &rp, sizeof(rp)); - + cmd->cmd_complete(cmd, mgmt_status(status)); mgmt_pending_remove(cmd); - return err; + return 0; } int mgmt_user_confirm_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, @@ -6784,8 +6889,7 @@ void mgmt_read_local_oob_data_complete(struct hci_dev *hdev, u8 *hash192, cmd_status(cmd->sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, mgmt_status(status)); } else { - if (test_bit(HCI_SC_ENABLED, &hdev->dev_flags) && - hash256 && rand256) { + if (bredr_sc_enabled(hdev) && hash256 && rand256) { struct mgmt_rp_read_local_oob_ext_data rp; memcpy(rp.hash192, hash192, sizeof(rp.hash192)); @@ -6812,6 +6916,73 @@ void mgmt_read_local_oob_data_complete(struct hci_dev *hdev, u8 *hash192, mgmt_pending_remove(cmd); } +static inline bool has_uuid(u8 *uuid, u16 uuid_count, u8 (*uuids)[16]) +{ + int i; + + for (i = 0; i < uuid_count; i++) { + if (!memcmp(uuid, uuids[i], 16)) + return true; + } + + return false; +} + +static bool eir_has_uuids(u8 *eir, u16 eir_len, u16 uuid_count, u8 (*uuids)[16]) +{ + u16 parsed = 0; + + while (parsed < eir_len) { + u8 field_len = eir[0]; + u8 uuid[16]; + int i; + + if (field_len == 0) + break; + + if (eir_len - parsed < field_len + 1) + break; + + switch (eir[1]) { + case EIR_UUID16_ALL: + case EIR_UUID16_SOME: + for (i = 0; i + 3 <= field_len; i += 2) { + memcpy(uuid, bluetooth_base_uuid, 16); + uuid[13] = eir[i + 3]; + uuid[12] = eir[i + 2]; + if (has_uuid(uuid, uuid_count, uuids)) + return true; + } + break; + case EIR_UUID32_ALL: + case EIR_UUID32_SOME: + for (i = 0; i + 5 <= field_len; i += 4) { + memcpy(uuid, bluetooth_base_uuid, 16); + uuid[15] = eir[i + 5]; + uuid[14] = eir[i + 4]; + uuid[13] = eir[i + 3]; + uuid[12] = eir[i + 2]; + if (has_uuid(uuid, uuid_count, uuids)) + return true; + } + break; + case EIR_UUID128_ALL: + case EIR_UUID128_SOME: + for (i = 0; i + 17 <= field_len; i += 16) { + memcpy(uuid, eir + i + 2, 16); + if (has_uuid(uuid, uuid_count, uuids)) + return true; + } + break; + } + + parsed += field_len + 1; + eir += field_len + 1; + } + + return false; +} + void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, u8 addr_type, u8 *dev_class, s8 rssi, u32 flags, u8 *eir, u16 eir_len, u8 *scan_rsp, u8 scan_rsp_len) @@ -6819,6 +6990,7 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, char buf[512]; struct mgmt_ev_device_found *ev = (void *) buf; size_t ev_size; + bool match; /* Don't send events for a non-kernel initiated discovery. With * LE one exception is if we have pend_le_reports > 0 in which @@ -6831,6 +7003,18 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, return; } + /* When using service discovery with a RSSI threshold, then check + * if such a RSSI threshold is specified. If a RSSI threshold has + * been specified, then all results with a RSSI smaller than the + * RSSI threshold will be dropped. + * + * For BR/EDR devices (pre 1.2) providing no RSSI during inquiry, + * the results are also dropped. + */ + if (hdev->discovery.rssi != HCI_RSSI_INVALID && + (rssi < hdev->discovery.rssi || rssi == HCI_RSSI_INVALID)) + return; + /* Make sure that the buffer is big enough. The 5 extra bytes * are for the potential CoD field. */ @@ -6839,20 +7023,75 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, memset(buf, 0, sizeof(buf)); + /* In case of device discovery with BR/EDR devices (pre 1.2), the + * RSSI value was reported as 0 when not available. This behavior + * is kept when using device discovery. This is required for full + * backwards compatibility with the API. + * + * However when using service discovery, the value 127 will be + * returned when the RSSI is not available. + */ + if (rssi == HCI_RSSI_INVALID && !hdev->discovery.report_invalid_rssi) + rssi = 0; + bacpy(&ev->addr.bdaddr, bdaddr); ev->addr.type = link_to_bdaddr(link_type, addr_type); ev->rssi = rssi; ev->flags = cpu_to_le32(flags); - if (eir_len > 0) + if (eir_len > 0) { + /* When using service discovery and a list of UUID is + * provided, results with no matching UUID should be + * dropped. In case there is a match the result is + * kept and checking possible scan response data + * will be skipped. + */ + if (hdev->discovery.uuid_count > 0) { + match = eir_has_uuids(eir, eir_len, + hdev->discovery.uuid_count, + hdev->discovery.uuids); + if (!match) + return; + } + + /* Copy EIR or advertising data into event */ memcpy(ev->eir, eir, eir_len); + } else { + /* When using service discovery and a list of UUID is + * provided, results with empty EIR or advertising data + * should be dropped since they do not match any UUID. + */ + if (hdev->discovery.uuid_count > 0) + return; + } if (dev_class && !eir_has_data_type(ev->eir, eir_len, EIR_CLASS_OF_DEV)) eir_len = eir_append_data(ev->eir, eir_len, EIR_CLASS_OF_DEV, dev_class, 3); - if (scan_rsp_len > 0) + if (scan_rsp_len > 0) { + /* When using service discovery and a list of UUID is + * provided, results with no matching UUID should be + * dropped if there is no previous match from the + * advertising data. + */ + if (hdev->discovery.uuid_count > 0) { + if (!match && !eir_has_uuids(scan_rsp, scan_rsp_len, + hdev->discovery.uuid_count, + hdev->discovery.uuids)) + return; + } + + /* Append scan response data to event */ memcpy(ev->eir + eir_len, scan_rsp, scan_rsp_len); + } else { + /* When using service discovery and a list of UUID is + * provided, results with empty scan response and no + * previous matched advertising data should be dropped. + */ + if (hdev->discovery.uuid_count > 0 && !match) + return; + } ev->eir_len = cpu_to_le16(eir_len + scan_rsp_len); ev_size = sizeof(*ev) + eir_len + scan_rsp_len; @@ -6886,23 +7125,9 @@ void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, void mgmt_discovering(struct hci_dev *hdev, u8 discovering) { struct mgmt_ev_discovering ev; - struct pending_cmd *cmd; BT_DBG("%s discovering %u", hdev->name, discovering); - if (discovering) - cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev); - else - cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev); - - if (cmd != NULL) { - u8 type = hdev->discovery.type; - - cmd_complete(cmd->sk, hdev->id, cmd->opcode, 0, &type, - sizeof(type)); - mgmt_pending_remove(cmd); - } - memset(&ev, 0, sizeof(ev)); ev.type = hdev->discovery.type; ev.discovering = discovering; diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c index 069b76e03b57..96bf16dcd9e9 100644 --- a/net/bluetooth/smp.c +++ b/net/bluetooth/smp.c @@ -29,14 +29,34 @@ #include <net/bluetooth/l2cap.h> #include <net/bluetooth/mgmt.h> +#include "ecc.h" #include "smp.h" +/* Low-level debug macros to be used for stuff that we don't want + * accidentially in dmesg, i.e. the values of the various crypto keys + * and the inputs & outputs of crypto functions. + */ +#ifdef DEBUG +#define SMP_DBG(fmt, ...) printk(KERN_DEBUG "%s: " fmt, __func__, \ + ##__VA_ARGS__) +#else +#define SMP_DBG(fmt, ...) no_printk(KERN_DEBUG "%s: " fmt, __func__, \ + ##__VA_ARGS__) +#endif + #define SMP_ALLOW_CMD(smp, code) set_bit(code, &smp->allow_cmd) +/* Keys which are not distributed with Secure Connections */ +#define SMP_SC_NO_DIST (SMP_DIST_ENC_KEY | SMP_DIST_LINK_KEY); + #define SMP_TIMEOUT msecs_to_jiffies(30000) -#define AUTH_REQ_MASK 0x07 -#define KEY_DIST_MASK 0x07 +#define AUTH_REQ_MASK(dev) (test_bit(HCI_SC_ENABLED, &(dev)->dev_flags) ? \ + 0x1f : 0x07) +#define KEY_DIST_MASK 0x07 + +/* Maximum message length that can be passed to aes_cmac */ +#define CMAC_MSG_MAX 80 enum { SMP_FLAG_TK_VALID, @@ -44,6 +64,12 @@ enum { SMP_FLAG_MITM_AUTH, SMP_FLAG_COMPLETE, SMP_FLAG_INITIATOR, + SMP_FLAG_SC, + SMP_FLAG_REMOTE_PK, + SMP_FLAG_DEBUG_KEY, + SMP_FLAG_WAIT_USER, + SMP_FLAG_DHKEY_PENDING, + SMP_FLAG_OOB, }; struct smp_chan { @@ -57,6 +83,7 @@ struct smp_chan { u8 rrnd[16]; /* SMP Pairing Random (remote) */ u8 pcnf[16]; /* SMP Pairing Confirm */ u8 tk[16]; /* SMP Temporary Key */ + u8 rr[16]; u8 enc_key_size; u8 remote_key_dist; bdaddr_t id_addr; @@ -67,9 +94,43 @@ struct smp_chan { struct smp_ltk *ltk; struct smp_ltk *slave_ltk; struct smp_irk *remote_irk; + u8 *link_key; unsigned long flags; + u8 method; + u8 passkey_round; + + /* Secure Connections variables */ + u8 local_pk[64]; + u8 local_sk[32]; + u8 remote_pk[64]; + u8 dhkey[32]; + u8 mackey[16]; struct crypto_blkcipher *tfm_aes; + struct crypto_hash *tfm_cmac; +}; + +/* These debug key values are defined in the SMP section of the core + * specification. debug_pk is the public debug key and debug_sk the + * private debug key. + */ +static const u8 debug_pk[64] = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, + 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, + 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, + + 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74, + 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76, + 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, + 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc, +}; + +static const u8 debug_sk[32] = { + 0xbd, 0x1a, 0x3c, 0xcd, 0xa6, 0xb8, 0x99, 0x58, + 0x99, 0xb7, 0x40, 0xeb, 0x7b, 0x60, 0xff, 0x4a, + 0x50, 0x3f, 0x10, 0xd2, 0xe3, 0xb3, 0xc9, 0x74, + 0x38, 0x5f, 0xc5, 0xa3, 0xd4, 0xf6, 0x49, 0x3f, }; static inline void swap_buf(const u8 *src, u8 *dst, size_t len) @@ -80,14 +141,22 @@ static inline void swap_buf(const u8 *src, u8 *dst, size_t len) dst[len - 1 - i] = src[i]; } -static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r) +/* The following functions map to the LE SC SMP crypto functions + * AES-CMAC, f4, f5, f6, g2 and h6. + */ + +static int aes_cmac(struct crypto_hash *tfm, const u8 k[16], const u8 *m, + size_t len, u8 mac[16]) { - struct blkcipher_desc desc; + uint8_t tmp[16], mac_msb[16], msg_msb[CMAC_MSG_MAX]; + struct hash_desc desc; struct scatterlist sg; - uint8_t tmp[16], data[16]; int err; - if (tfm == NULL) { + if (len > CMAC_MSG_MAX) + return -EFBIG; + + if (!tfm) { BT_ERR("tfm %p", tfm); return -EINVAL; } @@ -95,105 +164,233 @@ static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r) desc.tfm = tfm; desc.flags = 0; - /* The most significant octet of key corresponds to k[0] */ + crypto_hash_init(&desc); + + /* Swap key and message from LSB to MSB */ swap_buf(k, tmp, 16); + swap_buf(m, msg_msb, len); - err = crypto_blkcipher_setkey(tfm, tmp, 16); + SMP_DBG("msg (len %zu) %*phN", len, (int) len, m); + SMP_DBG("key %16phN", k); + + err = crypto_hash_setkey(tfm, tmp, 16); if (err) { BT_ERR("cipher setkey failed: %d", err); return err; } - /* Most significant octet of plaintextData corresponds to data[0] */ - swap_buf(r, data, 16); + sg_init_one(&sg, msg_msb, len); - sg_init_one(&sg, data, 16); + err = crypto_hash_update(&desc, &sg, len); + if (err) { + BT_ERR("Hash update error %d", err); + return err; + } - err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16); + err = crypto_hash_final(&desc, mac_msb); + if (err) { + BT_ERR("Hash final error %d", err); + return err; + } + + swap_buf(mac_msb, mac, 16); + + SMP_DBG("mac %16phN", mac); + + return 0; +} + +static int smp_f4(struct crypto_hash *tfm_cmac, const u8 u[32], const u8 v[32], + const u8 x[16], u8 z, u8 res[16]) +{ + u8 m[65]; + int err; + + SMP_DBG("u %32phN", u); + SMP_DBG("v %32phN", v); + SMP_DBG("x %16phN z %02x", x, z); + + m[0] = z; + memcpy(m + 1, v, 32); + memcpy(m + 33, u, 32); + + err = aes_cmac(tfm_cmac, x, m, sizeof(m), res); if (err) - BT_ERR("Encrypt data error %d", err); + return err; - /* Most significant octet of encryptedData corresponds to data[0] */ - swap_buf(data, r, 16); + SMP_DBG("res %16phN", res); return err; } -static int smp_ah(struct crypto_blkcipher *tfm, u8 irk[16], u8 r[3], u8 res[3]) +static int smp_f5(struct crypto_hash *tfm_cmac, u8 w[32], u8 n1[16], u8 n2[16], + u8 a1[7], u8 a2[7], u8 mackey[16], u8 ltk[16]) { - u8 _res[16]; + /* The btle, salt and length "magic" values are as defined in + * the SMP section of the Bluetooth core specification. In ASCII + * the btle value ends up being 'btle'. The salt is just a + * random number whereas length is the value 256 in little + * endian format. + */ + const u8 btle[4] = { 0x65, 0x6c, 0x74, 0x62 }; + const u8 salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60, + 0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c }; + const u8 length[2] = { 0x00, 0x01 }; + u8 m[53], t[16]; int err; - /* r' = padding || r */ - memcpy(_res, r, 3); - memset(_res + 3, 0, 13); + SMP_DBG("w %32phN", w); + SMP_DBG("n1 %16phN n2 %16phN", n1, n2); + SMP_DBG("a1 %7phN a2 %7phN", a1, a2); - err = smp_e(tfm, irk, _res); - if (err) { - BT_ERR("Encrypt error"); + err = aes_cmac(tfm_cmac, salt, w, 32, t); + if (err) return err; - } - /* The output of the random address function ah is: - * ah(h, r) = e(k, r') mod 2^24 - * The output of the security function e is then truncated to 24 bits - * by taking the least significant 24 bits of the output of e as the - * result of ah. - */ - memcpy(res, _res, 3); + SMP_DBG("t %16phN", t); + + memcpy(m, length, 2); + memcpy(m + 2, a2, 7); + memcpy(m + 9, a1, 7); + memcpy(m + 16, n2, 16); + memcpy(m + 32, n1, 16); + memcpy(m + 48, btle, 4); + + m[52] = 0; /* Counter */ + + err = aes_cmac(tfm_cmac, t, m, sizeof(m), mackey); + if (err) + return err; + + SMP_DBG("mackey %16phN", mackey); + + m[52] = 1; /* Counter */ + + err = aes_cmac(tfm_cmac, t, m, sizeof(m), ltk); + if (err) + return err; + + SMP_DBG("ltk %16phN", ltk); return 0; } -bool smp_irk_matches(struct hci_dev *hdev, u8 irk[16], bdaddr_t *bdaddr) +static int smp_f6(struct crypto_hash *tfm_cmac, const u8 w[16], + const u8 n1[16], u8 n2[16], const u8 r[16], + const u8 io_cap[3], const u8 a1[7], const u8 a2[7], + u8 res[16]) { - struct l2cap_chan *chan = hdev->smp_data; - struct crypto_blkcipher *tfm; - u8 hash[3]; + u8 m[65]; int err; - if (!chan || !chan->data) - return false; + SMP_DBG("w %16phN", w); + SMP_DBG("n1 %16phN n2 %16phN", n1, n2); + SMP_DBG("r %16phN io_cap %3phN a1 %7phN a2 %7phN", r, io_cap, a1, a2); - tfm = chan->data; + memcpy(m, a2, 7); + memcpy(m + 7, a1, 7); + memcpy(m + 14, io_cap, 3); + memcpy(m + 17, r, 16); + memcpy(m + 33, n2, 16); + memcpy(m + 49, n1, 16); - BT_DBG("RPA %pMR IRK %*phN", bdaddr, 16, irk); + err = aes_cmac(tfm_cmac, w, m, sizeof(m), res); + if (err) + return err; - err = smp_ah(tfm, irk, &bdaddr->b[3], hash); + BT_DBG("res %16phN", res); + + return err; +} + +static int smp_g2(struct crypto_hash *tfm_cmac, const u8 u[32], const u8 v[32], + const u8 x[16], const u8 y[16], u32 *val) +{ + u8 m[80], tmp[16]; + int err; + + SMP_DBG("u %32phN", u); + SMP_DBG("v %32phN", v); + SMP_DBG("x %16phN y %16phN", x, y); + + memcpy(m, y, 16); + memcpy(m + 16, v, 32); + memcpy(m + 48, u, 32); + + err = aes_cmac(tfm_cmac, x, m, sizeof(m), tmp); if (err) - return false; + return err; - return !memcmp(bdaddr->b, hash, 3); + *val = get_unaligned_le32(tmp); + *val %= 1000000; + + SMP_DBG("val %06u", *val); + + return 0; } -int smp_generate_rpa(struct hci_dev *hdev, u8 irk[16], bdaddr_t *rpa) +static int smp_h6(struct crypto_hash *tfm_cmac, const u8 w[16], + const u8 key_id[4], u8 res[16]) { - struct l2cap_chan *chan = hdev->smp_data; - struct crypto_blkcipher *tfm; int err; - if (!chan || !chan->data) - return -EOPNOTSUPP; + SMP_DBG("w %16phN key_id %4phN", w, key_id); - tfm = chan->data; + err = aes_cmac(tfm_cmac, w, key_id, 4, res); + if (err) + return err; - get_random_bytes(&rpa->b[3], 3); + SMP_DBG("res %16phN", res); - rpa->b[5] &= 0x3f; /* Clear two most significant bits */ - rpa->b[5] |= 0x40; /* Set second most significant bit */ + return err; +} - err = smp_ah(tfm, irk, &rpa->b[3], rpa->b); - if (err < 0) +/* The following functions map to the legacy SMP crypto functions e, c1, + * s1 and ah. + */ + +static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r) +{ + struct blkcipher_desc desc; + struct scatterlist sg; + uint8_t tmp[16], data[16]; + int err; + + if (!tfm) { + BT_ERR("tfm %p", tfm); + return -EINVAL; + } + + desc.tfm = tfm; + desc.flags = 0; + + /* The most significant octet of key corresponds to k[0] */ + swap_buf(k, tmp, 16); + + err = crypto_blkcipher_setkey(tfm, tmp, 16); + if (err) { + BT_ERR("cipher setkey failed: %d", err); return err; + } - BT_DBG("RPA %pMR", rpa); + /* Most significant octet of plaintextData corresponds to data[0] */ + swap_buf(r, data, 16); - return 0; + sg_init_one(&sg, data, 16); + + err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16); + if (err) + BT_ERR("Encrypt data error %d", err); + + /* Most significant octet of encryptedData corresponds to data[0] */ + swap_buf(data, r, 16); + + return err; } -static int smp_c1(struct crypto_blkcipher *tfm_aes, u8 k[16], u8 r[16], - u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia, u8 _rat, - bdaddr_t *ra, u8 res[16]) +static int smp_c1(struct crypto_blkcipher *tfm_aes, const u8 k[16], + const u8 r[16], const u8 preq[7], const u8 pres[7], u8 _iat, + const bdaddr_t *ia, u8 _rat, const bdaddr_t *ra, u8 res[16]) { u8 p1[16], p2[16]; int err; @@ -232,8 +429,8 @@ static int smp_c1(struct crypto_blkcipher *tfm_aes, u8 k[16], u8 r[16], return err; } -static int smp_s1(struct crypto_blkcipher *tfm_aes, u8 k[16], u8 r1[16], - u8 r2[16], u8 _r[16]) +static int smp_s1(struct crypto_blkcipher *tfm_aes, const u8 k[16], + const u8 r1[16], const u8 r2[16], u8 _r[16]) { int err; @@ -248,6 +445,80 @@ static int smp_s1(struct crypto_blkcipher *tfm_aes, u8 k[16], u8 r1[16], return err; } +static int smp_ah(struct crypto_blkcipher *tfm, const u8 irk[16], + const u8 r[3], u8 res[3]) +{ + u8 _res[16]; + int err; + + /* r' = padding || r */ + memcpy(_res, r, 3); + memset(_res + 3, 0, 13); + + err = smp_e(tfm, irk, _res); + if (err) { + BT_ERR("Encrypt error"); + return err; + } + + /* The output of the random address function ah is: + * ah(h, r) = e(k, r') mod 2^24 + * The output of the security function e is then truncated to 24 bits + * by taking the least significant 24 bits of the output of e as the + * result of ah. + */ + memcpy(res, _res, 3); + + return 0; +} + +bool smp_irk_matches(struct hci_dev *hdev, const u8 irk[16], + const bdaddr_t *bdaddr) +{ + struct l2cap_chan *chan = hdev->smp_data; + struct crypto_blkcipher *tfm; + u8 hash[3]; + int err; + + if (!chan || !chan->data) + return false; + + tfm = chan->data; + + BT_DBG("RPA %pMR IRK %*phN", bdaddr, 16, irk); + + err = smp_ah(tfm, irk, &bdaddr->b[3], hash); + if (err) + return false; + + return !memcmp(bdaddr->b, hash, 3); +} + +int smp_generate_rpa(struct hci_dev *hdev, const u8 irk[16], bdaddr_t *rpa) +{ + struct l2cap_chan *chan = hdev->smp_data; + struct crypto_blkcipher *tfm; + int err; + + if (!chan || !chan->data) + return -EOPNOTSUPP; + + tfm = chan->data; + + get_random_bytes(&rpa->b[3], 3); + + rpa->b[5] &= 0x3f; /* Clear two most significant bits */ + rpa->b[5] |= 0x40; /* Set second most significant bit */ + + err = smp_ah(tfm, irk, &rpa->b[3], rpa->b); + if (err < 0) + return err; + + BT_DBG("RPA %pMR", rpa); + + return 0; +} + static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data) { struct l2cap_chan *chan = conn->smp; @@ -282,17 +553,22 @@ static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data) schedule_delayed_work(&smp->security_timer, SMP_TIMEOUT); } -static __u8 authreq_to_seclevel(__u8 authreq) +static u8 authreq_to_seclevel(u8 authreq) { - if (authreq & SMP_AUTH_MITM) - return BT_SECURITY_HIGH; - else + if (authreq & SMP_AUTH_MITM) { + if (authreq & SMP_AUTH_SC) + return BT_SECURITY_FIPS; + else + return BT_SECURITY_HIGH; + } else { return BT_SECURITY_MEDIUM; + } } static __u8 seclevel_to_authreq(__u8 sec_level) { switch (sec_level) { + case BT_SECURITY_FIPS: case BT_SECURITY_HIGH: return SMP_AUTH_MITM | SMP_AUTH_BONDING; case BT_SECURITY_MEDIUM: @@ -310,7 +586,7 @@ static void build_pairing_cmd(struct l2cap_conn *conn, struct smp_chan *smp = chan->data; struct hci_conn *hcon = conn->hcon; struct hci_dev *hdev = hcon->hdev; - u8 local_dist = 0, remote_dist = 0; + u8 local_dist = 0, remote_dist = 0, oob_flag = SMP_OOB_NOT_PRESENT; if (test_bit(HCI_BONDABLE, &conn->hcon->hdev->dev_flags)) { local_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; @@ -326,24 +602,52 @@ static void build_pairing_cmd(struct l2cap_conn *conn, if (test_bit(HCI_PRIVACY, &hdev->dev_flags)) local_dist |= SMP_DIST_ID_KEY; + if (test_bit(HCI_SC_ENABLED, &hdev->dev_flags) && + (authreq & SMP_AUTH_SC)) { + struct oob_data *oob_data; + u8 bdaddr_type; + + if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) { + local_dist |= SMP_DIST_LINK_KEY; + remote_dist |= SMP_DIST_LINK_KEY; + } + + if (hcon->dst_type == ADDR_LE_DEV_PUBLIC) + bdaddr_type = BDADDR_LE_PUBLIC; + else + bdaddr_type = BDADDR_LE_RANDOM; + + oob_data = hci_find_remote_oob_data(hdev, &hcon->dst, + bdaddr_type); + if (oob_data) { + set_bit(SMP_FLAG_OOB, &smp->flags); + oob_flag = SMP_OOB_PRESENT; + memcpy(smp->rr, oob_data->rand256, 16); + memcpy(smp->pcnf, oob_data->hash256, 16); + } + + } else { + authreq &= ~SMP_AUTH_SC; + } + if (rsp == NULL) { req->io_capability = conn->hcon->io_capability; - req->oob_flag = SMP_OOB_NOT_PRESENT; + req->oob_flag = oob_flag; req->max_key_size = SMP_MAX_ENC_KEY_SIZE; req->init_key_dist = local_dist; req->resp_key_dist = remote_dist; - req->auth_req = (authreq & AUTH_REQ_MASK); + req->auth_req = (authreq & AUTH_REQ_MASK(hdev)); smp->remote_key_dist = remote_dist; return; } rsp->io_capability = conn->hcon->io_capability; - rsp->oob_flag = SMP_OOB_NOT_PRESENT; + rsp->oob_flag = oob_flag; rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE; rsp->init_key_dist = req->init_key_dist & remote_dist; rsp->resp_key_dist = req->resp_key_dist & local_dist; - rsp->auth_req = (authreq & AUTH_REQ_MASK); + rsp->auth_req = (authreq & AUTH_REQ_MASK(hdev)); smp->remote_key_dist = rsp->init_key_dist; } @@ -366,6 +670,7 @@ static void smp_chan_destroy(struct l2cap_conn *conn) { struct l2cap_chan *chan = conn->smp; struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; bool complete; BUG_ON(!smp); @@ -373,12 +678,24 @@ static void smp_chan_destroy(struct l2cap_conn *conn) cancel_delayed_work_sync(&smp->security_timer); complete = test_bit(SMP_FLAG_COMPLETE, &smp->flags); - mgmt_smp_complete(conn->hcon, complete); + mgmt_smp_complete(hcon, complete); kfree(smp->csrk); kfree(smp->slave_csrk); + kfree(smp->link_key); crypto_free_blkcipher(smp->tfm_aes); + crypto_free_hash(smp->tfm_cmac); + + /* Ensure that we don't leave any debug key around if debug key + * support hasn't been explicitly enabled. + */ + if (smp->ltk && smp->ltk->type == SMP_LTK_P256_DEBUG && + !test_bit(HCI_KEEP_DEBUG_KEYS, &hcon->hdev->dev_flags)) { + list_del_rcu(&smp->ltk->list); + kfree_rcu(smp->ltk, rcu); + smp->ltk = NULL; + } /* If pairing failed clean up any keys we might have */ if (!complete) { @@ -400,7 +717,7 @@ static void smp_chan_destroy(struct l2cap_conn *conn) chan->data = NULL; kfree(smp); - hci_conn_drop(conn->hcon); + hci_conn_drop(hcon); } static void smp_failure(struct l2cap_conn *conn, u8 reason) @@ -424,6 +741,7 @@ static void smp_failure(struct l2cap_conn *conn, u8 reason) #define REQ_PASSKEY 0x02 #define CFM_PASSKEY 0x03 #define REQ_OOB 0x04 +#define DSP_PASSKEY 0x05 #define OVERLAP 0xFF static const u8 gen_method[5][5] = { @@ -434,6 +752,14 @@ static const u8 gen_method[5][5] = { { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP }, }; +static const u8 sc_method[5][5] = { + { JUST_WORKS, JUST_CFM, REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY }, + { JUST_WORKS, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY }, + { DSP_PASSKEY, DSP_PASSKEY, REQ_PASSKEY, JUST_WORKS, DSP_PASSKEY }, + { JUST_WORKS, JUST_CFM, JUST_WORKS, JUST_WORKS, JUST_CFM }, + { DSP_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY }, +}; + static u8 get_auth_method(struct smp_chan *smp, u8 local_io, u8 remote_io) { /* If either side has unknown io_caps, use JUST_CFM (which gets @@ -443,6 +769,9 @@ static u8 get_auth_method(struct smp_chan *smp, u8 local_io, u8 remote_io) remote_io > SMP_IO_KEYBOARD_DISPLAY) return JUST_CFM; + if (test_bit(SMP_FLAG_SC, &smp->flags)) + return sc_method[remote_io][local_io]; + return gen_method[remote_io][local_io]; } @@ -452,7 +781,6 @@ static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth, struct hci_conn *hcon = conn->hcon; struct l2cap_chan *chan = conn->smp; struct smp_chan *smp = chan->data; - u8 method; u32 passkey = 0; int ret = 0; @@ -469,26 +797,28 @@ static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth, * table. */ if (!(auth & SMP_AUTH_MITM)) - method = JUST_CFM; + smp->method = JUST_CFM; else - method = get_auth_method(smp, local_io, remote_io); + smp->method = get_auth_method(smp, local_io, remote_io); /* Don't confirm locally initiated pairing attempts */ - if (method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR, &smp->flags)) - method = JUST_WORKS; + if (smp->method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR, + &smp->flags)) + smp->method = JUST_WORKS; /* Don't bother user space with no IO capabilities */ - if (method == JUST_CFM && hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) - method = JUST_WORKS; + if (smp->method == JUST_CFM && + hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) + smp->method = JUST_WORKS; /* If Just Works, Continue with Zero TK */ - if (method == JUST_WORKS) { + if (smp->method == JUST_WORKS) { set_bit(SMP_FLAG_TK_VALID, &smp->flags); return 0; } /* Not Just Works/Confirm results in MITM Authentication */ - if (method != JUST_CFM) { + if (smp->method != JUST_CFM) { set_bit(SMP_FLAG_MITM_AUTH, &smp->flags); if (hcon->pending_sec_level < BT_SECURITY_HIGH) hcon->pending_sec_level = BT_SECURITY_HIGH; @@ -497,15 +827,15 @@ static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth, /* If both devices have Keyoard-Display I/O, the master * Confirms and the slave Enters the passkey. */ - if (method == OVERLAP) { + if (smp->method == OVERLAP) { if (hcon->role == HCI_ROLE_MASTER) - method = CFM_PASSKEY; + smp->method = CFM_PASSKEY; else - method = REQ_PASSKEY; + smp->method = REQ_PASSKEY; } /* Generate random passkey. */ - if (method == CFM_PASSKEY) { + if (smp->method == CFM_PASSKEY) { memset(smp->tk, 0, sizeof(smp->tk)); get_random_bytes(&passkey, sizeof(passkey)); passkey %= 1000000; @@ -514,10 +844,10 @@ static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth, set_bit(SMP_FLAG_TK_VALID, &smp->flags); } - if (method == REQ_PASSKEY) + if (smp->method == REQ_PASSKEY) ret = mgmt_user_passkey_request(hcon->hdev, &hcon->dst, hcon->type, hcon->dst_type); - else if (method == JUST_CFM) + else if (smp->method == JUST_CFM) ret = mgmt_user_confirm_request(hcon->hdev, &hcon->dst, hcon->type, hcon->dst_type, passkey, 1); @@ -638,11 +968,13 @@ static void smp_notify_keys(struct l2cap_conn *conn) mgmt_new_irk(hdev, smp->remote_irk); /* Now that user space can be considered to know the * identity address track the connection based on it - * from now on. + * from now on (assuming this is an LE link). */ - bacpy(&hcon->dst, &smp->remote_irk->bdaddr); - hcon->dst_type = smp->remote_irk->addr_type; - queue_work(hdev->workqueue, &conn->id_addr_update_work); + if (hcon->type == LE_LINK) { + bacpy(&hcon->dst, &smp->remote_irk->bdaddr); + hcon->dst_type = smp->remote_irk->addr_type; + queue_work(hdev->workqueue, &conn->id_addr_update_work); + } /* When receiving an indentity resolving key for * a remote device that does not use a resolvable @@ -661,10 +993,20 @@ static void smp_notify_keys(struct l2cap_conn *conn) } } - /* The LTKs and CSRKs should be persistent only if both sides - * had the bonding bit set in their authentication requests. - */ - persistent = !!((req->auth_req & rsp->auth_req) & SMP_AUTH_BONDING); + if (hcon->type == ACL_LINK) { + if (hcon->key_type == HCI_LK_DEBUG_COMBINATION) + persistent = false; + else + persistent = !test_bit(HCI_CONN_FLUSH_KEY, + &hcon->flags); + } else { + /* The LTKs and CSRKs should be persistent only if both sides + * had the bonding bit set in their authentication requests. + */ + persistent = !!((req->auth_req & rsp->auth_req) & + SMP_AUTH_BONDING); + } + if (smp->csrk) { smp->csrk->bdaddr_type = hcon->dst_type; @@ -689,6 +1031,81 @@ static void smp_notify_keys(struct l2cap_conn *conn) bacpy(&smp->slave_ltk->bdaddr, &hcon->dst); mgmt_new_ltk(hdev, smp->slave_ltk, persistent); } + + if (smp->link_key) { + struct link_key *key; + u8 type; + + if (test_bit(SMP_FLAG_DEBUG_KEY, &smp->flags)) + type = HCI_LK_DEBUG_COMBINATION; + else if (hcon->sec_level == BT_SECURITY_FIPS) + type = HCI_LK_AUTH_COMBINATION_P256; + else + type = HCI_LK_UNAUTH_COMBINATION_P256; + + key = hci_add_link_key(hdev, smp->conn->hcon, &hcon->dst, + smp->link_key, type, 0, &persistent); + if (key) { + mgmt_new_link_key(hdev, key, persistent); + + /* Don't keep debug keys around if the relevant + * flag is not set. + */ + if (!test_bit(HCI_KEEP_DEBUG_KEYS, &hdev->dev_flags) && + key->type == HCI_LK_DEBUG_COMBINATION) { + list_del_rcu(&key->list); + kfree_rcu(key, rcu); + } + } + } +} + +static void sc_add_ltk(struct smp_chan *smp) +{ + struct hci_conn *hcon = smp->conn->hcon; + u8 key_type, auth; + + if (test_bit(SMP_FLAG_DEBUG_KEY, &smp->flags)) + key_type = SMP_LTK_P256_DEBUG; + else + key_type = SMP_LTK_P256; + + if (hcon->pending_sec_level == BT_SECURITY_FIPS) + auth = 1; + else + auth = 0; + + memset(smp->tk + smp->enc_key_size, 0, + SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size); + + smp->ltk = hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, + key_type, auth, smp->tk, smp->enc_key_size, + 0, 0); +} + +static void sc_generate_link_key(struct smp_chan *smp) +{ + /* These constants are as specified in the core specification. + * In ASCII they spell out to 'tmp1' and 'lebr'. + */ + const u8 tmp1[4] = { 0x31, 0x70, 0x6d, 0x74 }; + const u8 lebr[4] = { 0x72, 0x62, 0x65, 0x6c }; + + smp->link_key = kzalloc(16, GFP_KERNEL); + if (!smp->link_key) + return; + + if (smp_h6(smp->tfm_cmac, smp->tk, tmp1, smp->link_key)) { + kfree(smp->link_key); + smp->link_key = NULL; + return; + } + + if (smp_h6(smp->tfm_cmac, smp->link_key, lebr, smp->link_key)) { + kfree(smp->link_key); + smp->link_key = NULL; + return; + } } static void smp_allow_key_dist(struct smp_chan *smp) @@ -705,6 +1122,35 @@ static void smp_allow_key_dist(struct smp_chan *smp) SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO); } +static void sc_generate_ltk(struct smp_chan *smp) +{ + /* These constants are as specified in the core specification. + * In ASCII they spell out to 'tmp2' and 'brle'. + */ + const u8 tmp2[4] = { 0x32, 0x70, 0x6d, 0x74 }; + const u8 brle[4] = { 0x65, 0x6c, 0x72, 0x62 }; + struct hci_conn *hcon = smp->conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct link_key *key; + + key = hci_find_link_key(hdev, &hcon->dst); + if (!key) { + BT_ERR("%s No Link Key found to generate LTK", hdev->name); + return; + } + + if (key->type == HCI_LK_DEBUG_COMBINATION) + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + + if (smp_h6(smp->tfm_cmac, key->val, tmp2, smp->tk)) + return; + + if (smp_h6(smp->tfm_cmac, smp->tk, brle, smp->tk)) + return; + + sc_add_ltk(smp); +} + static void smp_distribute_keys(struct smp_chan *smp) { struct smp_cmd_pairing *req, *rsp; @@ -733,6 +1179,16 @@ static void smp_distribute_keys(struct smp_chan *smp) *keydist &= req->resp_key_dist; } + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + if (hcon->type == LE_LINK && (*keydist & SMP_DIST_LINK_KEY)) + sc_generate_link_key(smp); + if (hcon->type == ACL_LINK && (*keydist & SMP_DIST_ENC_KEY)) + sc_generate_ltk(smp); + + /* Clear the keys which are generated but not distributed */ + *keydist &= ~SMP_SC_NO_DIST; + } + BT_DBG("keydist 0x%x", *keydist); if (*keydist & SMP_DIST_ENC_KEY) { @@ -844,6 +1300,14 @@ static struct smp_chan *smp_chan_create(struct l2cap_conn *conn) return NULL; } + smp->tfm_cmac = crypto_alloc_hash("cmac(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(smp->tfm_cmac)) { + BT_ERR("Unable to create CMAC crypto context"); + crypto_free_blkcipher(smp->tfm_aes); + kfree(smp); + return NULL; + } + smp->conn = conn; chan->data = smp; @@ -856,6 +1320,213 @@ static struct smp_chan *smp_chan_create(struct l2cap_conn *conn) return smp; } +static int sc_mackey_and_ltk(struct smp_chan *smp, u8 mackey[16], u8 ltk[16]) +{ + struct hci_conn *hcon = smp->conn->hcon; + u8 *na, *nb, a[7], b[7]; + + if (hcon->out) { + na = smp->prnd; + nb = smp->rrnd; + } else { + na = smp->rrnd; + nb = smp->prnd; + } + + memcpy(a, &hcon->init_addr, 6); + memcpy(b, &hcon->resp_addr, 6); + a[6] = hcon->init_addr_type; + b[6] = hcon->resp_addr_type; + + return smp_f5(smp->tfm_cmac, smp->dhkey, na, nb, a, b, mackey, ltk); +} + +static void sc_dhkey_check(struct smp_chan *smp) +{ + struct hci_conn *hcon = smp->conn->hcon; + struct smp_cmd_dhkey_check check; + u8 a[7], b[7], *local_addr, *remote_addr; + u8 io_cap[3], r[16]; + + memcpy(a, &hcon->init_addr, 6); + memcpy(b, &hcon->resp_addr, 6); + a[6] = hcon->init_addr_type; + b[6] = hcon->resp_addr_type; + + if (hcon->out) { + local_addr = a; + remote_addr = b; + memcpy(io_cap, &smp->preq[1], 3); + } else { + local_addr = b; + remote_addr = a; + memcpy(io_cap, &smp->prsp[1], 3); + } + + memset(r, 0, sizeof(r)); + + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + put_unaligned_le32(hcon->passkey_notify, r); + + if (smp->method == REQ_OOB) + memcpy(r, smp->rr, 16); + + smp_f6(smp->tfm_cmac, smp->mackey, smp->prnd, smp->rrnd, r, io_cap, + local_addr, remote_addr, check.e); + + smp_send_cmd(smp->conn, SMP_CMD_DHKEY_CHECK, sizeof(check), &check); +} + +static u8 sc_passkey_send_confirm(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct smp_cmd_pairing_confirm cfm; + u8 r; + + r = ((hcon->passkey_notify >> smp->passkey_round) & 0x01); + r |= 0x80; + + get_random_bytes(smp->prnd, sizeof(smp->prnd)); + + if (smp_f4(smp->tfm_cmac, smp->local_pk, smp->remote_pk, smp->prnd, r, + cfm.confirm_val)) + return SMP_UNSPECIFIED; + + smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cfm), &cfm); + + return 0; +} + +static u8 sc_passkey_round(struct smp_chan *smp, u8 smp_op) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + u8 cfm[16], r; + + /* Ignore the PDU if we've already done 20 rounds (0 - 19) */ + if (smp->passkey_round >= 20) + return 0; + + switch (smp_op) { + case SMP_CMD_PAIRING_RANDOM: + r = ((hcon->passkey_notify >> smp->passkey_round) & 0x01); + r |= 0x80; + + if (smp_f4(smp->tfm_cmac, smp->remote_pk, smp->local_pk, + smp->rrnd, r, cfm)) + return SMP_UNSPECIFIED; + + if (memcmp(smp->pcnf, cfm, 16)) + return SMP_CONFIRM_FAILED; + + smp->passkey_round++; + + if (smp->passkey_round == 20) { + /* Generate MacKey and LTK */ + if (sc_mackey_and_ltk(smp, smp->mackey, smp->tk)) + return SMP_UNSPECIFIED; + } + + /* The round is only complete when the initiator + * receives pairing random. + */ + if (!hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + if (smp->passkey_round == 20) + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + else + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + return 0; + } + + /* Start the next round */ + if (smp->passkey_round != 20) + return sc_passkey_round(smp, 0); + + /* Passkey rounds are complete - start DHKey Check */ + sc_dhkey_check(smp); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + + break; + + case SMP_CMD_PAIRING_CONFIRM: + if (test_bit(SMP_FLAG_WAIT_USER, &smp->flags)) { + set_bit(SMP_FLAG_CFM_PENDING, &smp->flags); + return 0; + } + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + if (hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + return 0; + } + + return sc_passkey_send_confirm(smp); + + case SMP_CMD_PUBLIC_KEY: + default: + /* Initiating device starts the round */ + if (!hcon->out) + return 0; + + BT_DBG("%s Starting passkey round %u", hdev->name, + smp->passkey_round + 1); + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + + return sc_passkey_send_confirm(smp); + } + + return 0; +} + +static int sc_user_reply(struct smp_chan *smp, u16 mgmt_op, __le32 passkey) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + u8 smp_op; + + clear_bit(SMP_FLAG_WAIT_USER, &smp->flags); + + switch (mgmt_op) { + case MGMT_OP_USER_PASSKEY_NEG_REPLY: + smp_failure(smp->conn, SMP_PASSKEY_ENTRY_FAILED); + return 0; + case MGMT_OP_USER_CONFIRM_NEG_REPLY: + smp_failure(smp->conn, SMP_NUMERIC_COMP_FAILED); + return 0; + case MGMT_OP_USER_PASSKEY_REPLY: + hcon->passkey_notify = le32_to_cpu(passkey); + smp->passkey_round = 0; + + if (test_and_clear_bit(SMP_FLAG_CFM_PENDING, &smp->flags)) + smp_op = SMP_CMD_PAIRING_CONFIRM; + else + smp_op = 0; + + if (sc_passkey_round(smp, smp_op)) + return -EIO; + + return 0; + } + + /* Initiator sends DHKey check first */ + if (hcon->out) { + sc_dhkey_check(smp); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + } else if (test_and_clear_bit(SMP_FLAG_DHKEY_PENDING, &smp->flags)) { + sc_dhkey_check(smp); + sc_add_ltk(smp); + } + + return 0; +} + int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey) { struct l2cap_conn *conn = hcon->l2cap_data; @@ -881,6 +1552,11 @@ int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey) smp = chan->data; + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + err = sc_user_reply(smp, mgmt_op, passkey); + goto unlock; + } + switch (mgmt_op) { case MGMT_OP_USER_PASSKEY_REPLY: value = le32_to_cpu(passkey); @@ -916,6 +1592,46 @@ unlock: return err; } +static void build_bredr_pairing_cmd(struct smp_chan *smp, + struct smp_cmd_pairing *req, + struct smp_cmd_pairing *rsp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_dev *hdev = conn->hcon->hdev; + u8 local_dist = 0, remote_dist = 0; + + if (test_bit(HCI_BONDABLE, &hdev->dev_flags)) { + local_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; + remote_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; + } + + if (test_bit(HCI_RPA_RESOLVING, &hdev->dev_flags)) + remote_dist |= SMP_DIST_ID_KEY; + + if (test_bit(HCI_PRIVACY, &hdev->dev_flags)) + local_dist |= SMP_DIST_ID_KEY; + + if (!rsp) { + memset(req, 0, sizeof(*req)); + + req->init_key_dist = local_dist; + req->resp_key_dist = remote_dist; + req->max_key_size = SMP_MAX_ENC_KEY_SIZE; + + smp->remote_key_dist = remote_dist; + + return; + } + + memset(rsp, 0, sizeof(*rsp)); + + rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE; + rsp->init_key_dist = req->init_key_dist & remote_dist; + rsp->resp_key_dist = req->resp_key_dist & local_dist; + + smp->remote_key_dist = rsp->init_key_dist; +} + static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) { struct smp_cmd_pairing rsp, *req = (void *) skb->data; @@ -942,16 +1658,49 @@ static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) return SMP_UNSPECIFIED; /* We didn't start the pairing, so match remote */ - auth = req->auth_req & AUTH_REQ_MASK; + auth = req->auth_req & AUTH_REQ_MASK(hdev); if (!test_bit(HCI_BONDABLE, &hdev->dev_flags) && (auth & SMP_AUTH_BONDING)) return SMP_PAIRING_NOTSUPP; + if (test_bit(HCI_SC_ONLY, &hdev->dev_flags) && !(auth & SMP_AUTH_SC)) + return SMP_AUTH_REQUIREMENTS; + smp->preq[0] = SMP_CMD_PAIRING_REQ; memcpy(&smp->preq[1], req, sizeof(*req)); skb_pull(skb, sizeof(*req)); + /* SMP over BR/EDR requires special treatment */ + if (conn->hcon->type == ACL_LINK) { + /* We must have a BR/EDR SC link */ + if (!test_bit(HCI_CONN_AES_CCM, &conn->hcon->flags)) + return SMP_CROSS_TRANSP_NOT_ALLOWED; + + set_bit(SMP_FLAG_SC, &smp->flags); + + build_bredr_pairing_cmd(smp, req, &rsp); + + key_size = min(req->max_key_size, rsp.max_key_size); + if (check_enc_key_size(conn, key_size)) + return SMP_ENC_KEY_SIZE; + + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], &rsp, sizeof(rsp)); + smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp); + + smp_distribute_keys(smp); + return 0; + } + + build_pairing_cmd(conn, req, &rsp, auth); + + if (rsp.auth_req & SMP_AUTH_SC) + set_bit(SMP_FLAG_SC, &smp->flags); + if (conn->hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) sec_level = BT_SECURITY_MEDIUM; else @@ -970,8 +1719,6 @@ static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) return SMP_AUTH_REQUIREMENTS; } - build_pairing_cmd(conn, req, &rsp, auth); - key_size = min(req->max_key_size, rsp.max_key_size); if (check_enc_key_size(conn, key_size)) return SMP_ENC_KEY_SIZE; @@ -982,7 +1729,18 @@ static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) memcpy(&smp->prsp[1], &rsp, sizeof(rsp)); smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp); - SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + + clear_bit(SMP_FLAG_INITIATOR, &smp->flags); + + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + SMP_ALLOW_CMD(smp, SMP_CMD_PUBLIC_KEY); + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + /* Wait for Public Key from Initiating Device */ + return 0; + } else { + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + } /* Request setup of TK */ ret = tk_request(conn, 0, auth, rsp.io_capability, req->io_capability); @@ -992,11 +1750,46 @@ static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) return 0; } +static u8 sc_send_public_key(struct smp_chan *smp) +{ + struct hci_dev *hdev = smp->conn->hcon->hdev; + + BT_DBG(""); + + if (test_bit(HCI_USE_DEBUG_KEYS, &hdev->dev_flags)) { + BT_DBG("Using debug keys"); + memcpy(smp->local_pk, debug_pk, 64); + memcpy(smp->local_sk, debug_sk, 32); + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + } else { + while (true) { + /* Generate local key pair for Secure Connections */ + if (!ecc_make_key(smp->local_pk, smp->local_sk)) + return SMP_UNSPECIFIED; + + /* This is unlikely, but we need to check that + * we didn't accidentially generate a debug key. + */ + if (memcmp(smp->local_sk, debug_sk, 32)) + break; + } + } + + SMP_DBG("Local Public Key X: %32phN", smp->local_pk); + SMP_DBG("Local Public Key Y: %32phN", &smp->local_pk[32]); + SMP_DBG("Local Private Key: %32phN", smp->local_sk); + + smp_send_cmd(smp->conn, SMP_CMD_PUBLIC_KEY, 64, smp->local_pk); + + return 0; +} + static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb) { struct smp_cmd_pairing *req, *rsp = (void *) skb->data; struct l2cap_chan *chan = conn->smp; struct smp_chan *smp = chan->data; + struct hci_dev *hdev = conn->hcon->hdev; u8 key_size, auth; int ret; @@ -1016,7 +1809,31 @@ static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb) if (check_enc_key_size(conn, key_size)) return SMP_ENC_KEY_SIZE; - auth = rsp->auth_req & AUTH_REQ_MASK; + auth = rsp->auth_req & AUTH_REQ_MASK(hdev); + + if (test_bit(HCI_SC_ONLY, &hdev->dev_flags) && !(auth & SMP_AUTH_SC)) + return SMP_AUTH_REQUIREMENTS; + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], rsp, sizeof(*rsp)); + + /* Update remote key distribution in case the remote cleared + * some bits that we had enabled in our request. + */ + smp->remote_key_dist &= rsp->resp_key_dist; + + /* For BR/EDR this means we're done and can start phase 3 */ + if (conn->hcon->type == ACL_LINK) { + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + smp_distribute_keys(smp); + return 0; + } + + if ((req->auth_req & SMP_AUTH_SC) && (auth & SMP_AUTH_SC)) + set_bit(SMP_FLAG_SC, &smp->flags); + else if (conn->hcon->pending_sec_level > BT_SECURITY_HIGH) + conn->hcon->pending_sec_level = BT_SECURITY_HIGH; /* If we need MITM check that it can be achieved */ if (conn->hcon->pending_sec_level >= BT_SECURITY_HIGH) { @@ -1030,14 +1847,18 @@ static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb) get_random_bytes(smp->prnd, sizeof(smp->prnd)); - smp->prsp[0] = SMP_CMD_PAIRING_RSP; - memcpy(&smp->prsp[1], rsp, sizeof(*rsp)); - /* Update remote key distribution in case the remote cleared * some bits that we had enabled in our request. */ smp->remote_key_dist &= rsp->resp_key_dist; + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + SMP_ALLOW_CMD(smp, SMP_CMD_PUBLIC_KEY); + return sc_send_public_key(smp); + } + auth |= req->auth_req; ret = tk_request(conn, 0, auth, req->io_capability, rsp->io_capability); @@ -1053,6 +1874,28 @@ static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb) return 0; } +static u8 sc_check_confirm(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + + BT_DBG(""); + + /* Public Key exchange must happen before any other steps */ + if (!test_bit(SMP_FLAG_REMOTE_PK, &smp->flags)) + return SMP_UNSPECIFIED; + + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + return sc_passkey_round(smp, SMP_CMD_PAIRING_CONFIRM); + + if (conn->hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), + smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + } + + return 0; +} + static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb) { struct l2cap_chan *chan = conn->smp; @@ -1066,6 +1909,9 @@ static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb) memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf)); skb_pull(skb, sizeof(smp->pcnf)); + if (test_bit(SMP_FLAG_SC, &smp->flags)) + return sc_check_confirm(smp); + if (conn->hcon->out) { smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), smp->prnd); @@ -1085,6 +1931,10 @@ static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb) { struct l2cap_chan *chan = conn->smp; struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; + u8 *pkax, *pkbx, *na, *nb; + u32 passkey; + int err; BT_DBG("conn %p", conn); @@ -1094,7 +1944,75 @@ static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb) memcpy(smp->rrnd, skb->data, sizeof(smp->rrnd)); skb_pull(skb, sizeof(smp->rrnd)); - return smp_random(smp); + if (!test_bit(SMP_FLAG_SC, &smp->flags)) + return smp_random(smp); + + if (hcon->out) { + pkax = smp->local_pk; + pkbx = smp->remote_pk; + na = smp->prnd; + nb = smp->rrnd; + } else { + pkax = smp->remote_pk; + pkbx = smp->local_pk; + na = smp->rrnd; + nb = smp->prnd; + } + + if (smp->method == REQ_OOB) { + if (!hcon->out) + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + goto mackey_and_ltk; + } + + /* Passkey entry has special treatment */ + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + return sc_passkey_round(smp, SMP_CMD_PAIRING_RANDOM); + + if (hcon->out) { + u8 cfm[16]; + + err = smp_f4(smp->tfm_cmac, smp->remote_pk, smp->local_pk, + smp->rrnd, 0, cfm); + if (err) + return SMP_UNSPECIFIED; + + if (memcmp(smp->pcnf, cfm, 16)) + return SMP_CONFIRM_FAILED; + } else { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), + smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + } + +mackey_and_ltk: + /* Generate MacKey and LTK */ + err = sc_mackey_and_ltk(smp, smp->mackey, smp->tk); + if (err) + return SMP_UNSPECIFIED; + + if (smp->method == JUST_WORKS || smp->method == REQ_OOB) { + if (hcon->out) { + sc_dhkey_check(smp); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + } + return 0; + } + + err = smp_g2(smp->tfm_cmac, pkax, pkbx, na, nb, &passkey); + if (err) + return SMP_UNSPECIFIED; + + err = mgmt_user_confirm_request(hcon->hdev, &hcon->dst, hcon->type, + hcon->dst_type, passkey, 0); + if (err) + return SMP_UNSPECIFIED; + + set_bit(SMP_FLAG_WAIT_USER, &smp->flags); + + return 0; } static bool smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level) @@ -1102,8 +2020,7 @@ static bool smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level) struct smp_ltk *key; struct hci_conn *hcon = conn->hcon; - key = hci_find_ltk_by_addr(hcon->hdev, &hcon->dst, hcon->dst_type, - hcon->role); + key = hci_find_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, hcon->role); if (!key) return false; @@ -1136,8 +2053,7 @@ bool smp_sufficient_security(struct hci_conn *hcon, u8 sec_level, */ if (key_pref == SMP_USE_LTK && test_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags) && - hci_find_ltk_by_addr(hcon->hdev, &hcon->dst, hcon->dst_type, - hcon->role)) + hci_find_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, hcon->role)) return false; if (hcon->sec_level >= sec_level) @@ -1151,6 +2067,7 @@ static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb) struct smp_cmd_security_req *rp = (void *) skb->data; struct smp_cmd_pairing cp; struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; struct smp_chan *smp; u8 sec_level, auth; @@ -1162,7 +2079,10 @@ static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb) if (hcon->role != HCI_ROLE_MASTER) return SMP_CMD_NOTSUPP; - auth = rp->auth_req & AUTH_REQ_MASK; + auth = rp->auth_req & AUTH_REQ_MASK(hdev); + + if (test_bit(HCI_SC_ONLY, &hdev->dev_flags) && !(auth & SMP_AUTH_SC)) + return SMP_AUTH_REQUIREMENTS; if (hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) sec_level = BT_SECURITY_MEDIUM; @@ -1245,6 +2165,9 @@ int smp_conn_security(struct hci_conn *hcon, __u8 sec_level) authreq = seclevel_to_authreq(sec_level); + if (test_bit(HCI_SC_ENABLED, &hcon->hdev->dev_flags)) + authreq |= SMP_AUTH_SC; + /* Require MITM if IO Capability allows or the security level * requires it. */ @@ -1432,6 +2355,234 @@ static int smp_cmd_sign_info(struct l2cap_conn *conn, struct sk_buff *skb) return 0; } +static u8 sc_select_method(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct smp_cmd_pairing *local, *remote; + u8 local_mitm, remote_mitm, local_io, remote_io, method; + + if (test_bit(SMP_FLAG_OOB, &smp->flags)) + return REQ_OOB; + + /* The preq/prsp contain the raw Pairing Request/Response PDUs + * which are needed as inputs to some crypto functions. To get + * the "struct smp_cmd_pairing" from them we need to skip the + * first byte which contains the opcode. + */ + if (hcon->out) { + local = (void *) &smp->preq[1]; + remote = (void *) &smp->prsp[1]; + } else { + local = (void *) &smp->prsp[1]; + remote = (void *) &smp->preq[1]; + } + + local_io = local->io_capability; + remote_io = remote->io_capability; + + local_mitm = (local->auth_req & SMP_AUTH_MITM); + remote_mitm = (remote->auth_req & SMP_AUTH_MITM); + + /* If either side wants MITM, look up the method from the table, + * otherwise use JUST WORKS. + */ + if (local_mitm || remote_mitm) + method = get_auth_method(smp, local_io, remote_io); + else + method = JUST_WORKS; + + /* Don't confirm locally initiated pairing attempts */ + if (method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR, &smp->flags)) + method = JUST_WORKS; + + return method; +} + +static int smp_cmd_public_key(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_public_key *key = (void *) skb->data; + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_dev *hdev = hcon->hdev; + struct smp_cmd_pairing_confirm cfm; + int err; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*key)) + return SMP_INVALID_PARAMS; + + memcpy(smp->remote_pk, key, 64); + + /* Non-initiating device sends its public key after receiving + * the key from the initiating device. + */ + if (!hcon->out) { + err = sc_send_public_key(smp); + if (err) + return err; + } + + SMP_DBG("Remote Public Key X: %32phN", smp->remote_pk); + SMP_DBG("Remote Public Key Y: %32phN", &smp->remote_pk[32]); + + if (!ecdh_shared_secret(smp->remote_pk, smp->local_sk, smp->dhkey)) + return SMP_UNSPECIFIED; + + SMP_DBG("DHKey %32phN", smp->dhkey); + + set_bit(SMP_FLAG_REMOTE_PK, &smp->flags); + + smp->method = sc_select_method(smp); + + BT_DBG("%s selected method 0x%02x", hdev->name, smp->method); + + /* JUST_WORKS and JUST_CFM result in an unauthenticated key */ + if (smp->method == JUST_WORKS || smp->method == JUST_CFM) + hcon->pending_sec_level = BT_SECURITY_MEDIUM; + else + hcon->pending_sec_level = BT_SECURITY_FIPS; + + if (!memcmp(debug_pk, smp->remote_pk, 64)) + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + + if (smp->method == DSP_PASSKEY) { + get_random_bytes(&hcon->passkey_notify, + sizeof(hcon->passkey_notify)); + hcon->passkey_notify %= 1000000; + hcon->passkey_entered = 0; + smp->passkey_round = 0; + if (mgmt_user_passkey_notify(hdev, &hcon->dst, hcon->type, + hcon->dst_type, + hcon->passkey_notify, + hcon->passkey_entered)) + return SMP_UNSPECIFIED; + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + return sc_passkey_round(smp, SMP_CMD_PUBLIC_KEY); + } + + if (smp->method == REQ_OOB) { + err = smp_f4(smp->tfm_cmac, smp->remote_pk, smp->remote_pk, + smp->rr, 0, cfm.confirm_val); + if (err) + return SMP_UNSPECIFIED; + + if (memcmp(cfm.confirm_val, smp->pcnf, 16)) + return SMP_CONFIRM_FAILED; + + if (hcon->out) + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + return 0; + } + + if (hcon->out) + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + + if (smp->method == REQ_PASSKEY) { + if (mgmt_user_passkey_request(hdev, &hcon->dst, hcon->type, + hcon->dst_type)) + return SMP_UNSPECIFIED; + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + set_bit(SMP_FLAG_WAIT_USER, &smp->flags); + return 0; + } + + /* The Initiating device waits for the non-initiating device to + * send the confirm value. + */ + if (conn->hcon->out) + return 0; + + err = smp_f4(smp->tfm_cmac, smp->local_pk, smp->remote_pk, smp->prnd, + 0, cfm.confirm_val); + if (err) + return SMP_UNSPECIFIED; + + smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cfm), &cfm); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + return 0; +} + +static int smp_cmd_dhkey_check(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_dhkey_check *check = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct hci_conn *hcon = conn->hcon; + struct smp_chan *smp = chan->data; + u8 a[7], b[7], *local_addr, *remote_addr; + u8 io_cap[3], r[16], e[16]; + int err; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*check)) + return SMP_INVALID_PARAMS; + + memcpy(a, &hcon->init_addr, 6); + memcpy(b, &hcon->resp_addr, 6); + a[6] = hcon->init_addr_type; + b[6] = hcon->resp_addr_type; + + if (hcon->out) { + local_addr = a; + remote_addr = b; + memcpy(io_cap, &smp->prsp[1], 3); + } else { + local_addr = b; + remote_addr = a; + memcpy(io_cap, &smp->preq[1], 3); + } + + memset(r, 0, sizeof(r)); + + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + put_unaligned_le32(hcon->passkey_notify, r); + + err = smp_f6(smp->tfm_cmac, smp->mackey, smp->rrnd, smp->prnd, r, + io_cap, remote_addr, local_addr, e); + if (err) + return SMP_UNSPECIFIED; + + if (memcmp(check->e, e, 16)) + return SMP_DHKEY_CHECK_FAILED; + + if (!hcon->out) { + if (test_bit(SMP_FLAG_WAIT_USER, &smp->flags)) { + set_bit(SMP_FLAG_DHKEY_PENDING, &smp->flags); + return 0; + } + + /* Slave sends DHKey check as response to master */ + sc_dhkey_check(smp); + } + + sc_add_ltk(smp); + + if (hcon->out) { + hci_le_start_enc(hcon, 0, 0, smp->tk); + hcon->enc_key_size = smp->enc_key_size; + } + + return 0; +} + +static int smp_cmd_keypress_notify(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct smp_cmd_keypress_notify *kp = (void *) skb->data; + + BT_DBG("value 0x%02x", kp->value); + + return 0; +} + static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb) { struct l2cap_conn *conn = chan->conn; @@ -1440,11 +2591,6 @@ static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb) __u8 code, reason; int err = 0; - if (hcon->type != LE_LINK) { - kfree_skb(skb); - return 0; - } - if (skb->len < 1) return -EILSEQ; @@ -1516,6 +2662,18 @@ static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb) reason = smp_cmd_sign_info(conn, skb); break; + case SMP_CMD_PUBLIC_KEY: + reason = smp_cmd_public_key(conn, skb); + break; + + case SMP_CMD_DHKEY_CHECK: + reason = smp_cmd_dhkey_check(conn, skb); + break; + + case SMP_CMD_KEYPRESS_NOTIFY: + reason = smp_cmd_keypress_notify(conn, skb); + break; + default: BT_DBG("Unknown command code 0x%2.2x", code); reason = SMP_CMD_NOTSUPP; @@ -1551,6 +2709,74 @@ static void smp_teardown_cb(struct l2cap_chan *chan, int err) l2cap_chan_put(chan); } +static void bredr_pairing(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct smp_cmd_pairing req; + struct smp_chan *smp; + + BT_DBG("chan %p", chan); + + /* Only new pairings are interesting */ + if (!test_bit(HCI_CONN_NEW_LINK_KEY, &hcon->flags)) + return; + + /* Don't bother if we're not encrypted */ + if (!test_bit(HCI_CONN_ENCRYPT, &hcon->flags)) + return; + + /* Only master may initiate SMP over BR/EDR */ + if (hcon->role != HCI_ROLE_MASTER) + return; + + /* Secure Connections support must be enabled */ + if (!test_bit(HCI_SC_ENABLED, &hdev->dev_flags)) + return; + + /* BR/EDR must use Secure Connections for SMP */ + if (!test_bit(HCI_CONN_AES_CCM, &hcon->flags) && + !test_bit(HCI_FORCE_LESC, &hdev->dbg_flags)) + return; + + /* If our LE support is not enabled don't do anything */ + if (!test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) + return; + + /* Don't bother if remote LE support is not enabled */ + if (!lmp_host_le_capable(hcon)) + return; + + /* Remote must support SMP fixed chan for BR/EDR */ + if (!(conn->remote_fixed_chan & L2CAP_FC_SMP_BREDR)) + return; + + /* Don't bother if SMP is already ongoing */ + if (chan->data) + return; + + smp = smp_chan_create(conn); + if (!smp) { + BT_ERR("%s unable to create SMP context for BR/EDR", + hdev->name); + return; + } + + set_bit(SMP_FLAG_SC, &smp->flags); + + BT_DBG("%s starting SMP over BR/EDR", hdev->name); + + /* Prepare and send the BR/EDR SMP Pairing Request */ + build_bredr_pairing_cmd(smp, &req, NULL); + + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], &req, sizeof(req)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(req), &req); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP); +} + static void smp_resume_cb(struct l2cap_chan *chan) { struct smp_chan *smp = chan->data; @@ -1559,6 +2785,11 @@ static void smp_resume_cb(struct l2cap_chan *chan) BT_DBG("chan %p", chan); + if (hcon->type == ACL_LINK) { + bredr_pairing(chan); + return; + } + if (!smp) return; @@ -1573,11 +2804,15 @@ static void smp_resume_cb(struct l2cap_chan *chan) static void smp_ready_cb(struct l2cap_chan *chan) { struct l2cap_conn *conn = chan->conn; + struct hci_conn *hcon = conn->hcon; BT_DBG("chan %p", chan); conn->smp = chan; l2cap_chan_hold(chan); + + if (hcon->type == ACL_LINK && test_bit(HCI_CONN_ENCRYPT, &hcon->flags)) + bredr_pairing(chan); } static int smp_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) @@ -1682,34 +2917,40 @@ static const struct l2cap_ops smp_root_chan_ops = { .memcpy_fromiovec = l2cap_chan_no_memcpy_fromiovec, }; -int smp_register(struct hci_dev *hdev) +static struct l2cap_chan *smp_add_cid(struct hci_dev *hdev, u16 cid) { struct l2cap_chan *chan; struct crypto_blkcipher *tfm_aes; - BT_DBG("%s", hdev->name); + if (cid == L2CAP_CID_SMP_BREDR) { + tfm_aes = NULL; + goto create_chan; + } tfm_aes = crypto_alloc_blkcipher("ecb(aes)", 0, 0); if (IS_ERR(tfm_aes)) { - int err = PTR_ERR(tfm_aes); BT_ERR("Unable to create crypto context"); - return err; + return ERR_PTR(PTR_ERR(tfm_aes)); } +create_chan: chan = l2cap_chan_create(); if (!chan) { crypto_free_blkcipher(tfm_aes); - return -ENOMEM; + return ERR_PTR(-ENOMEM); } chan->data = tfm_aes; - l2cap_add_scid(chan, L2CAP_CID_SMP); + l2cap_add_scid(chan, cid); l2cap_chan_set_defaults(chan); bacpy(&chan->src, &hdev->bdaddr); - chan->src_type = BDADDR_LE_PUBLIC; + if (cid == L2CAP_CID_SMP) + chan->src_type = BDADDR_LE_PUBLIC; + else + chan->src_type = BDADDR_BREDR; chan->state = BT_LISTEN; chan->mode = L2CAP_MODE_BASIC; chan->imtu = L2CAP_DEFAULT_MTU; @@ -1718,20 +2959,14 @@ int smp_register(struct hci_dev *hdev) /* Set correct nesting level for a parent/listening channel */ atomic_set(&chan->nesting, L2CAP_NESTING_PARENT); - hdev->smp_data = chan; - - return 0; + return chan; } -void smp_unregister(struct hci_dev *hdev) +static void smp_del_chan(struct l2cap_chan *chan) { - struct l2cap_chan *chan = hdev->smp_data; - struct crypto_blkcipher *tfm_aes; - - if (!chan) - return; + struct crypto_blkcipher *tfm_aes; - BT_DBG("%s chan %p", hdev->name, chan); + BT_DBG("chan %p", chan); tfm_aes = chan->data; if (tfm_aes) { @@ -1739,6 +2974,52 @@ void smp_unregister(struct hci_dev *hdev) crypto_free_blkcipher(tfm_aes); } - hdev->smp_data = NULL; l2cap_chan_put(chan); } + +int smp_register(struct hci_dev *hdev) +{ + struct l2cap_chan *chan; + + BT_DBG("%s", hdev->name); + + chan = smp_add_cid(hdev, L2CAP_CID_SMP); + if (IS_ERR(chan)) + return PTR_ERR(chan); + + hdev->smp_data = chan; + + if (!lmp_sc_capable(hdev) && + !test_bit(HCI_FORCE_LESC, &hdev->dbg_flags)) + return 0; + + chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR); + if (IS_ERR(chan)) { + int err = PTR_ERR(chan); + chan = hdev->smp_data; + hdev->smp_data = NULL; + smp_del_chan(chan); + return err; + } + + hdev->smp_bredr_data = chan; + + return 0; +} + +void smp_unregister(struct hci_dev *hdev) +{ + struct l2cap_chan *chan; + + if (hdev->smp_bredr_data) { + chan = hdev->smp_bredr_data; + hdev->smp_bredr_data = NULL; + smp_del_chan(chan); + } + + if (hdev->smp_data) { + chan = hdev->smp_data; + hdev->smp_data = NULL; + smp_del_chan(chan); + } +} diff --git a/net/bluetooth/smp.h b/net/bluetooth/smp.h index f76083b85005..3296bf42ae80 100644 --- a/net/bluetooth/smp.h +++ b/net/bluetooth/smp.h @@ -50,10 +50,13 @@ struct smp_cmd_pairing { #define SMP_DIST_ENC_KEY 0x01 #define SMP_DIST_ID_KEY 0x02 #define SMP_DIST_SIGN 0x04 +#define SMP_DIST_LINK_KEY 0x08 #define SMP_AUTH_NONE 0x00 #define SMP_AUTH_BONDING 0x01 #define SMP_AUTH_MITM 0x04 +#define SMP_AUTH_SC 0x08 +#define SMP_AUTH_KEYPRESS 0x10 #define SMP_CMD_PAIRING_CONFIRM 0x03 struct smp_cmd_pairing_confirm { @@ -102,7 +105,23 @@ struct smp_cmd_security_req { __u8 auth_req; } __packed; -#define SMP_CMD_MAX 0x0b +#define SMP_CMD_PUBLIC_KEY 0x0c +struct smp_cmd_public_key { + __u8 x[32]; + __u8 y[32]; +} __packed; + +#define SMP_CMD_DHKEY_CHECK 0x0d +struct smp_cmd_dhkey_check { + __u8 e[16]; +} __packed; + +#define SMP_CMD_KEYPRESS_NOTIFY 0x0e +struct smp_cmd_keypress_notify { + __u8 value; +} __packed; + +#define SMP_CMD_MAX 0x0e #define SMP_PASSKEY_ENTRY_FAILED 0x01 #define SMP_OOB_NOT_AVAIL 0x02 @@ -114,6 +133,10 @@ struct smp_cmd_security_req { #define SMP_UNSPECIFIED 0x08 #define SMP_REPEATED_ATTEMPTS 0x09 #define SMP_INVALID_PARAMS 0x0a +#define SMP_DHKEY_CHECK_FAILED 0x0b +#define SMP_NUMERIC_COMP_FAILED 0x0c +#define SMP_BREDR_PAIRING_IN_PROGRESS 0x0d +#define SMP_CROSS_TRANSP_NOT_ALLOWED 0x0e #define SMP_MIN_ENC_KEY_SIZE 7 #define SMP_MAX_ENC_KEY_SIZE 16 @@ -123,12 +146,29 @@ enum { SMP_STK, SMP_LTK, SMP_LTK_SLAVE, + SMP_LTK_P256, + SMP_LTK_P256_DEBUG, }; +static inline bool smp_ltk_is_sc(struct smp_ltk *key) +{ + switch (key->type) { + case SMP_LTK_P256: + case SMP_LTK_P256_DEBUG: + return true; + } + + return false; +} + static inline u8 smp_ltk_sec_level(struct smp_ltk *key) { - if (key->authenticated) - return BT_SECURITY_HIGH; + if (key->authenticated) { + if (smp_ltk_is_sc(key)) + return BT_SECURITY_FIPS; + else + return BT_SECURITY_HIGH; + } return BT_SECURITY_MEDIUM; } @@ -145,8 +185,9 @@ bool smp_sufficient_security(struct hci_conn *hcon, u8 sec_level, int smp_conn_security(struct hci_conn *hcon, __u8 sec_level); int smp_user_confirm_reply(struct hci_conn *conn, u16 mgmt_op, __le32 passkey); -bool smp_irk_matches(struct hci_dev *hdev, u8 irk[16], bdaddr_t *bdaddr); -int smp_generate_rpa(struct hci_dev *hdev, u8 irk[16], bdaddr_t *rpa); +bool smp_irk_matches(struct hci_dev *hdev, const u8 irk[16], + const bdaddr_t *bdaddr); +int smp_generate_rpa(struct hci_dev *hdev, const u8 irk[16], bdaddr_t *rpa); int smp_register(struct hci_dev *hdev); void smp_unregister(struct hci_dev *hdev); diff --git a/net/ieee802154/6lowpan_rtnl.c b/net/ieee802154/6lowpan_rtnl.c index 290e14f2e92e..27eaa65e88e1 100644 --- a/net/ieee802154/6lowpan_rtnl.c +++ b/net/ieee802154/6lowpan_rtnl.c @@ -439,7 +439,6 @@ static void lowpan_set_lockdep_class_one(struct net_device *dev, &lowpan_netdev_xmit_lock_key); } - static int lowpan_dev_init(struct net_device *dev) { netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL); @@ -597,7 +596,7 @@ static int lowpan_newlink(struct net *src_net, struct net_device *dev, entry->ldev = dev; - /* Set the lowpan harware address to the wpan hardware address. */ + /* Set the lowpan hardware address to the wpan hardware address. */ memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN); mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx); diff --git a/net/ieee802154/af_ieee802154.c b/net/ieee802154/af_ieee802154.c index 26da1e179737..d0a1282cdf43 100644 --- a/net/ieee802154/af_ieee802154.c +++ b/net/ieee802154/af_ieee802154.c @@ -99,6 +99,7 @@ static int ieee802154_sock_release(struct socket *sock) } return 0; } + static int ieee802154_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len) { @@ -231,7 +232,6 @@ static const struct proto_ops ieee802154_dgram_ops = { #endif }; - /* Create a socket. Initialise the socket, blank the addresses * set the state. */ @@ -320,7 +320,6 @@ drop: return NET_RX_DROP; } - static struct packet_type ieee802154_packet_type = { .type = htons(ETH_P_IEEE802154), .func = ieee802154_rcv, @@ -354,6 +353,7 @@ err_dgram: out: return rc; } + static void __exit af_ieee802154_remove(void) { dev_remove_pack(&ieee802154_packet_type); diff --git a/net/ieee802154/dgram.c b/net/ieee802154/dgram.c index 2c7a93e7167e..d1930b70c4aa 100644 --- a/net/ieee802154/dgram.c +++ b/net/ieee802154/dgram.c @@ -154,7 +154,6 @@ static int dgram_ioctl(struct sock *sk, int cmd, unsigned long arg) spin_unlock_bh(&sk->sk_receive_queue.lock); return put_user(amount, (int __user *)arg); } - } return -ENOIOCTLCMD; diff --git a/net/ieee802154/netlink.c b/net/ieee802154/netlink.c index 63ee7d66950e..fa1464762d0d 100644 --- a/net/ieee802154/netlink.c +++ b/net/ieee802154/netlink.c @@ -1,5 +1,5 @@ /* - * Netlink inteface for IEEE 802.15.4 stack + * Netlink interface for IEEE 802.15.4 stack * * Copyright 2007, 2008 Siemens AG * @@ -73,7 +73,7 @@ out: } struct sk_buff *ieee802154_nl_new_reply(struct genl_info *info, - int flags, u8 req) + int flags, u8 req) { void *hdr; struct sk_buff *msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); @@ -147,7 +147,6 @@ static const struct genl_multicast_group ieee802154_mcgrps[] = { [IEEE802154_BEACON_MCGRP] = { .name = IEEE802154_MCAST_BEACON_NAME, }, }; - int __init ieee802154_nl_init(void) { return genl_register_family_with_ops_groups(&nl802154_family, diff --git a/net/ieee802154/nl-mac.c b/net/ieee802154/nl-mac.c index fe77f0c770b8..cd919493c976 100644 --- a/net/ieee802154/nl-mac.c +++ b/net/ieee802154/nl-mac.c @@ -1,5 +1,5 @@ /* - * Netlink inteface for IEEE 802.15.4 stack + * Netlink interface for IEEE 802.15.4 stack * * Copyright 2007, 2008 Siemens AG * @@ -346,7 +346,6 @@ int ieee802154_start_req(struct sk_buff *skb, struct genl_info *info) else page = 0; - if (addr.short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) { ieee802154_nl_start_confirm(dev, IEEE802154_NO_SHORT_ADDRESS); dev_put(dev); @@ -397,7 +396,6 @@ int ieee802154_scan_req(struct sk_buff *skb, struct genl_info *info) else page = 0; - ret = ieee802154_mlme_ops(dev)->scan_req(dev, type, channels, page, duration); @@ -548,8 +546,6 @@ out: return rc; } - - static int ieee802154_llsec_parse_key_id(struct genl_info *info, struct ieee802154_llsec_key_id *desc) @@ -765,8 +761,6 @@ out: return rc; } - - struct llsec_dump_data { struct sk_buff *skb; int s_idx, s_idx2; @@ -843,8 +837,6 @@ ieee802154_nl_llsec_change(struct sk_buff *skb, struct genl_info *info, return rc; } - - static int ieee802154_llsec_parse_key(struct genl_info *info, struct ieee802154_llsec_key *key) @@ -989,8 +981,6 @@ int ieee802154_llsec_dump_keys(struct sk_buff *skb, struct netlink_callback *cb) return ieee802154_llsec_dump_table(skb, cb, llsec_iter_keys); } - - static int llsec_parse_dev(struct genl_info *info, struct ieee802154_llsec_device *dev) @@ -1121,8 +1111,6 @@ int ieee802154_llsec_dump_devs(struct sk_buff *skb, struct netlink_callback *cb) return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devs); } - - static int llsec_add_devkey(struct net_device *dev, struct genl_info *info) { struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev); @@ -1237,8 +1225,6 @@ int ieee802154_llsec_dump_devkeys(struct sk_buff *skb, return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devkeys); } - - static int llsec_parse_seclevel(struct genl_info *info, struct ieee802154_llsec_seclevel *sl) diff --git a/net/ieee802154/nl-phy.c b/net/ieee802154/nl-phy.c index 80a946dddd90..7baf98b14611 100644 --- a/net/ieee802154/nl-phy.c +++ b/net/ieee802154/nl-phy.c @@ -1,5 +1,5 @@ /* - * Netlink inteface for IEEE 802.15.4 stack + * Netlink interface for IEEE 802.15.4 stack * * Copyright 2007, 2008 Siemens AG * @@ -94,7 +94,6 @@ int ieee802154_list_phy(struct sk_buff *skb, struct genl_info *info) if (name[nla_len(info->attrs[IEEE802154_ATTR_PHY_NAME]) - 1] != '\0') return -EINVAL; /* phy name should be null-terminated */ - phy = wpan_phy_find(name); if (!phy) return -ENODEV; diff --git a/net/ieee802154/raw.c b/net/ieee802154/raw.c index 61e9d2972947..1674b115c891 100644 --- a/net/ieee802154/raw.c +++ b/net/ieee802154/raw.c @@ -221,7 +221,6 @@ static int raw_rcv_skb(struct sock *sk, struct sk_buff *skb) return NET_RX_SUCCESS; } - void ieee802154_raw_deliver(struct net_device *dev, struct sk_buff *skb) { struct sock *sk; diff --git a/net/mac80211/chan.c b/net/mac80211/chan.c index c7c514220298..5d6dae9e4aac 100644 --- a/net/mac80211/chan.c +++ b/net/mac80211/chan.c @@ -932,6 +932,21 @@ ieee80211_vif_chanctx_reservation_complete(struct ieee80211_sub_if_data *sdata) } } +static void +ieee80211_vif_update_chandef(struct ieee80211_sub_if_data *sdata, + const struct cfg80211_chan_def *chandef) +{ + struct ieee80211_sub_if_data *vlan; + + sdata->vif.bss_conf.chandef = *chandef; + + if (sdata->vif.type != NL80211_IFTYPE_AP) + return; + + list_for_each_entry(vlan, &sdata->u.ap.vlans, u.vlan.list) + vlan->vif.bss_conf.chandef = *chandef; +} + static int ieee80211_vif_use_reserved_reassign(struct ieee80211_sub_if_data *sdata) { @@ -994,7 +1009,7 @@ ieee80211_vif_use_reserved_reassign(struct ieee80211_sub_if_data *sdata) if (sdata->vif.bss_conf.chandef.width != sdata->reserved_chandef.width) changed = BSS_CHANGED_BANDWIDTH; - sdata->vif.bss_conf.chandef = sdata->reserved_chandef; + ieee80211_vif_update_chandef(sdata, &sdata->reserved_chandef); if (changed) ieee80211_bss_info_change_notify(sdata, changed); @@ -1336,7 +1351,7 @@ static int ieee80211_vif_use_reserved_switch(struct ieee80211_local *local) sdata->reserved_chandef.width) changed = BSS_CHANGED_BANDWIDTH; - sdata->vif.bss_conf.chandef = sdata->reserved_chandef; + ieee80211_vif_update_chandef(sdata, &sdata->reserved_chandef); if (changed) ieee80211_bss_info_change_notify(sdata, changed); @@ -1507,7 +1522,7 @@ int ieee80211_vif_use_channel(struct ieee80211_sub_if_data *sdata, goto out; } - sdata->vif.bss_conf.chandef = *chandef; + ieee80211_vif_update_chandef(sdata, chandef); ret = ieee80211_assign_vif_chanctx(sdata, ctx); if (ret) { @@ -1649,7 +1664,7 @@ int ieee80211_vif_change_bandwidth(struct ieee80211_sub_if_data *sdata, break; } - sdata->vif.bss_conf.chandef = *chandef; + ieee80211_vif_update_chandef(sdata, chandef); ieee80211_recalc_chanctx_chantype(local, ctx); diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c index 538fe4ef5c85..417355390873 100644 --- a/net/mac80211/iface.c +++ b/net/mac80211/iface.c @@ -520,6 +520,7 @@ int ieee80211_do_open(struct wireless_dev *wdev, bool coming_up) sdata->vif.cab_queue = master->vif.cab_queue; memcpy(sdata->vif.hw_queue, master->vif.hw_queue, sizeof(sdata->vif.hw_queue)); + sdata->vif.bss_conf.chandef = master->vif.bss_conf.chandef; break; } case NL80211_IFTYPE_AP: diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c index ba06cd003375..75a9bf50207e 100644 --- a/net/mac80211/mlme.c +++ b/net/mac80211/mlme.c @@ -552,13 +552,17 @@ static void ieee80211_add_vht_ie(struct ieee80211_sub_if_data *sdata, cap = vht_cap.cap; if (sdata->u.mgd.flags & IEEE80211_STA_DISABLE_80P80MHZ) { - cap &= ~IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ; - cap |= IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ; + u32 bw = cap & IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK; + + cap &= ~IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK; + if (bw == IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ || + bw == IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ) + cap |= IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ; } if (sdata->u.mgd.flags & IEEE80211_STA_DISABLE_160MHZ) { cap &= ~IEEE80211_VHT_CAP_SHORT_GI_160; - cap &= ~IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ; + cap &= ~IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK; } /* @@ -2263,9 +2267,7 @@ static void ieee80211_mgd_probe_ap(struct ieee80211_sub_if_data *sdata, "detected beacon loss from AP (missed %d beacons) - probing\n", beacon_loss_count); - ieee80211_cqm_rssi_notify(&sdata->vif, - NL80211_CQM_RSSI_BEACON_LOSS_EVENT, - GFP_KERNEL); + ieee80211_cqm_beacon_loss_notify(&sdata->vif, GFP_KERNEL); } /* @@ -4898,3 +4900,13 @@ void ieee80211_cqm_rssi_notify(struct ieee80211_vif *vif, cfg80211_cqm_rssi_notify(sdata->dev, rssi_event, gfp); } EXPORT_SYMBOL(ieee80211_cqm_rssi_notify); + +void ieee80211_cqm_beacon_loss_notify(struct ieee80211_vif *vif, gfp_t gfp) +{ + struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif); + + trace_api_cqm_beacon_loss_notify(sdata->local, sdata); + + cfg80211_cqm_beacon_loss_notify(sdata->dev, gfp); +} +EXPORT_SYMBOL(ieee80211_cqm_beacon_loss_notify); diff --git a/net/mac80211/rate.c b/net/mac80211/rate.c index 08ab7d6d1517..d53355b011f5 100644 --- a/net/mac80211/rate.c +++ b/net/mac80211/rate.c @@ -446,7 +446,8 @@ static void rate_fixup_ratelist(struct ieee80211_vif *vif, * * XXX: Should this check all retry rates? */ - if (!(rates[0].flags & IEEE80211_TX_RC_MCS)) { + if (!(rates[0].flags & + (IEEE80211_TX_RC_MCS | IEEE80211_TX_RC_VHT_MCS))) { u32 basic_rates = vif->bss_conf.basic_rates; s8 baserate = basic_rates ? ffs(basic_rates) - 1 : 0; diff --git a/net/mac80211/rate.h b/net/mac80211/rate.h index 18babe302832..38652f09feaf 100644 --- a/net/mac80211/rate.h +++ b/net/mac80211/rate.h @@ -37,13 +37,35 @@ static inline void rate_control_tx_status(struct ieee80211_local *local, struct rate_control_ref *ref = local->rate_ctrl; struct ieee80211_sta *ista = &sta->sta; void *priv_sta = sta->rate_ctrl_priv; + struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); if (!ref || !test_sta_flag(sta, WLAN_STA_RATE_CONTROL)) return; - ref->ops->tx_status(ref->priv, sband, ista, priv_sta, skb); + if (ref->ops->tx_status) + ref->ops->tx_status(ref->priv, sband, ista, priv_sta, skb); + else + ref->ops->tx_status_noskb(ref->priv, sband, ista, priv_sta, info); } +static inline void +rate_control_tx_status_noskb(struct ieee80211_local *local, + struct ieee80211_supported_band *sband, + struct sta_info *sta, + struct ieee80211_tx_info *info) +{ + struct rate_control_ref *ref = local->rate_ctrl; + struct ieee80211_sta *ista = &sta->sta; + void *priv_sta = sta->rate_ctrl_priv; + + if (!ref || !test_sta_flag(sta, WLAN_STA_RATE_CONTROL)) + return; + + if (WARN_ON_ONCE(!ref->ops->tx_status_noskb)) + return; + + ref->ops->tx_status_noskb(ref->priv, sband, ista, priv_sta, info); +} static inline void rate_control_rate_init(struct sta_info *sta) { diff --git a/net/mac80211/rc80211_minstrel.c b/net/mac80211/rc80211_minstrel.c index c2b91bf47f6d..d51f6b1c549b 100644 --- a/net/mac80211/rc80211_minstrel.c +++ b/net/mac80211/rc80211_minstrel.c @@ -223,11 +223,10 @@ minstrel_update_stats(struct minstrel_priv *mp, struct minstrel_sta_info *mi) static void minstrel_tx_status(void *priv, struct ieee80211_supported_band *sband, struct ieee80211_sta *sta, void *priv_sta, - struct sk_buff *skb) + struct ieee80211_tx_info *info) { struct minstrel_priv *mp = priv; struct minstrel_sta_info *mi = priv_sta; - struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); struct ieee80211_tx_rate *ar = info->status.rates; int i, ndx; int success; @@ -674,7 +673,7 @@ static u32 minstrel_get_expected_throughput(void *priv_sta) const struct rate_control_ops mac80211_minstrel = { .name = "minstrel", - .tx_status = minstrel_tx_status, + .tx_status_noskb = minstrel_tx_status, .get_rate = minstrel_get_rate, .rate_init = minstrel_rate_init, .alloc = minstrel_alloc, diff --git a/net/mac80211/rc80211_minstrel_ht.c b/net/mac80211/rc80211_minstrel_ht.c index d013429af7ce..80452cfd2dc5 100644 --- a/net/mac80211/rc80211_minstrel_ht.c +++ b/net/mac80211/rc80211_minstrel_ht.c @@ -706,11 +706,10 @@ minstrel_aggr_check(struct ieee80211_sta *pubsta, struct sk_buff *skb) static void minstrel_ht_tx_status(void *priv, struct ieee80211_supported_band *sband, struct ieee80211_sta *sta, void *priv_sta, - struct sk_buff *skb) + struct ieee80211_tx_info *info) { struct minstrel_ht_sta_priv *msp = priv_sta; struct minstrel_ht_sta *mi = &msp->ht; - struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); struct ieee80211_tx_rate *ar = info->status.rates; struct minstrel_rate_stats *rate, *rate2; struct minstrel_priv *mp = priv; @@ -718,7 +717,8 @@ minstrel_ht_tx_status(void *priv, struct ieee80211_supported_band *sband, int i; if (!msp->is_ht) - return mac80211_minstrel.tx_status(priv, sband, sta, &msp->legacy, skb); + return mac80211_minstrel.tx_status_noskb(priv, sband, sta, + &msp->legacy, info); /* This packet was aggregated but doesn't carry status info */ if ((info->flags & IEEE80211_TX_CTL_AMPDU) && @@ -779,9 +779,6 @@ minstrel_ht_tx_status(void *priv, struct ieee80211_supported_band *sband, if (time_after(jiffies, mi->stats_update + (mp->update_interval / 2 * HZ) / 1000)) { update = true; minstrel_ht_update_stats(mp, mi); - if (!(info->flags & IEEE80211_TX_CTL_AMPDU) && - mi->max_prob_rate / MCS_GROUP_RATES != MINSTREL_CCK_GROUP) - minstrel_aggr_check(sta, skb); } if (update) @@ -1023,6 +1020,10 @@ minstrel_ht_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta, if (!msp->is_ht) return mac80211_minstrel.get_rate(priv, sta, &msp->legacy, txrc); + if (!(info->flags & IEEE80211_TX_CTL_AMPDU) && + mi->max_prob_rate / MCS_GROUP_RATES != MINSTREL_CCK_GROUP) + minstrel_aggr_check(sta, txrc->skb); + info->flags |= mi->tx_flags; minstrel_ht_check_cck_shortpreamble(mp, mi, txrc->short_preamble); @@ -1339,7 +1340,7 @@ static u32 minstrel_ht_get_expected_throughput(void *priv_sta) static const struct rate_control_ops mac80211_minstrel_ht = { .name = "minstrel_ht", - .tx_status = minstrel_ht_tx_status, + .tx_status_noskb = minstrel_ht_tx_status, .get_rate = minstrel_ht_get_rate, .rate_init = minstrel_ht_rate_init, .rate_update = minstrel_ht_rate_update, diff --git a/net/mac80211/status.c b/net/mac80211/status.c index 71de2d3866cc..bb146f377ee4 100644 --- a/net/mac80211/status.c +++ b/net/mac80211/status.c @@ -592,10 +592,9 @@ static void ieee80211_tx_latency_end_msrmnt(struct ieee80211_local *local, #define STA_LOST_TDLS_PKT_THRESHOLD 10 #define STA_LOST_TDLS_PKT_TIME (10*HZ) /* 10secs since last ACK */ -static void ieee80211_lost_packet(struct sta_info *sta, struct sk_buff *skb) +static void ieee80211_lost_packet(struct sta_info *sta, + struct ieee80211_tx_info *info) { - struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); - /* This packet was aggregated but doesn't carry status info */ if ((info->flags & IEEE80211_TX_CTL_AMPDU) && !(info->flags & IEEE80211_TX_STAT_AMPDU)) @@ -622,24 +621,13 @@ static void ieee80211_lost_packet(struct sta_info *sta, struct sk_buff *skb) sta->lost_packets = 0; } -void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb) +static int ieee80211_tx_get_rates(struct ieee80211_hw *hw, + struct ieee80211_tx_info *info, + int *retry_count) { - struct sk_buff *skb2; - struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; - struct ieee80211_local *local = hw_to_local(hw); - struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); - __le16 fc; - struct ieee80211_supported_band *sband; - struct ieee80211_sub_if_data *sdata; - struct net_device *prev_dev = NULL; - struct sta_info *sta, *tmp; - int retry_count = -1, i; int rates_idx = -1; - bool send_to_cooked; - bool acked; - struct ieee80211_bar *bar; - int rtap_len; - int shift = 0; + int count = -1; + int i; for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) { if ((info->flags & IEEE80211_TX_CTL_AMPDU) && @@ -657,12 +645,91 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb) break; } - retry_count += info->status.rates[i].count; + count += info->status.rates[i].count; } rates_idx = i - 1; - if (retry_count < 0) - retry_count = 0; + if (count < 0) + count = 0; + + *retry_count = count; + return rates_idx; +} + +void ieee80211_tx_status_noskb(struct ieee80211_hw *hw, + struct ieee80211_sta *pubsta, + struct ieee80211_tx_info *info) +{ + struct ieee80211_local *local = hw_to_local(hw); + struct ieee80211_supported_band *sband; + int retry_count; + int rates_idx; + bool acked; + + rates_idx = ieee80211_tx_get_rates(hw, info, &retry_count); + + sband = hw->wiphy->bands[info->band]; + + acked = !!(info->flags & IEEE80211_TX_STAT_ACK); + if (pubsta) { + struct sta_info *sta; + + sta = container_of(pubsta, struct sta_info, sta); + + if (!acked) + sta->tx_retry_failed++; + sta->tx_retry_count += retry_count; + + if (acked) { + sta->last_rx = jiffies; + + if (sta->lost_packets) + sta->lost_packets = 0; + + /* Track when last TDLS packet was ACKed */ + if (test_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH)) + sta->last_tdls_pkt_time = jiffies; + } else { + ieee80211_lost_packet(sta, info); + } + + rate_control_tx_status_noskb(local, sband, sta, info); + } + + if (acked) { + local->dot11TransmittedFrameCount++; + if (!pubsta) + local->dot11MulticastTransmittedFrameCount++; + if (retry_count > 0) + local->dot11RetryCount++; + if (retry_count > 1) + local->dot11MultipleRetryCount++; + } else { + local->dot11FailedCount++; + } +} +EXPORT_SYMBOL(ieee80211_tx_status_noskb); + +void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb) +{ + struct sk_buff *skb2; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data; + struct ieee80211_local *local = hw_to_local(hw); + struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb); + __le16 fc; + struct ieee80211_supported_band *sband; + struct ieee80211_sub_if_data *sdata; + struct net_device *prev_dev = NULL; + struct sta_info *sta, *tmp; + int retry_count; + int rates_idx; + bool send_to_cooked; + bool acked; + struct ieee80211_bar *bar; + int rtap_len; + int shift = 0; + + rates_idx = ieee80211_tx_get_rates(hw, info, &retry_count); rcu_read_lock(); @@ -767,7 +834,7 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb) if (test_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH)) sta->last_tdls_pkt_time = jiffies; } else { - ieee80211_lost_packet(sta, skb); + ieee80211_lost_packet(sta, info); } } diff --git a/net/mac80211/trace.h b/net/mac80211/trace.h index 85ccfbe863db..8e461a02c6a8 100644 --- a/net/mac80211/trace.h +++ b/net/mac80211/trace.h @@ -1829,6 +1829,12 @@ TRACE_EVENT(api_cqm_rssi_notify, ) ); +DEFINE_EVENT(local_sdata_evt, api_cqm_beacon_loss_notify, + TP_PROTO(struct ieee80211_local *local, + struct ieee80211_sub_if_data *sdata), + TP_ARGS(local, sdata) +); + TRACE_EVENT(api_scan_completed, TP_PROTO(struct ieee80211_local *local, bool aborted), diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c index 66ddbbeccd20..058686a721a1 100644 --- a/net/mac80211/tx.c +++ b/net/mac80211/tx.c @@ -60,7 +60,7 @@ static __le16 ieee80211_duration(struct ieee80211_tx_data *tx, rcu_read_unlock(); /* assume HW handles this */ - if (tx->rate.flags & IEEE80211_TX_RC_MCS) + if (tx->rate.flags & (IEEE80211_TX_RC_MCS | IEEE80211_TX_RC_VHT_MCS)) return 0; /* uh huh? */ diff --git a/net/mac80211/util.c b/net/mac80211/util.c index bb9664cb8831..974ebe70f5b0 100644 --- a/net/mac80211/util.c +++ b/net/mac80211/util.c @@ -1339,6 +1339,7 @@ static int ieee80211_build_preq_ies_band(struct ieee80211_local *local, int ext_rates_len; int shift; u32 rate_flags; + bool have_80mhz = false; *offset = 0; @@ -1467,7 +1468,15 @@ static int ieee80211_build_preq_ies_band(struct ieee80211_local *local, *offset = noffset; } - if (sband->vht_cap.vht_supported) { + /* Check if any channel in this sband supports at least 80 MHz */ + for (i = 0; i < sband->n_channels; i++) { + if (!(sband->channels[i].flags & IEEE80211_CHAN_NO_80MHZ)) { + have_80mhz = true; + break; + } + } + + if (sband->vht_cap.vht_supported && have_80mhz) { if (end - pos < 2 + sizeof(struct ieee80211_vht_cap)) goto out_err; pos = ieee80211_ie_build_vht_cap(pos, &sband->vht_cap, diff --git a/net/mac802154/iface.c b/net/mac802154/iface.c index 38dfc72d24b6..9ae893057dd7 100644 --- a/net/mac802154/iface.c +++ b/net/mac802154/iface.c @@ -510,11 +510,9 @@ ieee802154_if_add(struct ieee802154_local *local, const char *name, if (ret) goto err; - if (ndev) { - ret = register_netdevice(ndev); - if (ret < 0) - goto err; - } + ret = register_netdevice(ndev); + if (ret < 0) + goto err; mutex_lock(&local->iflist_mtx); list_add_tail_rcu(&sdata->list, &local->interfaces); diff --git a/net/mac802154/llsec.c b/net/mac802154/llsec.c index fa0d5237c2e0..dcf73958133a 100644 --- a/net/mac802154/llsec.c +++ b/net/mac802154/llsec.c @@ -75,8 +75,6 @@ void mac802154_llsec_destroy(struct mac802154_llsec *sec) } } - - int mac802154_llsec_get_params(struct mac802154_llsec *sec, struct ieee802154_llsec_params *params) { @@ -117,8 +115,6 @@ int mac802154_llsec_set_params(struct mac802154_llsec *sec, return 0; } - - static struct mac802154_llsec_key* llsec_key_alloc(const struct ieee802154_llsec_key *template) { @@ -294,8 +290,6 @@ int mac802154_llsec_key_del(struct mac802154_llsec *sec, return -ENOENT; } - - static bool llsec_dev_use_shortaddr(__le16 short_addr) { return short_addr != cpu_to_le16(IEEE802154_ADDR_UNDEF) && @@ -304,12 +298,12 @@ static bool llsec_dev_use_shortaddr(__le16 short_addr) static u32 llsec_dev_hash_short(__le16 short_addr, __le16 pan_id) { - return ((__force u16) short_addr) << 16 | (__force u16) pan_id; + return ((__force u16)short_addr) << 16 | (__force u16)pan_id; } static u64 llsec_dev_hash_long(__le64 hwaddr) { - return (__force u64) hwaddr; + return (__force u64)hwaddr; } static struct mac802154_llsec_device* @@ -411,8 +405,6 @@ int mac802154_llsec_dev_del(struct mac802154_llsec *sec, __le64 device_addr) return 0; } - - static struct mac802154_llsec_device_key* llsec_devkey_find(struct mac802154_llsec_device *dev, const struct ieee802154_llsec_key_id *key) @@ -475,8 +467,6 @@ int mac802154_llsec_devkey_del(struct mac802154_llsec *sec, return 0; } - - static struct mac802154_llsec_seclevel* llsec_find_seclevel(const struct mac802154_llsec *sec, const struct ieee802154_llsec_seclevel *sl) @@ -532,8 +522,6 @@ int mac802154_llsec_seclevel_del(struct mac802154_llsec *sec, return 0; } - - static int llsec_recover_addr(struct mac802154_llsec *sec, struct ieee802154_addr *addr) { @@ -609,7 +597,6 @@ found: return llsec_key_get(key); } - static void llsec_geniv(u8 iv[16], __le64 addr, const struct ieee802154_sechdr *sec) { @@ -786,8 +773,6 @@ fail: return rc; } - - static struct mac802154_llsec_device* llsec_lookup_dev(struct mac802154_llsec *sec, const struct ieee802154_addr *addr) diff --git a/net/mac802154/mib.c b/net/mac802154/mib.c index 3596b29ead6b..5cf019a57fd7 100644 --- a/net/mac802154/mib.c +++ b/net/mac802154/mib.c @@ -104,7 +104,6 @@ void mac802154_dev_set_page_channel(struct net_device *dev, u8 page, u8 chan) } } - int mac802154_get_params(struct net_device *dev, struct ieee802154_llsec_params *params) { @@ -136,7 +135,6 @@ int mac802154_set_params(struct net_device *dev, return res; } - int mac802154_add_key(struct net_device *dev, const struct ieee802154_llsec_key_id *id, const struct ieee802154_llsec_key *key) @@ -168,7 +166,6 @@ int mac802154_del_key(struct net_device *dev, return res; } - int mac802154_add_dev(struct net_device *dev, const struct ieee802154_llsec_device *llsec_dev) { @@ -198,7 +195,6 @@ int mac802154_del_dev(struct net_device *dev, __le64 dev_addr) return res; } - int mac802154_add_devkey(struct net_device *dev, __le64 device_addr, const struct ieee802154_llsec_device_key *key) @@ -231,7 +227,6 @@ int mac802154_del_devkey(struct net_device *dev, return res; } - int mac802154_add_seclevel(struct net_device *dev, const struct ieee802154_llsec_seclevel *sl) { @@ -262,7 +257,6 @@ int mac802154_del_seclevel(struct net_device *dev, return res; } - void mac802154_lock_table(struct net_device *dev) { struct ieee802154_sub_if_data *sdata = IEEE802154_DEV_TO_SUB_IF(dev); diff --git a/net/mac802154/rx.c b/net/mac802154/rx.c index 041dbd5958d4..c0d67b2b4132 100644 --- a/net/mac802154/rx.c +++ b/net/mac802154/rx.c @@ -85,8 +85,7 @@ ieee802154_subif_frame(struct ieee802154_sub_if_data *sdata, default: spin_unlock_bh(&sdata->mib_lock); pr_debug("invalid dest mode\n"); - kfree_skb(skb); - return NET_RX_DROP; + goto fail; } spin_unlock_bh(&sdata->mib_lock); diff --git a/net/nfc/digital_dep.c b/net/nfc/digital_dep.c index b60aa35c074f..f72be7433df3 100644 --- a/net/nfc/digital_dep.c +++ b/net/nfc/digital_dep.c @@ -17,6 +17,9 @@ #include "digital.h" +#define DIGITAL_NFC_DEP_N_RETRY_NACK 2 +#define DIGITAL_NFC_DEP_N_RETRY_ATN 2 + #define DIGITAL_NFC_DEP_FRAME_DIR_OUT 0xD4 #define DIGITAL_NFC_DEP_FRAME_DIR_IN 0xD5 @@ -32,20 +35,32 @@ #define DIGITAL_ATR_REQ_MIN_SIZE 16 #define DIGITAL_ATR_REQ_MAX_SIZE 64 -#define DIGITAL_LR_BITS_PAYLOAD_SIZE_254B 0x30 -#define DIGITAL_FSL_BITS_PAYLOAD_SIZE_254B \ - (DIGITAL_LR_BITS_PAYLOAD_SIZE_254B >> 4) +#define DIGITAL_DID_MAX 14 + +#define DIGITAL_PAYLOAD_SIZE_MAX 254 +#define DIGITAL_PAYLOAD_BITS_TO_PP(s) (((s) & 0x3) << 4) +#define DIGITAL_PAYLOAD_PP_TO_BITS(s) (((s) >> 4) & 0x3) +#define DIGITAL_PAYLOAD_BITS_TO_FSL(s) ((s) & 0x3) +#define DIGITAL_PAYLOAD_FSL_TO_BITS(s) ((s) & 0x3) + #define DIGITAL_GB_BIT 0x02 +#define DIGITAL_NFC_DEP_REQ_RES_HEADROOM 2 /* SoD: [SB (NFC-A)] + LEN */ +#define DIGITAL_NFC_DEP_REQ_RES_TAILROOM 2 /* EoD: 2-byte CRC */ + #define DIGITAL_NFC_DEP_PFB_TYPE(pfb) ((pfb) & 0xE0) #define DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT 0x10 +#define DIGITAL_NFC_DEP_PFB_MI_BIT 0x10 +#define DIGITAL_NFC_DEP_PFB_NACK_BIT 0x10 +#define DIGITAL_NFC_DEP_PFB_DID_BIT 0x04 #define DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb) \ ((pfb) & DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT) -#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb) ((pfb) & 0x10) +#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_MI_BIT) +#define DIGITAL_NFC_DEP_NACK_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_NACK_BIT) #define DIGITAL_NFC_DEP_NAD_BIT_SET(pfb) ((pfb) & 0x08) -#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & 0x04) +#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_DID_BIT) #define DIGITAL_NFC_DEP_PFB_PNI(pfb) ((pfb) & 0x03) #define DIGITAL_NFC_DEP_PFB_I_PDU 0x00 @@ -97,6 +112,34 @@ struct digital_dep_req_res { static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg, struct sk_buff *resp); +static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg, + struct sk_buff *resp); + +static const u8 digital_payload_bits_map[4] = { + [0] = 64, + [1] = 128, + [2] = 192, + [3] = 254 +}; + +static u8 digital_payload_bits_to_size(u8 payload_bits) +{ + if (payload_bits >= ARRAY_SIZE(digital_payload_bits_map)) + return 0; + + return digital_payload_bits_map[payload_bits]; +} + +static u8 digital_payload_size_to_bits(u8 payload_size) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(digital_payload_bits_map); i++) + if (digital_payload_bits_map[i] == payload_size) + return i; + + return 0xff; +} static void digital_skb_push_dep_sod(struct nfc_digital_dev *ddev, struct sk_buff *skb) @@ -129,6 +172,106 @@ static int digital_skb_pull_dep_sod(struct nfc_digital_dev *ddev, return 0; } +static struct sk_buff * +digital_send_dep_data_prep(struct nfc_digital_dev *ddev, struct sk_buff *skb, + struct digital_dep_req_res *dep_req_res, + struct digital_data_exch *data_exch) +{ + struct sk_buff *new_skb; + + if (skb->len > ddev->remote_payload_max) { + dep_req_res->pfb |= DIGITAL_NFC_DEP_PFB_MI_BIT; + + new_skb = digital_skb_alloc(ddev, ddev->remote_payload_max); + if (!new_skb) { + kfree_skb(ddev->chaining_skb); + ddev->chaining_skb = NULL; + + return ERR_PTR(-ENOMEM); + } + + skb_reserve(new_skb, ddev->tx_headroom + NFC_HEADER_SIZE + + DIGITAL_NFC_DEP_REQ_RES_HEADROOM); + memcpy(skb_put(new_skb, ddev->remote_payload_max), skb->data, + ddev->remote_payload_max); + skb_pull(skb, ddev->remote_payload_max); + + ddev->chaining_skb = skb; + ddev->data_exch = data_exch; + } else { + ddev->chaining_skb = NULL; + new_skb = skb; + } + + return new_skb; +} + +static struct sk_buff * +digital_recv_dep_data_gather(struct nfc_digital_dev *ddev, u8 pfb, + struct sk_buff *resp, + int (*send_ack)(struct nfc_digital_dev *ddev, + struct digital_data_exch + *data_exch), + struct digital_data_exch *data_exch) +{ + struct sk_buff *new_skb; + int rc; + + if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb) && (!ddev->chaining_skb)) { + ddev->chaining_skb = + nfc_alloc_recv_skb(8 * ddev->local_payload_max, + GFP_KERNEL); + if (!ddev->chaining_skb) { + rc = -ENOMEM; + goto error; + } + } + + if (ddev->chaining_skb) { + if (resp->len > skb_tailroom(ddev->chaining_skb)) { + new_skb = skb_copy_expand(ddev->chaining_skb, + skb_headroom( + ddev->chaining_skb), + 8 * ddev->local_payload_max, + GFP_KERNEL); + if (!new_skb) { + rc = -ENOMEM; + goto error; + } + + kfree_skb(ddev->chaining_skb); + ddev->chaining_skb = new_skb; + } + + memcpy(skb_put(ddev->chaining_skb, resp->len), resp->data, + resp->len); + + kfree_skb(resp); + resp = NULL; + + if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) { + rc = send_ack(ddev, data_exch); + if (rc) + goto error; + + return NULL; + } + + resp = ddev->chaining_skb; + ddev->chaining_skb = NULL; + } + + return resp; + +error: + kfree_skb(resp); + + kfree_skb(ddev->chaining_skb); + ddev->chaining_skb = NULL; + + return ERR_PTR(rc); +} + static void digital_in_recv_psl_res(struct nfc_digital_dev *ddev, void *arg, struct sk_buff *resp) { @@ -198,6 +341,8 @@ static int digital_in_send_psl_req(struct nfc_digital_dev *ddev, { struct sk_buff *skb; struct digital_psl_req *psl_req; + int rc; + u8 payload_size, payload_bits; skb = digital_skb_alloc(ddev, sizeof(*psl_req)); if (!skb) @@ -211,14 +356,24 @@ static int digital_in_send_psl_req(struct nfc_digital_dev *ddev, psl_req->cmd = DIGITAL_CMD_PSL_REQ; psl_req->did = 0; psl_req->brs = (0x2 << 3) | 0x2; /* 424F both directions */ - psl_req->fsl = DIGITAL_FSL_BITS_PAYLOAD_SIZE_254B; + + payload_size = min(ddev->local_payload_max, ddev->remote_payload_max); + payload_bits = digital_payload_size_to_bits(payload_size); + psl_req->fsl = DIGITAL_PAYLOAD_BITS_TO_FSL(payload_bits); + + ddev->local_payload_max = payload_size; + ddev->remote_payload_max = payload_size; digital_skb_push_dep_sod(ddev, skb); ddev->skb_add_crc(skb); - return digital_in_send_cmd(ddev, skb, 500, digital_in_recv_psl_res, - target); + rc = digital_in_send_cmd(ddev, skb, 500, digital_in_recv_psl_res, + target); + if (rc) + kfree_skb(skb); + + return rc; } static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg, @@ -226,7 +381,7 @@ static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg, { struct nfc_target *target = arg; struct digital_atr_res *atr_res; - u8 gb_len; + u8 gb_len, payload_bits; int rc; if (IS_ERR(resp)) { @@ -256,6 +411,14 @@ static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg, atr_res = (struct digital_atr_res *)resp->data; + payload_bits = DIGITAL_PAYLOAD_PP_TO_BITS(atr_res->pp); + ddev->remote_payload_max = digital_payload_bits_to_size(payload_bits); + + if (!ddev->remote_payload_max) { + rc = -EINVAL; + goto exit; + } + rc = nfc_set_remote_general_bytes(ddev->nfc_dev, atr_res->gb, gb_len); if (rc) goto exit; @@ -286,6 +449,8 @@ int digital_in_send_atr_req(struct nfc_digital_dev *ddev, struct sk_buff *skb; struct digital_atr_req *atr_req; uint size; + int rc; + u8 payload_bits; size = DIGITAL_ATR_REQ_MIN_SIZE + gb_len; @@ -314,7 +479,9 @@ int digital_in_send_atr_req(struct nfc_digital_dev *ddev, atr_req->bs = 0; atr_req->br = 0; - atr_req->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B; + ddev->local_payload_max = DIGITAL_PAYLOAD_SIZE_MAX; + payload_bits = digital_payload_size_to_bits(ddev->local_payload_max); + atr_req->pp = DIGITAL_PAYLOAD_BITS_TO_PP(payload_bits); if (gb_len) { atr_req->pp |= DIGITAL_GB_BIT; @@ -325,8 +492,113 @@ int digital_in_send_atr_req(struct nfc_digital_dev *ddev, ddev->skb_add_crc(skb); - return digital_in_send_cmd(ddev, skb, 500, digital_in_recv_atr_res, - target); + rc = digital_in_send_cmd(ddev, skb, 500, digital_in_recv_atr_res, + target); + if (rc) + kfree_skb(skb); + + return rc; +} + +static int digital_in_send_ack(struct nfc_digital_dev *ddev, + struct digital_data_exch *data_exch) +{ + struct digital_dep_req_res *dep_req; + struct sk_buff *skb; + int rc; + + skb = digital_skb_alloc(ddev, 1); + if (!skb) + return -ENOMEM; + + skb_push(skb, sizeof(struct digital_dep_req_res)); + + dep_req = (struct digital_dep_req_res *)skb->data; + + dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT; + dep_req->cmd = DIGITAL_CMD_DEP_REQ; + dep_req->pfb = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU | + ddev->curr_nfc_dep_pni; + + digital_skb_push_dep_sod(ddev, skb); + + ddev->skb_add_crc(skb); + + ddev->saved_skb = skb_get(skb); + ddev->saved_skb_len = skb->len; + + rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res, + data_exch); + if (rc) { + kfree_skb(skb); + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + } + + return rc; +} + +static int digital_in_send_nack(struct nfc_digital_dev *ddev, + struct digital_data_exch *data_exch) +{ + struct digital_dep_req_res *dep_req; + struct sk_buff *skb; + int rc; + + skb = digital_skb_alloc(ddev, 1); + if (!skb) + return -ENOMEM; + + skb_push(skb, sizeof(struct digital_dep_req_res)); + + dep_req = (struct digital_dep_req_res *)skb->data; + + dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT; + dep_req->cmd = DIGITAL_CMD_DEP_REQ; + dep_req->pfb = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU | + DIGITAL_NFC_DEP_PFB_NACK_BIT | ddev->curr_nfc_dep_pni; + + digital_skb_push_dep_sod(ddev, skb); + + ddev->skb_add_crc(skb); + + rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res, + data_exch); + if (rc) + kfree_skb(skb); + + return rc; +} + +static int digital_in_send_atn(struct nfc_digital_dev *ddev, + struct digital_data_exch *data_exch) +{ + struct digital_dep_req_res *dep_req; + struct sk_buff *skb; + int rc; + + skb = digital_skb_alloc(ddev, 1); + if (!skb) + return -ENOMEM; + + skb_push(skb, sizeof(struct digital_dep_req_res)); + + dep_req = (struct digital_dep_req_res *)skb->data; + + dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT; + dep_req->cmd = DIGITAL_CMD_DEP_REQ; + dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU; + + digital_skb_push_dep_sod(ddev, skb); + + ddev->skb_add_crc(skb); + + rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res, + data_exch); + if (rc) + kfree_skb(skb); + + return rc; } static int digital_in_send_rtox(struct nfc_digital_dev *ddev, @@ -355,12 +627,30 @@ static int digital_in_send_rtox(struct nfc_digital_dev *ddev, ddev->skb_add_crc(skb); + ddev->saved_skb = skb_get(skb); + ddev->saved_skb_len = skb->len; + rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res, data_exch); + if (rc) { + kfree_skb(skb); + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + } return rc; } +static int digital_in_send_saved_skb(struct nfc_digital_dev *ddev, + struct digital_data_exch *data_exch) +{ + skb_get(ddev->saved_skb); + skb_push(ddev->saved_skb, ddev->saved_skb_len); + + return digital_in_send_cmd(ddev, ddev->saved_skb, 1500, + digital_in_recv_dep_res, data_exch); +} + static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg, struct sk_buff *resp) { @@ -373,25 +663,67 @@ static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg, if (IS_ERR(resp)) { rc = PTR_ERR(resp); resp = NULL; + + if (((rc != -ETIMEDOUT) || ddev->nack_count) && + (ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) { + ddev->atn_count = 0; + + rc = digital_in_send_nack(ddev, data_exch); + if (rc) + goto error; + + return; + } else if ((rc == -ETIMEDOUT) && + (ddev->atn_count++ < DIGITAL_NFC_DEP_N_RETRY_ATN)) { + ddev->nack_count = 0; + + rc = digital_in_send_atn(ddev, data_exch); + if (rc) + goto error; + + return; + } + + goto exit; + } + + rc = digital_skb_pull_dep_sod(ddev, resp); + if (rc) { + PROTOCOL_ERR("14.4.1.2"); goto exit; } rc = ddev->skb_check_crc(resp); if (rc) { + if ((resp->len >= 4) && + (ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) { + ddev->atn_count = 0; + + rc = digital_in_send_nack(ddev, data_exch); + if (rc) + goto error; + + kfree_skb(resp); + + return; + } + PROTOCOL_ERR("14.4.1.6"); goto error; } - rc = digital_skb_pull_dep_sod(ddev, resp); - if (rc) { - PROTOCOL_ERR("14.4.1.2"); + ddev->atn_count = 0; + ddev->nack_count = 0; + + if (resp->len > ddev->local_payload_max) { + rc = -EMSGSIZE; goto exit; } + size = sizeof(struct digital_dep_req_res); dep_res = (struct digital_dep_req_res *)resp->data; - if (resp->len < sizeof(struct digital_dep_req_res) || - dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN || + if (resp->len < size || dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN || dep_res->cmd != DIGITAL_CMD_DEP_RES) { rc = -EIO; goto error; @@ -399,6 +731,24 @@ static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg, pfb = dep_res->pfb; + if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) { + PROTOCOL_ERR("14.8.2.1"); + rc = -EIO; + goto error; + } + + if (DIGITAL_NFC_DEP_NAD_BIT_SET(pfb)) { + rc = -EIO; + goto exit; + } + + if (size > resp->len) { + rc = -EIO; + goto error; + } + + skb_pull(resp, size); + switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) { case DIGITAL_NFC_DEP_PFB_I_PDU: if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) { @@ -409,21 +759,71 @@ static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg, ddev->curr_nfc_dep_pni = DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1); + + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + + resp = digital_recv_dep_data_gather(ddev, pfb, resp, + digital_in_send_ack, + data_exch); + if (IS_ERR(resp)) { + rc = PTR_ERR(resp); + resp = NULL; + goto error; + } + + /* If resp is NULL then we're still chaining so return and + * wait for the next part of the PDU. Else, the PDU is + * complete so pass it up. + */ + if (!resp) + return; + rc = 0; break; case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU: + if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) { + PROTOCOL_ERR("14.12.3.3"); + rc = -EIO; + goto exit; + } + + ddev->curr_nfc_dep_pni = + DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1); + + if (ddev->chaining_skb && !DIGITAL_NFC_DEP_NACK_BIT_SET(pfb)) { + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + + rc = digital_in_send_dep_req(ddev, NULL, + ddev->chaining_skb, + ddev->data_exch); + if (rc) + goto error; + + return; + } + pr_err("Received a ACK/NACK PDU\n"); - rc = -EIO; - goto error; + rc = -EINVAL; + goto exit; case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU: - if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) { - rc = -EINVAL; - goto error; + if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) { /* ATN */ + rc = digital_in_send_saved_skb(ddev, data_exch); + if (rc) { + kfree_skb(ddev->saved_skb); + goto error; + } + + return; } - rc = digital_in_send_rtox(ddev, data_exch, resp->data[3]); + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + + rc = digital_in_send_rtox(ddev, data_exch, resp->data[0]); if (rc) goto error; @@ -431,30 +831,18 @@ static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg, return; } - if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) { - pr_err("MI bit set. Chained PDU not supported\n"); - rc = -EIO; - goto error; - } - - size = sizeof(struct digital_dep_req_res); - - if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) - size++; - - if (size > resp->len) { - rc = -EIO; - goto error; - } - - skb_pull(resp, size); - exit: data_exch->cb(data_exch->cb_context, resp, rc); error: kfree(data_exch); + kfree_skb(ddev->chaining_skb); + ddev->chaining_skb = NULL; + + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + if (rc) kfree_skb(resp); } @@ -464,20 +852,47 @@ int digital_in_send_dep_req(struct nfc_digital_dev *ddev, struct digital_data_exch *data_exch) { struct digital_dep_req_res *dep_req; + struct sk_buff *chaining_skb, *tmp_skb; + int rc; skb_push(skb, sizeof(struct digital_dep_req_res)); dep_req = (struct digital_dep_req_res *)skb->data; + dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT; dep_req->cmd = DIGITAL_CMD_DEP_REQ; dep_req->pfb = ddev->curr_nfc_dep_pni; - digital_skb_push_dep_sod(ddev, skb); + ddev->atn_count = 0; + ddev->nack_count = 0; - ddev->skb_add_crc(skb); + chaining_skb = ddev->chaining_skb; + + tmp_skb = digital_send_dep_data_prep(ddev, skb, dep_req, data_exch); + if (IS_ERR(tmp_skb)) + return PTR_ERR(tmp_skb); + + digital_skb_push_dep_sod(ddev, tmp_skb); + + ddev->skb_add_crc(tmp_skb); - return digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res, - data_exch); + ddev->saved_skb = skb_get(tmp_skb); + ddev->saved_skb_len = tmp_skb->len; + + rc = digital_in_send_cmd(ddev, tmp_skb, 1500, digital_in_recv_dep_res, + data_exch); + if (rc) { + if (tmp_skb != skb) + kfree_skb(tmp_skb); + + kfree_skb(chaining_skb); + ddev->chaining_skb = NULL; + + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + } + + return rc; } static void digital_tg_set_rf_tech(struct nfc_digital_dev *ddev, u8 rf_tech) @@ -507,11 +922,106 @@ static void digital_tg_set_rf_tech(struct nfc_digital_dev *ddev, u8 rf_tech) } } +static int digital_tg_send_ack(struct nfc_digital_dev *ddev, + struct digital_data_exch *data_exch) +{ + struct digital_dep_req_res *dep_res; + struct sk_buff *skb; + int rc; + + skb = digital_skb_alloc(ddev, 1); + if (!skb) + return -ENOMEM; + + skb_push(skb, sizeof(struct digital_dep_req_res)); + + dep_res = (struct digital_dep_req_res *)skb->data; + + dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN; + dep_res->cmd = DIGITAL_CMD_DEP_RES; + dep_res->pfb = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU | + ddev->curr_nfc_dep_pni; + + if (ddev->did) { + dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT; + + memcpy(skb_put(skb, sizeof(ddev->did)), &ddev->did, + sizeof(ddev->did)); + } + + ddev->curr_nfc_dep_pni = + DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1); + + digital_skb_push_dep_sod(ddev, skb); + + ddev->skb_add_crc(skb); + + ddev->saved_skb = skb_get(skb); + ddev->saved_skb_len = skb->len; + + rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req, + data_exch); + if (rc) { + kfree_skb(skb); + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + } + + return rc; +} + +static int digital_tg_send_atn(struct nfc_digital_dev *ddev) +{ + struct digital_dep_req_res *dep_res; + struct sk_buff *skb; + int rc; + + skb = digital_skb_alloc(ddev, 1); + if (!skb) + return -ENOMEM; + + skb_push(skb, sizeof(struct digital_dep_req_res)); + + dep_res = (struct digital_dep_req_res *)skb->data; + + dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN; + dep_res->cmd = DIGITAL_CMD_DEP_RES; + dep_res->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU; + + if (ddev->did) { + dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT; + + memcpy(skb_put(skb, sizeof(ddev->did)), &ddev->did, + sizeof(ddev->did)); + } + + digital_skb_push_dep_sod(ddev, skb); + + ddev->skb_add_crc(skb); + + rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req, + NULL); + if (rc) + kfree_skb(skb); + + return rc; +} + +static int digital_tg_send_saved_skb(struct nfc_digital_dev *ddev) +{ + skb_get(ddev->saved_skb); + skb_push(ddev->saved_skb, ddev->saved_skb_len); + + return digital_tg_send_cmd(ddev, ddev->saved_skb, 1500, + digital_tg_recv_dep_req, NULL); +} + static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg, struct sk_buff *resp) { int rc; struct digital_dep_req_res *dep_req; + u8 pfb; size_t size; if (IS_ERR(resp)) { @@ -532,6 +1042,11 @@ static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg, goto exit; } + if (resp->len > ddev->local_payload_max) { + rc = -EMSGSIZE; + goto exit; + } + size = sizeof(struct digital_dep_req_res); dep_req = (struct digital_dep_req_res *)resp->data; @@ -541,34 +1056,147 @@ static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg, goto exit; } - if (DIGITAL_NFC_DEP_DID_BIT_SET(dep_req->pfb)) - size++; + pfb = dep_req->pfb; - if (resp->len < size) { + if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) { + if (ddev->did && (ddev->did == resp->data[3])) { + size++; + } else { + rc = -EIO; + goto exit; + } + } else if (ddev->did) { rc = -EIO; goto exit; } - switch (DIGITAL_NFC_DEP_PFB_TYPE(dep_req->pfb)) { + if (DIGITAL_NFC_DEP_NAD_BIT_SET(pfb)) { + rc = -EIO; + goto exit; + } + + if (size > resp->len) { + rc = -EIO; + goto exit; + } + + skb_pull(resp, size); + + switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) { case DIGITAL_NFC_DEP_PFB_I_PDU: pr_debug("DIGITAL_NFC_DEP_PFB_I_PDU\n"); - ddev->curr_nfc_dep_pni = DIGITAL_NFC_DEP_PFB_PNI(dep_req->pfb); + + if ((ddev->atn_count && (DIGITAL_NFC_DEP_PFB_PNI(pfb - 1) != + ddev->curr_nfc_dep_pni)) || + (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni)) { + PROTOCOL_ERR("14.12.3.4"); + rc = -EIO; + goto exit; + } + + if (ddev->atn_count) { + ddev->atn_count = 0; + + rc = digital_tg_send_saved_skb(ddev); + if (rc) + goto exit; + + return; + } + + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + + resp = digital_recv_dep_data_gather(ddev, pfb, resp, + digital_tg_send_ack, NULL); + if (IS_ERR(resp)) { + rc = PTR_ERR(resp); + resp = NULL; + goto exit; + } + + /* If resp is NULL then we're still chaining so return and + * wait for the next part of the PDU. Else, the PDU is + * complete so pass it up. + */ + if (!resp) + return; + + rc = 0; break; case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU: - pr_err("Received a ACK/NACK PDU\n"); - rc = -EINVAL; - goto exit; + if (!DIGITAL_NFC_DEP_NACK_BIT_SET(pfb)) { /* ACK */ + if ((ddev->atn_count && + (DIGITAL_NFC_DEP_PFB_PNI(pfb - 1) != + ddev->curr_nfc_dep_pni)) || + (DIGITAL_NFC_DEP_PFB_PNI(pfb) != + ddev->curr_nfc_dep_pni) || + !ddev->chaining_skb || !ddev->saved_skb) { + rc = -EIO; + goto exit; + } + + if (ddev->atn_count) { + ddev->atn_count = 0; + + rc = digital_tg_send_saved_skb(ddev); + if (rc) + goto exit; + + return; + } + + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + + rc = digital_tg_send_dep_res(ddev, ddev->chaining_skb); + if (rc) + goto exit; + } else { /* NACK */ + if ((DIGITAL_NFC_DEP_PFB_PNI(pfb + 1) != + ddev->curr_nfc_dep_pni) || + !ddev->saved_skb) { + rc = -EIO; + goto exit; + } + + ddev->atn_count = 0; + + rc = digital_tg_send_saved_skb(ddev); + if (rc) { + kfree_skb(ddev->saved_skb); + goto exit; + } + } + + return; case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU: - pr_err("Received a SUPERVISOR PDU\n"); - rc = -EINVAL; - goto exit; - } + if (DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) { + rc = -EINVAL; + goto exit; + } - skb_pull(resp, size); + rc = digital_tg_send_atn(ddev); + if (rc) + goto exit; + + ddev->atn_count++; + + kfree_skb(resp); + return; + } rc = nfc_tm_data_received(ddev->nfc_dev, resp); exit: + kfree_skb(ddev->chaining_skb); + ddev->chaining_skb = NULL; + + ddev->atn_count = 0; + + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + if (rc) kfree_skb(resp); } @@ -576,20 +1204,54 @@ exit: int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb) { struct digital_dep_req_res *dep_res; + struct sk_buff *chaining_skb, *tmp_skb; + int rc; skb_push(skb, sizeof(struct digital_dep_req_res)); + dep_res = (struct digital_dep_req_res *)skb->data; dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN; dep_res->cmd = DIGITAL_CMD_DEP_RES; dep_res->pfb = ddev->curr_nfc_dep_pni; - digital_skb_push_dep_sod(ddev, skb); + if (ddev->did) { + dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT; - ddev->skb_add_crc(skb); + memcpy(skb_put(skb, sizeof(ddev->did)), &ddev->did, + sizeof(ddev->did)); + } + + ddev->curr_nfc_dep_pni = + DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1); + + chaining_skb = ddev->chaining_skb; + + tmp_skb = digital_send_dep_data_prep(ddev, skb, dep_res, NULL); + if (IS_ERR(tmp_skb)) + return PTR_ERR(tmp_skb); + + digital_skb_push_dep_sod(ddev, tmp_skb); + + ddev->skb_add_crc(tmp_skb); + + ddev->saved_skb = skb_get(tmp_skb); + ddev->saved_skb_len = tmp_skb->len; + + rc = digital_tg_send_cmd(ddev, tmp_skb, 1500, digital_tg_recv_dep_req, + NULL); + if (rc) { + if (tmp_skb != skb) + kfree_skb(tmp_skb); + + kfree_skb(chaining_skb); + ddev->chaining_skb = NULL; - return digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req, - NULL); + kfree_skb(ddev->saved_skb); + ddev->saved_skb = NULL; + } + + return rc; } static void digital_tg_send_psl_res_complete(struct nfc_digital_dev *ddev, @@ -632,9 +1294,10 @@ static int digital_tg_send_psl_res(struct nfc_digital_dev *ddev, u8 did, ddev->skb_add_crc(skb); + ddev->curr_nfc_dep_pni = 0; + rc = digital_tg_send_cmd(ddev, skb, 0, digital_tg_send_psl_res_complete, (void *)(unsigned long)rf_tech); - if (rc) kfree_skb(skb); @@ -647,7 +1310,7 @@ static void digital_tg_recv_psl_req(struct nfc_digital_dev *ddev, void *arg, int rc; struct digital_psl_req *psl_req; u8 rf_tech; - u8 dsi; + u8 dsi, payload_size, payload_bits; if (IS_ERR(resp)) { rc = PTR_ERR(resp); @@ -692,6 +1355,18 @@ static void digital_tg_recv_psl_req(struct nfc_digital_dev *ddev, void *arg, goto exit; } + payload_bits = DIGITAL_PAYLOAD_FSL_TO_BITS(psl_req->fsl); + payload_size = digital_payload_bits_to_size(payload_bits); + + if (!payload_size || (payload_size > min(ddev->local_payload_max, + ddev->remote_payload_max))) { + rc = -EINVAL; + goto exit; + } + + ddev->local_payload_max = payload_size; + ddev->remote_payload_max = payload_size; + rc = digital_tg_send_psl_res(ddev, psl_req->did, rf_tech); exit: @@ -712,6 +1387,8 @@ static void digital_tg_send_atr_res_complete(struct nfc_digital_dev *ddev, if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB) offset++; + ddev->atn_count = 0; + if (resp->data[offset] == DIGITAL_CMD_PSL_REQ) digital_tg_recv_psl_req(ddev, arg, resp); else @@ -723,7 +1400,7 @@ static int digital_tg_send_atr_res(struct nfc_digital_dev *ddev, { struct digital_atr_res *atr_res; struct sk_buff *skb; - u8 *gb; + u8 *gb, payload_bits; size_t gb_len; int rc; @@ -744,7 +1421,11 @@ static int digital_tg_send_atr_res(struct nfc_digital_dev *ddev, atr_res->cmd = DIGITAL_CMD_ATR_RES; memcpy(atr_res->nfcid3, atr_req->nfcid3, sizeof(atr_req->nfcid3)); atr_res->to = 8; - atr_res->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B; + + ddev->local_payload_max = DIGITAL_PAYLOAD_SIZE_MAX; + payload_bits = digital_payload_size_to_bits(ddev->local_payload_max); + atr_res->pp = DIGITAL_PAYLOAD_BITS_TO_PP(payload_bits); + if (gb_len) { skb_put(skb, gb_len); @@ -756,12 +1437,12 @@ static int digital_tg_send_atr_res(struct nfc_digital_dev *ddev, ddev->skb_add_crc(skb); + ddev->curr_nfc_dep_pni = 0; + rc = digital_tg_send_cmd(ddev, skb, 999, digital_tg_send_atr_res_complete, NULL); - if (rc) { + if (rc) kfree_skb(skb); - return rc; - } return rc; } @@ -772,7 +1453,7 @@ void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg, int rc; struct digital_atr_req *atr_req; size_t gb_len, min_size; - u8 poll_tech_count; + u8 poll_tech_count, payload_bits; if (IS_ERR(resp)) { rc = PTR_ERR(resp); @@ -815,11 +1496,22 @@ void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg, atr_req = (struct digital_atr_req *)resp->data; if (atr_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT || - atr_req->cmd != DIGITAL_CMD_ATR_REQ) { + atr_req->cmd != DIGITAL_CMD_ATR_REQ || + atr_req->did > DIGITAL_DID_MAX) { rc = -EINVAL; goto exit; } + payload_bits = DIGITAL_PAYLOAD_PP_TO_BITS(atr_req->pp); + ddev->remote_payload_max = digital_payload_bits_to_size(payload_bits); + + if (!ddev->remote_payload_max) { + rc = -EINVAL; + goto exit; + } + + ddev->did = atr_req->did; + rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING, NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED); if (rc) diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c index 677d24bb70f8..91df487aa0a9 100644 --- a/net/nfc/hci/command.c +++ b/net/nfc/hci/command.c @@ -345,6 +345,9 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate, pr_debug("\n"); + if (hdev->gate2pipe[dest_gate] == NFC_HCI_DO_NOT_CREATE_PIPE) + return 0; + if (hdev->gate2pipe[dest_gate] != NFC_HCI_INVALID_PIPE) return -EADDRINUSE; diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c index 117708263ced..ef50e7716c4a 100644 --- a/net/nfc/hci/core.c +++ b/net/nfc/hci/core.c @@ -167,6 +167,48 @@ exit: void nfc_hci_cmd_received(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd, struct sk_buff *skb) { + int r = 0; + u8 gate = nfc_hci_pipe2gate(hdev, pipe); + u8 local_gate, new_pipe; + u8 gate_opened = 0x00; + + pr_debug("from gate %x pipe %x cmd %x\n", gate, pipe, cmd); + + switch (cmd) { + case NFC_HCI_ADM_NOTIFY_PIPE_CREATED: + if (skb->len != 5) { + r = -EPROTO; + break; + } + + local_gate = skb->data[3]; + new_pipe = skb->data[4]; + nfc_hci_send_response(hdev, gate, NFC_HCI_ANY_OK, NULL, 0); + + /* save the new created pipe and bind with local gate, + * the description for skb->data[3] is destination gate id + * but since we received this cmd from host controller, we + * are the destination and it is our local gate + */ + hdev->gate2pipe[local_gate] = new_pipe; + break; + case NFC_HCI_ANY_OPEN_PIPE: + /* if the pipe is already created, we allow remote host to + * open it + */ + if (gate != 0xff) + nfc_hci_send_response(hdev, gate, NFC_HCI_ANY_OK, + &gate_opened, 1); + break; + case NFC_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED: + nfc_hci_send_response(hdev, gate, NFC_HCI_ANY_OK, NULL, 0); + break; + default: + pr_info("Discarded unknown cmd %x to gate %x\n", cmd, gate); + r = -EINVAL; + break; + } + kfree_skb(skb); } @@ -717,6 +759,19 @@ static int hci_disable_se(struct nfc_dev *nfc_dev, u32 se_idx) return 0; } +static int hci_se_io(struct nfc_dev *nfc_dev, u32 se_idx, + u8 *apdu, size_t apdu_length, + se_io_cb_t cb, void *cb_context) +{ + struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); + + if (hdev->ops->se_io) + return hdev->ops->se_io(hdev, se_idx, apdu, + apdu_length, cb, cb_context); + + return 0; +} + static void nfc_hci_failure(struct nfc_hci_dev *hdev, int err) { mutex_lock(&hdev->msg_tx_mutex); @@ -830,6 +885,7 @@ static struct nfc_ops hci_nfc_ops = { .discover_se = hci_discover_se, .enable_se = hci_enable_se, .disable_se = hci_disable_se, + .se_io = hci_se_io, }; struct nfc_hci_dev *nfc_hci_allocate_device(struct nfc_hci_ops *ops, diff --git a/net/nfc/llcp_commands.c b/net/nfc/llcp_commands.c index c4da0c2d8a14..3621a902cb6e 100644 --- a/net/nfc/llcp_commands.c +++ b/net/nfc/llcp_commands.c @@ -401,7 +401,8 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) u8 *miux_tlv = NULL, miux_tlv_length; u8 *rw_tlv = NULL, rw_tlv_length, rw; int err; - u16 size = 0, miux; + u16 size = 0; + __be16 miux; pr_debug("Sending CONNECT\n"); @@ -465,7 +466,8 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) u8 *miux_tlv = NULL, miux_tlv_length; u8 *rw_tlv = NULL, rw_tlv_length, rw; int err; - u16 size = 0, miux; + u16 size = 0; + __be16 miux; pr_debug("Sending CC\n"); diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c index 51e788797317..b18f07ccb504 100644 --- a/net/nfc/llcp_core.c +++ b/net/nfc/llcp_core.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2011 Intel Corporation. All rights reserved. + * Copyright (C) 2014 Marvell International Ltd. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -1511,8 +1512,10 @@ int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb) struct nfc_llcp_local *local; local = nfc_llcp_find_local(dev); - if (local == NULL) + if (local == NULL) { + kfree_skb(skb); return -ENODEV; + } __nfc_llcp_recv(local, skb); diff --git a/net/nfc/llcp_sock.c b/net/nfc/llcp_sock.c index 83bc785d5855..e181e290427c 100644 --- a/net/nfc/llcp_sock.c +++ b/net/nfc/llcp_sock.c @@ -524,13 +524,13 @@ static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr, static inline unsigned int llcp_accept_poll(struct sock *parent) { - struct nfc_llcp_sock *llcp_sock, *n, *parent_sock; + struct nfc_llcp_sock *llcp_sock, *parent_sock; struct sock *sk; parent_sock = nfc_llcp_sock(parent); - list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue, - accept_queue) { + list_for_each_entry(llcp_sock, &parent_sock->accept_queue, + accept_queue) { sk = &llcp_sock->sk; if (sk->sk_state == LLCP_CONNECTED) diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c index 90b16cb40058..51feb5e63008 100644 --- a/net/nfc/nci/core.c +++ b/net/nfc/nci/core.c @@ -3,6 +3,7 @@ * NFC Controller (NFCC) and a Device Host (DH). * * Copyright (C) 2011 Texas Instruments, Inc. + * Copyright (C) 2014 Marvell International Ltd. * * Written by Ilan Elias <ilane@ti.com> * @@ -196,18 +197,24 @@ static void nci_set_config_req(struct nci_dev *ndev, unsigned long opt) nci_send_cmd(ndev, NCI_OP_CORE_SET_CONFIG_CMD, (3 + param->len), &cmd); } +struct nci_rf_discover_param { + __u32 im_protocols; + __u32 tm_protocols; +}; + static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) { + struct nci_rf_discover_param *param = + (struct nci_rf_discover_param *)opt; struct nci_rf_disc_cmd cmd; - __u32 protocols = opt; cmd.num_disc_configs = 0; if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && - (protocols & NFC_PROTO_JEWEL_MASK || - protocols & NFC_PROTO_MIFARE_MASK || - protocols & NFC_PROTO_ISO14443_MASK || - protocols & NFC_PROTO_NFC_DEP_MASK)) { + (param->im_protocols & NFC_PROTO_JEWEL_MASK || + param->im_protocols & NFC_PROTO_MIFARE_MASK || + param->im_protocols & NFC_PROTO_ISO14443_MASK || + param->im_protocols & NFC_PROTO_NFC_DEP_MASK)) { cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = NCI_NFC_A_PASSIVE_POLL_MODE; cmd.disc_configs[cmd.num_disc_configs].frequency = 1; @@ -215,7 +222,7 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) } if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && - (protocols & NFC_PROTO_ISO14443_B_MASK)) { + (param->im_protocols & NFC_PROTO_ISO14443_B_MASK)) { cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = NCI_NFC_B_PASSIVE_POLL_MODE; cmd.disc_configs[cmd.num_disc_configs].frequency = 1; @@ -223,8 +230,8 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) } if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && - (protocols & NFC_PROTO_FELICA_MASK || - protocols & NFC_PROTO_NFC_DEP_MASK)) { + (param->im_protocols & NFC_PROTO_FELICA_MASK || + param->im_protocols & NFC_PROTO_NFC_DEP_MASK)) { cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = NCI_NFC_F_PASSIVE_POLL_MODE; cmd.disc_configs[cmd.num_disc_configs].frequency = 1; @@ -232,13 +239,25 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) } if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && - (protocols & NFC_PROTO_ISO15693_MASK)) { + (param->im_protocols & NFC_PROTO_ISO15693_MASK)) { cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = NCI_NFC_V_PASSIVE_POLL_MODE; cmd.disc_configs[cmd.num_disc_configs].frequency = 1; cmd.num_disc_configs++; } + if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS - 1) && + (param->tm_protocols & NFC_PROTO_NFC_DEP_MASK)) { + cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = + NCI_NFC_A_PASSIVE_LISTEN_MODE; + cmd.disc_configs[cmd.num_disc_configs].frequency = 1; + cmd.num_disc_configs++; + cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = + NCI_NFC_F_PASSIVE_LISTEN_MODE; + cmd.disc_configs[cmd.num_disc_configs].frequency = 1; + cmd.num_disc_configs++; + } + nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_CMD, (1 + (cmd.num_disc_configs * sizeof(struct disc_config))), &cmd); @@ -280,7 +299,7 @@ static void nci_rf_deactivate_req(struct nci_dev *ndev, unsigned long opt) { struct nci_rf_deactivate_cmd cmd; - cmd.type = NCI_DEACTIVATE_TYPE_IDLE_MODE; + cmd.type = opt; nci_send_cmd(ndev, NCI_OP_RF_DEACTIVATE_CMD, sizeof(struct nci_rf_deactivate_cmd), &cmd); @@ -441,6 +460,7 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev) { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); struct nci_set_config_param param; + int rc; param.val = nfc_get_local_general_bytes(nfc_dev, ¶m.len); if ((param.val == NULL) || (param.len == 0)) @@ -451,14 +471,45 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev) param.id = NCI_PN_ATR_REQ_GEN_BYTES; + rc = nci_request(ndev, nci_set_config_req, (unsigned long)¶m, + msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT)); + if (rc) + return rc; + + param.id = NCI_LN_ATR_RES_GEN_BYTES; + return nci_request(ndev, nci_set_config_req, (unsigned long)¶m, msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT)); } +static int nci_set_listen_parameters(struct nfc_dev *nfc_dev) +{ + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + int rc; + __u8 val; + + val = NCI_LA_SEL_INFO_NFC_DEP_MASK; + + rc = nci_set_config(ndev, NCI_LA_SEL_INFO, 1, &val); + if (rc) + return rc; + + val = NCI_LF_PROTOCOL_TYPE_NFC_DEP_MASK; + + rc = nci_set_config(ndev, NCI_LF_PROTOCOL_TYPE, 1, &val); + if (rc) + return rc; + + val = NCI_LF_CON_BITR_F_212 | NCI_LF_CON_BITR_F_424; + + return nci_set_config(ndev, NCI_LF_CON_BITR_F, 1, &val); +} + static int nci_start_poll(struct nfc_dev *nfc_dev, __u32 im_protocols, __u32 tm_protocols) { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + struct nci_rf_discover_param param; int rc; if ((atomic_read(&ndev->state) == NCI_DISCOVERY) || @@ -476,13 +527,14 @@ static int nci_start_poll(struct nfc_dev *nfc_dev, (atomic_read(&ndev->state) == NCI_POLL_ACTIVE)) { pr_debug("target active or w4 select, implicitly deactivate\n"); - rc = nci_request(ndev, nci_rf_deactivate_req, 0, + rc = nci_request(ndev, nci_rf_deactivate_req, + NCI_DEACTIVATE_TYPE_IDLE_MODE, msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT)); if (rc) return -EBUSY; } - if (im_protocols & NFC_PROTO_NFC_DEP_MASK) { + if ((im_protocols | tm_protocols) & NFC_PROTO_NFC_DEP_MASK) { rc = nci_set_local_general_bytes(nfc_dev); if (rc) { pr_err("failed to set local general bytes\n"); @@ -490,7 +542,15 @@ static int nci_start_poll(struct nfc_dev *nfc_dev, } } - rc = nci_request(ndev, nci_rf_discover_req, im_protocols, + if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) { + rc = nci_set_listen_parameters(nfc_dev); + if (rc) + pr_err("failed to set listen parameters\n"); + } + + param.im_protocols = im_protocols; + param.tm_protocols = tm_protocols; + rc = nci_request(ndev, nci_rf_discover_req, (unsigned long)¶m, msecs_to_jiffies(NCI_RF_DISC_TIMEOUT)); if (!rc) @@ -509,7 +569,7 @@ static void nci_stop_poll(struct nfc_dev *nfc_dev) return; } - nci_request(ndev, nci_rf_deactivate_req, 0, + nci_request(ndev, nci_rf_deactivate_req, NCI_DEACTIVATE_TYPE_IDLE_MODE, msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT)); } @@ -594,7 +654,8 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev, ndev->target_active_prot = 0; if (atomic_read(&ndev->state) == NCI_POLL_ACTIVE) { - nci_request(ndev, nci_rf_deactivate_req, 0, + nci_request(ndev, nci_rf_deactivate_req, + NCI_DEACTIVATE_TYPE_SLEEP_MODE, msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT)); } } @@ -622,9 +683,24 @@ static int nci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target, static int nci_dep_link_down(struct nfc_dev *nfc_dev) { + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + int rc; + pr_debug("entry\n"); - nci_deactivate_target(nfc_dev, NULL); + if (nfc_dev->rf_mode == NFC_RF_INITIATOR) { + nci_deactivate_target(nfc_dev, NULL); + } else { + if (atomic_read(&ndev->state) == NCI_LISTEN_ACTIVE || + atomic_read(&ndev->state) == NCI_DISCOVERY) { + nci_request(ndev, nci_rf_deactivate_req, 0, + msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT)); + } + + rc = nfc_tm_deactivated(nfc_dev); + if (rc) + pr_err("error when signaling tm deactivation\n"); + } return 0; } @@ -658,18 +734,58 @@ static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target, return rc; } +static int nci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb) +{ + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + int rc; + + rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb); + if (rc) + pr_err("unable to send data\n"); + + return rc; +} + static int nci_enable_se(struct nfc_dev *nfc_dev, u32 se_idx) { + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + + if (ndev->ops->enable_se) + return ndev->ops->enable_se(ndev, se_idx); + return 0; } static int nci_disable_se(struct nfc_dev *nfc_dev, u32 se_idx) { + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + + if (ndev->ops->disable_se) + return ndev->ops->disable_se(ndev, se_idx); + return 0; } static int nci_discover_se(struct nfc_dev *nfc_dev) { + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + + if (ndev->ops->discover_se) + return ndev->ops->discover_se(ndev); + + return 0; +} + +static int nci_se_io(struct nfc_dev *nfc_dev, u32 se_idx, + u8 *apdu, size_t apdu_length, + se_io_cb_t cb, void *cb_context) +{ + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + + if (ndev->ops->se_io) + return ndev->ops->se_io(ndev, se_idx, apdu, + apdu_length, cb, cb_context); + return 0; } @@ -683,9 +799,11 @@ static struct nfc_ops nci_nfc_ops = { .activate_target = nci_activate_target, .deactivate_target = nci_deactivate_target, .im_transceive = nci_transceive, + .tm_send = nci_tm_send, .enable_se = nci_enable_se, .disable_se = nci_disable_se, .discover_se = nci_discover_se, + .se_io = nci_se_io, }; /* ---- Interface to NCI drivers ---- */ diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c index 427ef2c7ab68..a2de2a8cb00e 100644 --- a/net/nfc/nci/data.c +++ b/net/nfc/nci/data.c @@ -3,6 +3,7 @@ * NFC Controller (NFCC) and a Device Host (DH). * * Copyright (C) 2011 Texas Instruments, Inc. + * Copyright (C) 2014 Marvell International Ltd. * * Written by Ilan Elias <ilane@ti.com> * @@ -184,11 +185,16 @@ exit: static void nci_add_rx_data_frag(struct nci_dev *ndev, struct sk_buff *skb, - __u8 pbf) + __u8 pbf, __u8 status) { int reassembly_len; int err = 0; + if (status) { + err = status; + goto exit; + } + if (ndev->rx_data_reassembly) { reassembly_len = ndev->rx_data_reassembly->len; @@ -223,13 +229,24 @@ static void nci_add_rx_data_frag(struct nci_dev *ndev, } exit: - nci_data_exchange_complete(ndev, skb, err); + if (ndev->nfc_dev->rf_mode == NFC_RF_INITIATOR) { + nci_data_exchange_complete(ndev, skb, err); + } else if (ndev->nfc_dev->rf_mode == NFC_RF_TARGET) { + /* Data received in Target mode, forward to nfc core */ + err = nfc_tm_data_received(ndev->nfc_dev, skb); + if (err) + pr_err("unable to handle received data\n"); + } else { + pr_err("rf mode unknown\n"); + kfree_skb(skb); + } } /* Rx Data packet */ void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb) { __u8 pbf = nci_pbf(skb->data); + __u8 status = 0; pr_debug("len %d\n", skb->len); @@ -247,8 +264,9 @@ void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb) ndev->target_active_prot == NFC_PROTO_ISO15693) { /* frame I/F => remove the status byte */ pr_debug("frame I/F => remove the status byte\n"); + status = skb->data[skb->len - 1]; skb_trim(skb, (skb->len - 1)); } - nci_add_rx_data_frag(ndev, skb, pbf); + nci_add_rx_data_frag(ndev, skb, pbf, nci_to_errno(status)); } diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c index 205b35f666db..22e453cb787d 100644 --- a/net/nfc/nci/ntf.c +++ b/net/nfc/nci/ntf.c @@ -103,7 +103,7 @@ static __u8 *nci_extract_rf_params_nfca_passive_poll(struct nci_dev *ndev, struct rf_tech_specific_params_nfca_poll *nfca_poll, __u8 *data) { - nfca_poll->sens_res = __le16_to_cpu(*((__u16 *)data)); + nfca_poll->sens_res = __le16_to_cpu(*((__le16 *)data)); data += 2; nfca_poll->nfcid1_len = min_t(__u8, *data++, NFC_NFCID1_MAXSIZE); @@ -167,7 +167,19 @@ static __u8 *nci_extract_rf_params_nfcv_passive_poll(struct nci_dev *ndev, return data; } -__u32 nci_get_prop_rf_protocol(struct nci_dev *ndev, __u8 rf_protocol) +static __u8 *nci_extract_rf_params_nfcf_passive_listen(struct nci_dev *ndev, + struct rf_tech_specific_params_nfcf_listen *nfcf_listen, + __u8 *data) +{ + nfcf_listen->local_nfcid2_len = min_t(__u8, *data++, + NFC_NFCID2_MAXSIZE); + memcpy(nfcf_listen->local_nfcid2, data, nfcf_listen->local_nfcid2_len); + data += nfcf_listen->local_nfcid2_len; + + return data; +} + +static __u32 nci_get_prop_rf_protocol(struct nci_dev *ndev, __u8 rf_protocol) { if (ndev->ops->get_rfprotocol) return ndev->ops->get_rfprotocol(ndev, rf_protocol); @@ -401,17 +413,29 @@ static int nci_extract_activation_params_nfc_dep(struct nci_dev *ndev, struct nci_rf_intf_activated_ntf *ntf, __u8 *data) { struct activation_params_poll_nfc_dep *poll; + struct activation_params_listen_nfc_dep *listen; switch (ntf->activation_rf_tech_and_mode) { case NCI_NFC_A_PASSIVE_POLL_MODE: case NCI_NFC_F_PASSIVE_POLL_MODE: poll = &ntf->activation_params.poll_nfc_dep; - poll->atr_res_len = min_t(__u8, *data++, 63); + poll->atr_res_len = min_t(__u8, *data++, + NFC_ATR_RES_MAXSIZE - 2); pr_debug("atr_res_len %d\n", poll->atr_res_len); if (poll->atr_res_len > 0) memcpy(poll->atr_res, data, poll->atr_res_len); break; + case NCI_NFC_A_PASSIVE_LISTEN_MODE: + case NCI_NFC_F_PASSIVE_LISTEN_MODE: + listen = &ntf->activation_params.listen_nfc_dep; + listen->atr_req_len = min_t(__u8, *data++, + NFC_ATR_REQ_MAXSIZE - 2); + pr_debug("atr_req_len %d\n", listen->atr_req_len); + if (listen->atr_req_len > 0) + memcpy(listen->atr_req, data, listen->atr_req_len); + break; + default: pr_err("unsupported activation_rf_tech_and_mode 0x%x\n", ntf->activation_rf_tech_and_mode); @@ -444,6 +468,48 @@ static void nci_target_auto_activated(struct nci_dev *ndev, nfc_targets_found(ndev->nfc_dev, ndev->targets, ndev->n_targets); } +static int nci_store_general_bytes_nfc_dep(struct nci_dev *ndev, + struct nci_rf_intf_activated_ntf *ntf) +{ + ndev->remote_gb_len = 0; + + if (ntf->activation_params_len <= 0) + return NCI_STATUS_OK; + + switch (ntf->activation_rf_tech_and_mode) { + case NCI_NFC_A_PASSIVE_POLL_MODE: + case NCI_NFC_F_PASSIVE_POLL_MODE: + ndev->remote_gb_len = min_t(__u8, + (ntf->activation_params.poll_nfc_dep.atr_res_len + - NFC_ATR_RES_GT_OFFSET), + NFC_ATR_RES_GB_MAXSIZE); + memcpy(ndev->remote_gb, + (ntf->activation_params.poll_nfc_dep.atr_res + + NFC_ATR_RES_GT_OFFSET), + ndev->remote_gb_len); + break; + + case NCI_NFC_A_PASSIVE_LISTEN_MODE: + case NCI_NFC_F_PASSIVE_LISTEN_MODE: + ndev->remote_gb_len = min_t(__u8, + (ntf->activation_params.listen_nfc_dep.atr_req_len + - NFC_ATR_REQ_GT_OFFSET), + NFC_ATR_REQ_GB_MAXSIZE); + memcpy(ndev->remote_gb, + (ntf->activation_params.listen_nfc_dep.atr_req + + NFC_ATR_REQ_GT_OFFSET), + ndev->remote_gb_len); + break; + + default: + pr_err("unsupported activation_rf_tech_and_mode 0x%x\n", + ntf->activation_rf_tech_and_mode); + return NCI_STATUS_RF_PROTOCOL_ERROR; + } + + return NCI_STATUS_OK; +} + static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb) { @@ -493,6 +559,16 @@ static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev, &(ntf.rf_tech_specific_params.nfcv_poll), data); break; + case NCI_NFC_A_PASSIVE_LISTEN_MODE: + /* no RF technology specific parameters */ + break; + + case NCI_NFC_F_PASSIVE_LISTEN_MODE: + data = nci_extract_rf_params_nfcf_passive_listen(ndev, + &(ntf.rf_tech_specific_params.nfcf_listen), + data); + break; + default: pr_err("unsupported activation_rf_tech_and_mode 0x%x\n", ntf.activation_rf_tech_and_mode); @@ -546,32 +622,39 @@ exit: /* store general bytes to be reported later in dep_link_up */ if (ntf.rf_interface == NCI_RF_INTERFACE_NFC_DEP) { - ndev->remote_gb_len = 0; - - if (ntf.activation_params_len > 0) { - /* ATR_RES general bytes at offset 15 */ - ndev->remote_gb_len = min_t(__u8, - (ntf.activation_params - .poll_nfc_dep.atr_res_len - - NFC_ATR_RES_GT_OFFSET), - NFC_MAX_GT_LEN); - memcpy(ndev->remote_gb, - (ntf.activation_params.poll_nfc_dep - .atr_res + NFC_ATR_RES_GT_OFFSET), - ndev->remote_gb_len); - } + err = nci_store_general_bytes_nfc_dep(ndev, &ntf); + if (err != NCI_STATUS_OK) + pr_err("unable to store general bytes\n"); } } - if (atomic_read(&ndev->state) == NCI_DISCOVERY) { - /* A single target was found and activated automatically */ - atomic_set(&ndev->state, NCI_POLL_ACTIVE); - if (err == NCI_STATUS_OK) - nci_target_auto_activated(ndev, &ntf); - } else { /* ndev->state == NCI_W4_HOST_SELECT */ - /* A selected target was activated, so complete the request */ - atomic_set(&ndev->state, NCI_POLL_ACTIVE); - nci_req_complete(ndev, err); + if (!(ntf.activation_rf_tech_and_mode & NCI_RF_TECH_MODE_LISTEN_MASK)) { + /* Poll mode */ + if (atomic_read(&ndev->state) == NCI_DISCOVERY) { + /* A single target was found and activated + * automatically */ + atomic_set(&ndev->state, NCI_POLL_ACTIVE); + if (err == NCI_STATUS_OK) + nci_target_auto_activated(ndev, &ntf); + } else { /* ndev->state == NCI_W4_HOST_SELECT */ + /* A selected target was activated, so complete the + * request */ + atomic_set(&ndev->state, NCI_POLL_ACTIVE); + nci_req_complete(ndev, err); + } + } else { + /* Listen mode */ + atomic_set(&ndev->state, NCI_LISTEN_ACTIVE); + if (err == NCI_STATUS_OK && + ntf.rf_protocol == NCI_RF_PROTOCOL_NFC_DEP) { + err = nfc_tm_activated(ndev->nfc_dev, + NFC_PROTO_NFC_DEP_MASK, + NFC_COMM_PASSIVE, + ndev->remote_gb, + ndev->remote_gb_len); + if (err != NCI_STATUS_OK) + pr_err("error when signaling tm activation\n"); + } } } @@ -595,8 +678,21 @@ static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev, if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags)) nci_data_exchange_complete(ndev, NULL, -EIO); - nci_clear_target_list(ndev); - atomic_set(&ndev->state, NCI_IDLE); + switch (ntf->type) { + case NCI_DEACTIVATE_TYPE_IDLE_MODE: + nci_clear_target_list(ndev); + atomic_set(&ndev->state, NCI_IDLE); + break; + case NCI_DEACTIVATE_TYPE_SLEEP_MODE: + case NCI_DEACTIVATE_TYPE_SLEEP_AF_MODE: + atomic_set(&ndev->state, NCI_W4_HOST_SELECT); + break; + case NCI_DEACTIVATE_TYPE_DISCOVERY: + nci_clear_target_list(ndev); + atomic_set(&ndev->state, NCI_DISCOVERY); + break; + } + nci_req_complete(ndev, NCI_STATUS_OK); } diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index 43cb1c17e267..44989fc8cddf 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -810,6 +810,31 @@ out: return rc; } +static int nfc_genl_activate_target(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + u32 device_idx, target_idx, protocol; + int rc; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) + return -EINVAL; + + device_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(device_idx); + if (!dev) + return -ENODEV; + + target_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]); + protocol = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]); + + nfc_deactivate_target(dev, target_idx); + rc = nfc_activate_target(dev, target_idx, protocol); + + nfc_put_device(dev); + return 0; +} + static int nfc_genl_dep_link_up(struct sk_buff *skb, struct genl_info *info) { struct nfc_dev *dev; @@ -1285,6 +1310,51 @@ static int nfc_genl_dump_ses_done(struct netlink_callback *cb) return 0; } +static int nfc_se_io(struct nfc_dev *dev, u32 se_idx, + u8 *apdu, size_t apdu_length, + se_io_cb_t cb, void *cb_context) +{ + struct nfc_se *se; + int rc; + + pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx); + + device_lock(&dev->dev); + + if (!device_is_registered(&dev->dev)) { + rc = -ENODEV; + goto error; + } + + if (!dev->dev_up) { + rc = -ENODEV; + goto error; + } + + if (!dev->ops->se_io) { + rc = -EOPNOTSUPP; + goto error; + } + + se = nfc_find_se(dev, se_idx); + if (!se) { + rc = -EINVAL; + goto error; + } + + if (se->state != NFC_SE_ENABLED) { + rc = -ENODEV; + goto error; + } + + rc = dev->ops->se_io(dev, se_idx, apdu, + apdu_length, cb, cb_context); + +error: + device_unlock(&dev->dev); + return rc; +} + struct se_io_ctx { u32 dev_idx; u32 se_idx; @@ -1367,7 +1437,7 @@ static int nfc_genl_se_io(struct sk_buff *skb, struct genl_info *info) ctx->dev_idx = dev_idx; ctx->se_idx = se_idx; - return dev->ops->se_io(dev, se_idx, apdu, apdu_len, se_io_cb, ctx); + return nfc_se_io(dev, se_idx, apdu, apdu_len, se_io_cb, ctx); } static const struct genl_ops nfc_genl_ops[] = { @@ -1455,6 +1525,11 @@ static const struct genl_ops nfc_genl_ops[] = { .doit = nfc_genl_se_io, .policy = nfc_genl_policy, }, + { + .cmd = NFC_CMD_ACTIVATE_TARGET, + .doit = nfc_genl_activate_target, + .policy = nfc_genl_policy, + }, }; diff --git a/net/wireless/Kconfig b/net/wireless/Kconfig index 29c8675f9a11..22ba971741e5 100644 --- a/net/wireless/Kconfig +++ b/net/wireless/Kconfig @@ -175,7 +175,7 @@ config CFG80211_INTERNAL_REGDB Most distributions have a CRDA package. So if unsure, say N. config CFG80211_WEXT - bool "cfg80211 wireless extensions compatibility" + bool depends on CFG80211 select WEXT_CORE help diff --git a/net/wireless/core.c b/net/wireless/core.c index 4c2e501203d1..53dda7728f86 100644 --- a/net/wireless/core.c +++ b/net/wireless/core.c @@ -546,6 +546,20 @@ int wiphy_register(struct wiphy *wiphy) !rdev->ops->tdls_cancel_channel_switch))) return -EINVAL; + /* + * if a wiphy has unsupported modes for regulatory channel enforcement, + * opt-out of enforcement checking + */ + if (wiphy->interface_modes & ~(BIT(NL80211_IFTYPE_STATION) | + BIT(NL80211_IFTYPE_P2P_CLIENT) | + BIT(NL80211_IFTYPE_AP) | + BIT(NL80211_IFTYPE_P2P_GO) | + BIT(NL80211_IFTYPE_ADHOC) | + BIT(NL80211_IFTYPE_P2P_DEVICE) | + BIT(NL80211_IFTYPE_AP_VLAN) | + BIT(NL80211_IFTYPE_MONITOR))) + wiphy->regulatory_flags |= REGULATORY_IGNORE_STALE_KICKOFF; + if (WARN_ON(wiphy->coalesce && (!wiphy->coalesce->n_rules || !wiphy->coalesce->n_patterns) && diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c index 6e4177701d86..a17d6bc6b22c 100644 --- a/net/wireless/nl80211.c +++ b/net/wireless/nl80211.c @@ -2317,7 +2317,8 @@ static inline u64 wdev_id(struct wireless_dev *wdev) static int nl80211_send_chandef(struct sk_buff *msg, const struct cfg80211_chan_def *chandef) { - WARN_ON(!cfg80211_chandef_valid(chandef)); + if (WARN_ON(!cfg80211_chandef_valid(chandef))) + return -EINVAL; if (nla_put_u32(msg, NL80211_ATTR_WIPHY_FREQ, chandef->chan->center_freq)) @@ -5421,11 +5422,11 @@ static int nl80211_set_reg(struct sk_buff *skb, struct genl_info *info) { struct nlattr *tb[NL80211_REG_RULE_ATTR_MAX + 1]; struct nlattr *nl_reg_rule; - char *alpha2 = NULL; - int rem_reg_rules = 0, r = 0; + char *alpha2; + int rem_reg_rules, r; u32 num_rules = 0, rule_idx = 0, size_of_regd; enum nl80211_dfs_regions dfs_region = NL80211_DFS_UNSET; - struct ieee80211_regdomain *rd = NULL; + struct ieee80211_regdomain *rd; if (!info->attrs[NL80211_ATTR_REG_ALPHA2]) return -EINVAL; @@ -6562,8 +6563,6 @@ static int nl80211_dump_survey(struct sk_buff *skb, } while (1) { - struct ieee80211_channel *chan; - res = rdev_dump_survey(rdev, wdev->netdev, survey_idx, &survey); if (res == -ENOENT) break; @@ -6576,9 +6575,7 @@ static int nl80211_dump_survey(struct sk_buff *skb, goto out; } - chan = ieee80211_get_channel(&rdev->wiphy, - survey.channel->center_freq); - if (!chan || chan->flags & IEEE80211_CHAN_DISABLED) { + if (survey.channel->flags & IEEE80211_CHAN_DISABLED) { survey_idx++; continue; } @@ -11770,55 +11767,155 @@ void cfg80211_mgmt_tx_status(struct wireless_dev *wdev, u64 cookie, } EXPORT_SYMBOL(cfg80211_mgmt_tx_status); -void cfg80211_cqm_rssi_notify(struct net_device *dev, - enum nl80211_cqm_rssi_threshold_event rssi_event, - gfp_t gfp) +static struct sk_buff *cfg80211_prepare_cqm(struct net_device *dev, + const char *mac, gfp_t gfp) { struct wireless_dev *wdev = dev->ieee80211_ptr; - struct wiphy *wiphy = wdev->wiphy; - struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy); - struct sk_buff *msg; - struct nlattr *pinfoattr; - void *hdr; - - trace_cfg80211_cqm_rssi_notify(dev, rssi_event); + struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy); + struct sk_buff *msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); + void **cb; - msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); if (!msg) - return; + return NULL; - hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_NOTIFY_CQM); - if (!hdr) { + cb = (void **)msg->cb; + + cb[0] = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_NOTIFY_CQM); + if (!cb[0]) { nlmsg_free(msg); - return; + return NULL; } if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || nla_put_u32(msg, NL80211_ATTR_IFINDEX, dev->ifindex)) goto nla_put_failure; - pinfoattr = nla_nest_start(msg, NL80211_ATTR_CQM); - if (!pinfoattr) + if (mac && nla_put(msg, NL80211_ATTR_MAC, ETH_ALEN, mac)) goto nla_put_failure; - if (nla_put_u32(msg, NL80211_ATTR_CQM_RSSI_THRESHOLD_EVENT, - rssi_event)) + cb[1] = nla_nest_start(msg, NL80211_ATTR_CQM); + if (!cb[1]) goto nla_put_failure; - nla_nest_end(msg, pinfoattr); + cb[2] = rdev; - genlmsg_end(msg, hdr); + return msg; + nla_put_failure: + nlmsg_free(msg); + return NULL; +} + +static void cfg80211_send_cqm(struct sk_buff *msg, gfp_t gfp) +{ + void **cb = (void **)msg->cb; + struct cfg80211_registered_device *rdev = cb[2]; + + nla_nest_end(msg, cb[1]); + genlmsg_end(msg, cb[0]); + + memset(msg->cb, 0, sizeof(msg->cb)); genlmsg_multicast_netns(&nl80211_fam, wiphy_net(&rdev->wiphy), msg, 0, NL80211_MCGRP_MLME, gfp); +} + +void cfg80211_cqm_rssi_notify(struct net_device *dev, + enum nl80211_cqm_rssi_threshold_event rssi_event, + gfp_t gfp) +{ + struct sk_buff *msg; + + trace_cfg80211_cqm_rssi_notify(dev, rssi_event); + + if (WARN_ON(rssi_event != NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW && + rssi_event != NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH)) + return; + + msg = cfg80211_prepare_cqm(dev, NULL, gfp); + if (!msg) + return; + + if (nla_put_u32(msg, NL80211_ATTR_CQM_RSSI_THRESHOLD_EVENT, + rssi_event)) + goto nla_put_failure; + + cfg80211_send_cqm(msg, gfp); + return; nla_put_failure: - genlmsg_cancel(msg, hdr); nlmsg_free(msg); } EXPORT_SYMBOL(cfg80211_cqm_rssi_notify); +void cfg80211_cqm_txe_notify(struct net_device *dev, + const u8 *peer, u32 num_packets, + u32 rate, u32 intvl, gfp_t gfp) +{ + struct sk_buff *msg; + + msg = cfg80211_prepare_cqm(dev, peer, gfp); + if (!msg) + return; + + if (nla_put_u32(msg, NL80211_ATTR_CQM_TXE_PKTS, num_packets)) + goto nla_put_failure; + + if (nla_put_u32(msg, NL80211_ATTR_CQM_TXE_RATE, rate)) + goto nla_put_failure; + + if (nla_put_u32(msg, NL80211_ATTR_CQM_TXE_INTVL, intvl)) + goto nla_put_failure; + + cfg80211_send_cqm(msg, gfp); + return; + + nla_put_failure: + nlmsg_free(msg); +} +EXPORT_SYMBOL(cfg80211_cqm_txe_notify); + +void cfg80211_cqm_pktloss_notify(struct net_device *dev, + const u8 *peer, u32 num_packets, gfp_t gfp) +{ + struct sk_buff *msg; + + trace_cfg80211_cqm_pktloss_notify(dev, peer, num_packets); + + msg = cfg80211_prepare_cqm(dev, peer, gfp); + if (!msg) + return; + + if (nla_put_u32(msg, NL80211_ATTR_CQM_PKT_LOSS_EVENT, num_packets)) + goto nla_put_failure; + + cfg80211_send_cqm(msg, gfp); + return; + + nla_put_failure: + nlmsg_free(msg); +} +EXPORT_SYMBOL(cfg80211_cqm_pktloss_notify); + +void cfg80211_cqm_beacon_loss_notify(struct net_device *dev, gfp_t gfp) +{ + struct sk_buff *msg; + + msg = cfg80211_prepare_cqm(dev, NULL, gfp); + if (!msg) + return; + + if (nla_put_flag(msg, NL80211_ATTR_CQM_BEACON_LOSS_EVENT)) + goto nla_put_failure; + + cfg80211_send_cqm(msg, gfp); + return; + + nla_put_failure: + nlmsg_free(msg); +} +EXPORT_SYMBOL(cfg80211_cqm_beacon_loss_notify); + static void nl80211_gtk_rekey_notify(struct cfg80211_registered_device *rdev, struct net_device *netdev, const u8 *bssid, const u8 *replay_ctr, gfp_t gfp) @@ -12007,59 +12104,6 @@ void cfg80211_ch_switch_started_notify(struct net_device *dev, } EXPORT_SYMBOL(cfg80211_ch_switch_started_notify); -void cfg80211_cqm_txe_notify(struct net_device *dev, - const u8 *peer, u32 num_packets, - u32 rate, u32 intvl, gfp_t gfp) -{ - struct wireless_dev *wdev = dev->ieee80211_ptr; - struct wiphy *wiphy = wdev->wiphy; - struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy); - struct sk_buff *msg; - struct nlattr *pinfoattr; - void *hdr; - - msg = nlmsg_new(NLMSG_GOODSIZE, gfp); - if (!msg) - return; - - hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_NOTIFY_CQM); - if (!hdr) { - nlmsg_free(msg); - return; - } - - if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || - nla_put_u32(msg, NL80211_ATTR_IFINDEX, dev->ifindex) || - nla_put(msg, NL80211_ATTR_MAC, ETH_ALEN, peer)) - goto nla_put_failure; - - pinfoattr = nla_nest_start(msg, NL80211_ATTR_CQM); - if (!pinfoattr) - goto nla_put_failure; - - if (nla_put_u32(msg, NL80211_ATTR_CQM_TXE_PKTS, num_packets)) - goto nla_put_failure; - - if (nla_put_u32(msg, NL80211_ATTR_CQM_TXE_RATE, rate)) - goto nla_put_failure; - - if (nla_put_u32(msg, NL80211_ATTR_CQM_TXE_INTVL, intvl)) - goto nla_put_failure; - - nla_nest_end(msg, pinfoattr); - - genlmsg_end(msg, hdr); - - genlmsg_multicast_netns(&nl80211_fam, wiphy_net(&rdev->wiphy), msg, 0, - NL80211_MCGRP_MLME, gfp); - return; - - nla_put_failure: - genlmsg_cancel(msg, hdr); - nlmsg_free(msg); -} -EXPORT_SYMBOL(cfg80211_cqm_txe_notify); - void nl80211_radar_notify(struct cfg80211_registered_device *rdev, const struct cfg80211_chan_def *chandef, @@ -12108,54 +12152,6 @@ nl80211_radar_notify(struct cfg80211_registered_device *rdev, nlmsg_free(msg); } -void cfg80211_cqm_pktloss_notify(struct net_device *dev, - const u8 *peer, u32 num_packets, gfp_t gfp) -{ - struct wireless_dev *wdev = dev->ieee80211_ptr; - struct wiphy *wiphy = wdev->wiphy; - struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy); - struct sk_buff *msg; - struct nlattr *pinfoattr; - void *hdr; - - trace_cfg80211_cqm_pktloss_notify(dev, peer, num_packets); - - msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp); - if (!msg) - return; - - hdr = nl80211hdr_put(msg, 0, 0, 0, NL80211_CMD_NOTIFY_CQM); - if (!hdr) { - nlmsg_free(msg); - return; - } - - if (nla_put_u32(msg, NL80211_ATTR_WIPHY, rdev->wiphy_idx) || - nla_put_u32(msg, NL80211_ATTR_IFINDEX, dev->ifindex) || - nla_put(msg, NL80211_ATTR_MAC, ETH_ALEN, peer)) - goto nla_put_failure; - - pinfoattr = nla_nest_start(msg, NL80211_ATTR_CQM); - if (!pinfoattr) - goto nla_put_failure; - - if (nla_put_u32(msg, NL80211_ATTR_CQM_PKT_LOSS_EVENT, num_packets)) - goto nla_put_failure; - - nla_nest_end(msg, pinfoattr); - - genlmsg_end(msg, hdr); - - genlmsg_multicast_netns(&nl80211_fam, wiphy_net(&rdev->wiphy), msg, 0, - NL80211_MCGRP_MLME, gfp); - return; - - nla_put_failure: - genlmsg_cancel(msg, hdr); - nlmsg_free(msg); -} -EXPORT_SYMBOL(cfg80211_cqm_pktloss_notify); - void cfg80211_probe_status(struct net_device *dev, const u8 *addr, u64 cookie, bool acked, gfp_t gfp) { diff --git a/net/wireless/reg.c b/net/wireless/reg.c index 32d8310b0f85..47be6163381c 100644 --- a/net/wireless/reg.c +++ b/net/wireless/reg.c @@ -56,6 +56,7 @@ #include <net/cfg80211.h> #include "core.h" #include "reg.h" +#include "rdev-ops.h" #include "regdb.h" #include "nl80211.h" @@ -66,6 +67,12 @@ #define REG_DBG_PRINT(args...) #endif +/* + * Grace period we give before making sure all current interfaces reside on + * channels allowed by the current regulatory domain. + */ +#define REG_ENFORCE_GRACE_MS 60000 + /** * enum reg_request_treatment - regulatory request treatment * @@ -210,6 +217,9 @@ struct reg_beacon { struct ieee80211_channel chan; }; +static void reg_check_chans_work(struct work_struct *work); +static DECLARE_DELAYED_WORK(reg_check_chans, reg_check_chans_work); + static void reg_todo(struct work_struct *work); static DECLARE_WORK(reg_work, reg_todo); @@ -1518,6 +1528,96 @@ static void reg_call_notifier(struct wiphy *wiphy, wiphy->reg_notifier(wiphy, request); } +static bool reg_wdev_chan_valid(struct wiphy *wiphy, struct wireless_dev *wdev) +{ + struct ieee80211_channel *ch; + struct cfg80211_chan_def chandef; + struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy); + bool ret = true; + + wdev_lock(wdev); + + if (!wdev->netdev || !netif_running(wdev->netdev)) + goto out; + + switch (wdev->iftype) { + case NL80211_IFTYPE_AP: + case NL80211_IFTYPE_P2P_GO: + if (!wdev->beacon_interval) + goto out; + + ret = cfg80211_reg_can_beacon(wiphy, + &wdev->chandef, wdev->iftype); + break; + case NL80211_IFTYPE_STATION: + case NL80211_IFTYPE_P2P_CLIENT: + case NL80211_IFTYPE_ADHOC: + if (!wdev->current_bss || + !wdev->current_bss->pub.channel) + goto out; + + ch = wdev->current_bss->pub.channel; + if (rdev->ops->get_channel && + !rdev_get_channel(rdev, wdev, &chandef)) + ret = cfg80211_chandef_usable(wiphy, &chandef, + IEEE80211_CHAN_DISABLED); + else + ret = !(ch->flags & IEEE80211_CHAN_DISABLED); + break; + case NL80211_IFTYPE_MONITOR: + case NL80211_IFTYPE_AP_VLAN: + case NL80211_IFTYPE_P2P_DEVICE: + /* no enforcement required */ + break; + default: + /* others not implemented for now */ + WARN_ON(1); + break; + } + +out: + wdev_unlock(wdev); + return ret; +} + +static void reg_leave_invalid_chans(struct wiphy *wiphy) +{ + struct wireless_dev *wdev; + struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy); + + ASSERT_RTNL(); + + list_for_each_entry(wdev, &rdev->wdev_list, list) + if (!reg_wdev_chan_valid(wiphy, wdev)) + cfg80211_leave(rdev, wdev); +} + +static void reg_check_chans_work(struct work_struct *work) +{ + struct cfg80211_registered_device *rdev; + + REG_DBG_PRINT("Verifying active interfaces after reg change\n"); + rtnl_lock(); + + list_for_each_entry(rdev, &cfg80211_rdev_list, list) + if (!(rdev->wiphy.regulatory_flags & + REGULATORY_IGNORE_STALE_KICKOFF)) + reg_leave_invalid_chans(&rdev->wiphy); + + rtnl_unlock(); +} + +static void reg_check_channels(void) +{ + /* + * Give usermode a chance to do something nicer (move to another + * channel, orderly disconnection), before forcing a disconnection. + */ + mod_delayed_work(system_power_efficient_wq, + ®_check_chans, + msecs_to_jiffies(REG_ENFORCE_GRACE_MS)); +} + static void wiphy_update_regulatory(struct wiphy *wiphy, enum nl80211_reg_initiator initiator) { @@ -1557,6 +1657,8 @@ static void update_all_wiphy_regulatory(enum nl80211_reg_initiator initiator) wiphy = &rdev->wiphy; wiphy_update_regulatory(wiphy, initiator); } + + reg_check_channels(); } static void handle_channel_custom(struct wiphy *wiphy, @@ -1976,8 +2078,10 @@ static void reg_process_hint(struct regulatory_request *reg_request) /* This is required so that the orig_* parameters are saved */ if (treatment == REG_REQ_ALREADY_SET && wiphy && - wiphy->regulatory_flags & REGULATORY_STRICT_REG) + wiphy->regulatory_flags & REGULATORY_STRICT_REG) { wiphy_update_regulatory(wiphy, reg_request->initiator); + reg_check_channels(); + } return; @@ -2858,6 +2962,7 @@ void regulatory_exit(void) cancel_work_sync(®_work); cancel_delayed_work_sync(®_timeout); + cancel_delayed_work_sync(®_check_chans); /* Lock to suppress warnings */ rtnl_lock(); |