From patchwork Fri Aug 9 09:45:29 2024 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: David Lin X-Patchwork-Id: 818265 Received: from AM0PR83CU005.outbound.protection.outlook.com (mail-westeuropeazon11010001.outbound.protection.outlook.com [52.101.69.1]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-GCM-SHA384 (256/256 bits)) (No client certificate requested) by smtp.subspace.kernel.org (Postfix) with ESMTPS id 791DE19DF40; Fri, 9 Aug 2024 09:48:02 +0000 (UTC) Authentication-Results: smtp.subspace.kernel.org; arc=fail smtp.client-ip=52.101.69.1 ARC-Seal: i=2; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1723196886; cv=fail; b=s7dWfr/K45RF9CX/NziNwpBGE1mR1gYmI1sulcdjiithq/hKmMGL7wvW1Tq4D7tdlwny+6lNMhB6CWnBjoDZppi+I2ta9077E/N+RBZrkyKmE+OxxGWNvuwmQ/VqBpTeC7RbxDiV+FNEddJ+aXrO3uLfsp7DQKVJJSCrpN6zHrY= ARC-Message-Signature: i=2; a=rsa-sha256; d=subspace.kernel.org; s=arc-20240116; t=1723196886; c=relaxed/simple; bh=tpfCXag9igXOnb/Y7Mna/367mSvAiIZUSb89xTy6+as=; h=From:To:Cc:Subject:Date:Message-Id:In-Reply-To:References: Content-Type:MIME-Version; b=jfcSsdrPnT00TWNBhH5fMn4B5G8H09QmZnftC4Qld/fNzGclYbcQfnYzKQDD78J0cS9vUNHNtiOWEMjDkJ1hzqfDakPN+5PtLFVuk414irwDS2EE/hoYORJwpCeoVeiQQsZ5WPppm4szh6g3osr2D+S/cEWwSct7/BmoJgQNrTA= ARC-Authentication-Results: i=2; smtp.subspace.kernel.org; dmarc=pass (p=none dis=none) header.from=nxp.com; spf=pass smtp.mailfrom=nxp.com; dkim=pass (2048-bit key) header.d=nxp.com header.i=@nxp.com header.b=Lv7+uiMy; arc=fail smtp.client-ip=52.101.69.1 Authentication-Results: smtp.subspace.kernel.org; dmarc=pass (p=none dis=none) header.from=nxp.com Authentication-Results: smtp.subspace.kernel.org; spf=pass smtp.mailfrom=nxp.com Authentication-Results: smtp.subspace.kernel.org; dkim=pass (2048-bit key) header.d=nxp.com header.i=@nxp.com header.b="Lv7+uiMy" ARC-Seal: i=1; a=rsa-sha256; s=arcselector10001; d=microsoft.com; cv=none; b=EJkE2g064Y3YrYxNksxOSzOootbxAFGEAdV0/V5LDdcKr8EZVeTYTtsQDpvL+A16RPLxmZ7m2y4ie10BOLosZwLVwIQNYY0Cm9qluc1SSA2GXBhTQmt/R2B3FDGURwSdozwvAjIv7jx6w/XGEcEa8YO+W72jMAiUtRhaafT9nhduB1i61wYrn3Z4yWusXpPHvxgEfyzicbFfJC2d4JTGcJ3b97OkEMop8LkyWaVshiPlnsKEYoluQfB2WCIbpq1A08XiVfi9IXsSvYzGaCdaAY8BLczCtbbZt6BKOkiBG4nqJV64VL5iDLoauWsU8YT5k3q86wt6sGbnghDXdUkF1A== ARC-Message-Signature: i=1; a=rsa-sha256; c=relaxed/relaxed; d=microsoft.com; s=arcselector10001; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-AntiSpam-MessageData-ChunkCount:X-MS-Exchange-AntiSpam-MessageData-0:X-MS-Exchange-AntiSpam-MessageData-1; bh=7zbgy+vJKXupLqBAyhE0jhVfMSCKGY18qtybv2w6LBI=; b=hxNOqDrXDR4ALnKn0fWKhumnoDBEr5AeFbRsIpWYoj5+71GYFyUyKiiInlN4ooH6/xNuOLMz8407S3reF+y9TK5UAWfnLxbKnAKAJ7FgI8C0yVNekKFx4mu0e2MNfgffRkjQvnKtprAijWpUQJNRI0LtJruoZSjS/UxLRAYYSLovDqCkP7o1knKCgrGYqGBe8ab1sb+21xYAtqqmakNWToRYyKsH+Px8y8NN65vS72zj3MWScGPte86HnRCTVudxdNhIKKCKPwvfE0zw7qHvmxlwy2x9yLERSOCz+Hse0VM+NydINkS3f+oQFebO17HglLbL+Cluqa2e8BrT/7dmmA== ARC-Authentication-Results: i=1; mx.microsoft.com 1; spf=pass smtp.mailfrom=nxp.com; dmarc=pass action=none header.from=nxp.com; dkim=pass header.d=nxp.com; arc=none DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=nxp.com; s=selector1; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-SenderADCheck; bh=7zbgy+vJKXupLqBAyhE0jhVfMSCKGY18qtybv2w6LBI=; b=Lv7+uiMyeGHCuvPTJXmhktmtc9Uoehd+iN2LyyxbAV0q+tb2s5+Ie4m9o4A33PGvCyoyri+cqFkzP9iaULzQI2WM4PRytq9cIzudVq/mKQGhN12VaB2QuTVXtXugcgAwTzU4PQSeA1O8kClLtWthiMrYceqgjaFATTEzS0aPOubP07GJkMS6q3aezshtT8kxo7xGAjxFayP+qRKwMgkaPUzrbD+URqxKza1IFGDPe/s0CfotYMNbFnQ3IbzXzThvfzZOEq5FL7IBo7/yHNKclZRjF451sqQOh8Zb4O4lO0zagC64vYxrYXLGMzq9402IaNFpRdFGN2/e7kmA2S3v5w== Authentication-Results: dkim=none (message not signed) header.d=none;dmarc=none action=none header.from=nxp.com; Received: from PA4PR04MB9638.eurprd04.prod.outlook.com (2603:10a6:102:273::20) by GV1PR04MB9120.eurprd04.prod.outlook.com (2603:10a6:150:27::16) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.7849.13; Fri, 9 Aug 2024 09:47:59 +0000 Received: from PA4PR04MB9638.eurprd04.prod.outlook.com ([fe80::f950:3bb6:6848:2257]) by PA4PR04MB9638.eurprd04.prod.outlook.com ([fe80::f950:3bb6:6848:2257%5]) with mapi id 15.20.7828.023; Fri, 9 Aug 2024 09:47:59 +0000 From: David Lin To: linux-wireless@vger.kernel.org Cc: linux-kernel@vger.kernel.org, kvalo@kernel.org, johannes@sipsolutions.net, briannorris@chromium.org, francesco@dolcini.it, tsung-hsien.hsieh@nxp.com, David Lin Subject: [PATCH v2 39/43] wifi: nxpwifi: add wmm.c Date: Fri, 9 Aug 2024 17:45:29 +0800 Message-Id: <20240809094533.1660-40-yu-hao.lin@nxp.com> X-Mailer: git-send-email 2.25.1 In-Reply-To: <20240809094533.1660-1-yu-hao.lin@nxp.com> References: <20240809094533.1660-1-yu-hao.lin@nxp.com> X-ClientProxiedBy: TYCP286CA0178.JPNP286.PROD.OUTLOOK.COM (2603:1096:400:3c6::9) To PA4PR04MB9638.eurprd04.prod.outlook.com (2603:10a6:102:273::20) Precedence: bulk X-Mailing-List: linux-wireless@vger.kernel.org List-Id: List-Subscribe: List-Unsubscribe: MIME-Version: 1.0 X-MS-PublicTrafficType: Email X-MS-TrafficTypeDiagnostic: PA4PR04MB9638:EE_|GV1PR04MB9120:EE_ X-MS-Office365-Filtering-Correlation-Id: 9a4c2686-1d3f-40d0-a1ac-08dcb8585a7a X-MS-Exchange-SenderADCheck: 1 X-MS-Exchange-AntiSpam-Relay: 0 X-Microsoft-Antispam: BCL:0;ARA:13230040|366016|376014|52116014|1800799024; X-Microsoft-Antispam-Message-Info: v1FgRAcI/w8rz9fx6vljXLtAPSjXRNu698BhHexA6VCGAmVvKfp28s2ft7dGLzeJDss79q0RX0aZwEakHM3BbFSQ8irtoaDoWXQX85WDSMb2btetHemlwC5tgampLAd2kCvXI4YrmpzrENdwGR5p/qQdJ/z6GkRQV7l2XKVuTWiUwG0iuDW5UWGP57tDVb1oFBK0EOrOhKzSm4AVh32Ylj+BaYAGbQyxeFqfMFGvEfu6s2yNyfI1VXx5RnqqfwaSeV9lePwg+rstfU8QMC5ofzJB0rhM96omldysnzCvFYrXmiXjYQ/yFckZr9c4/YfkcVkS9BqGaY8PM7ar29oQDL3ZfHKhxfAcLmvQnD/dmxUN/pq5DGrkzg6IB66Il6feF4g1HS0c/uLjwhkNTpep+qbK7lVFTMS8btEyTSUGtkJRU/5LmPvSHE71qeYo/LlUsLedhxe2OOfb/2aVKX1oZPsfv/COQ3DVhv6n3jl/YzIVLsvjM2Hl7Z6TcJDTvJfqmaGta01Da0OBiYHktn9q/cKco/cXazQugXMLO7/OCIp3btI2JYKRYAgwljekdsoSZPhVnA5pubDoZtlseCaxOmA1A9ylDIDH9SwGu21/5mHQ0jAmAl2tK3uisRq2abg8d06uHUu8QzpHelvcYBM5ot0KH2p9vd8VI5DYR+FqoXmNf7cbHoZW3ofH9RL4Bkcf2p+Gp8Z7aoM0mf1UeQqlTkh+ncbxGG/yhmehJH/MqAoFKAMYywIrhzPnad2J9ZETKhZM7zxOVcbrKxfw8ev1vl5uo5emZaRdJa4pXNb9hM3kXN4fqQiWDawQYFjYktMVHese+l74nrTEvmrw2J04uCULq41RIfGyzfU9p0H4aaqn+4FngKgce184wY6V1+T7Tzcfhqn34mm50BLERgWvoRYqOmp3rgMF27USeS36Pkgeq6qiLSLE1hracl4TYaIQh6WNaVuWs9Ifg5GNH07V+ZxrCQZgWuUgqBxhb50jOgOlTI8jsEmEqlSEEcqJ1pBRIO/05aB/D26RYORlDNE9iBb3Sp7l+jBW+8/i7tRQ8JqSGJpYAVckEGQBDBNgw9j37ri52Q0dxNgMaKe3bBa7TaGnx1y2jRs84R9OJ7YE2yfLRUYCltwIX681DvIvaaPxZNwFPRlfVrQeaVj7Y3KSvp7kn6HKh8hmCT7y72WMs/CHYOdrIrGxKl/VXl993wy8iWiSkebVEnGYe5pdoFIEyeM0kTtF/EsSrZv54Oh1SGVYLG16LXKEdu5WxWeCJNDKoDIJNohqXQJstfVTNLQjtQev/68KHeyUq1rvaQyLa7BC2sX7xHy3Lld5CGS4u/0Wt8fnJ2JR+3b6Xd/haGgY+w== X-Forefront-Antispam-Report: CIP:255.255.255.255; CTRY:; LANG:en; SCL:1; SRV:; IPV:NLI; SFV:NSPM; H:PA4PR04MB9638.eurprd04.prod.outlook.com; PTR:; CAT:NONE; SFS:(13230040)(366016)(376014)(52116014)(1800799024); DIR:OUT; SFP:1101; X-MS-Exchange-AntiSpam-MessageData-ChunkCount: 1 X-MS-Exchange-AntiSpam-MessageData-0: +hhiuO9GPes/tonJOm4NwRWRTTfLiLDwkRer/MrdubYjjt9M0ST8EQntG5h9Tugj6/l543wf/UqU/PsD+RIqpfipOsAH71xMdPMtp8vRQB+8AUjR3j7JASmvs4v2WC3OD0wtiC9SKpmZ+3Y1Be5qOCFGyZ393mrK2QkyGg9zgd9P8XNm71u0EtM2xqsTzdtL00YZofF0ghH1AZtfrRY+y4DvqdnGtZLAfIUDLF30rglaC6c8HOO/cnUsKvheYSVb9hCD1+Y5FPnYqx6/93SnCgLSWFuIOu6S+2atFRi2HPg9l6g0717wX1MLgO5um9fgZAyMn09US5y04UO8xCwpBzybGSw5Mz0hMITh0cLoCfUnZbKe1EYFyEdJC0QVB00mYiE0gRAFMUL8oYjbv1O/jw/H8IZ9DUTLQlnmfLqySrpBj8WIcxPDS7drd7VsYSYhpMxpl4lk1jc6eThfA4mFF45q4e5EvSv3l/SXUxcJ5fv9MTVvdUpZcRv6NzheXHVthPS+fAjYnjf+DQWc+kXk1Akt3nW5xUznGlGNDY4V3tOJVNveMrkIvMaYaJAQlgA6mhDEyj84/v36Im3WYSTTXEBzjI9TbIgctc/D0PL/uaT8Hut+gKMzXTBVH0lbyBAtuUfa7MZbw+WNQ1aiKmFR2h2UyZBPJcRWcRk0iIVvQq2BhXjHyf/dBSR1dJXkOeXyY8oLd54y/djIfGI4A7NAdV9Wv0NWigrcQKK9YM52U4PmghVyQa2yArLqgemqJh1FPCTq4BReeUCntnSSvaTcwvhdpys2bqPIxPtZqkm9Eg+WlmznriOzXDniBuIhTWZkDsY7Wv4qFcXj35CrHzUc7RKCGgH2SrdUzdWyQ97dHwfl0sw/Fzhb8XmH3WsBAIz+1/H884/Mof8soEErjl9TaOVBaIUyWqCZW2qbALxbjtk/eevhvSYvRGLbd2gO4d+EJNtBsIB+dUMgJIEt8P9VhBVkGVSLorUu1/m0rgcoW80RiU57RsYuZUrNOchE14/DjHIyajJL2ogBBMrIfI5Ah0wJmjfGQUxiWq+D3BFoMN5wL+CxswB18ngXUsndhnq/wU1xnP9kyVTjY1PY+uUcuyGU3vUegytOLmlAoMfE8/p6X/dlIASl3PbgYpaoV5pgm/+6SP7t4POLWiv4lQC+bin+1oaw/fpXywEX58jAt/GQPFX+PRT0jbLQmJYSaRkW3zjslipqgD4oljDZvG4rz49WoOXkm9JClKiLQ66LwaKednlmAQRbEXCG2hYh9VA4m8TzstEK09odTVGpyUEny0sAl2YSyES9z6UgxRf0PNObh832LnO3rYmNvmPrV0ySLAImhJRsQYVscjlr6aMQXVTlByNF4jeMZY721a4nmcAjFpJaJpyN5ZAn6LGW63gFVhr9DFGVW6UT4WFdf+yQWqn9XrWzWYxsLeefLJlOjBlFka8VkwRbDmtAUXQ8NlqoSZyVaHyiv6KEIvhKFszULBYaeEJLVuzopMI4HJXzoooRfN1MDnMEHPl4BuQChiVJtR9Pa/t4PunJQfJWwyjz31XxEfhx/eQqOiSoK+iI7g7KrO0aVDRrYgMzUB2tPvgxB8BzlLVxKOurrvrSRLlUGOZDASNLB+8WCfD11XPJdF4f8yTR2lAtE9bFvvv29HcD X-OriginatorOrg: nxp.com X-MS-Exchange-CrossTenant-Network-Message-Id: 9a4c2686-1d3f-40d0-a1ac-08dcb8585a7a X-MS-Exchange-CrossTenant-AuthSource: PA4PR04MB9638.eurprd04.prod.outlook.com X-MS-Exchange-CrossTenant-AuthAs: Internal X-MS-Exchange-CrossTenant-OriginalArrivalTime: 09 Aug 2024 09:47:59.7558 (UTC) X-MS-Exchange-CrossTenant-FromEntityHeader: Hosted X-MS-Exchange-CrossTenant-Id: 686ea1d3-bc2b-4c6f-a92c-d99c5c301635 X-MS-Exchange-CrossTenant-MailboxType: HOSTED X-MS-Exchange-CrossTenant-UserPrincipalName: c9FIZNTjYPKaBw0+O0Sb6gHyLIDHV2y8jNUvetk9YM0Oc4vsEv5SE8AQMHfUITdDJGv6DArE65lc/m6u2wucIQ== X-MS-Exchange-Transport-CrossTenantHeadersStamped: GV1PR04MB9120 --- drivers/net/wireless/nxp/nxpwifi/wmm.c | 1379 ++++++++++++++++++++++++ 1 file changed, 1379 insertions(+) create mode 100644 drivers/net/wireless/nxp/nxpwifi/wmm.c diff --git a/drivers/net/wireless/nxp/nxpwifi/wmm.c b/drivers/net/wireless/nxp/nxpwifi/wmm.c new file mode 100644 index 000000000000..d8ae54458096 --- /dev/null +++ b/drivers/net/wireless/nxp/nxpwifi/wmm.c @@ -0,0 +1,1379 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * NXP Wireless LAN device driver: WMM + * + * Copyright 2011-2024 NXP + */ + +#include "decl.h" +#include "cfg.h" +#include "util.h" +#include "fw.h" +#include "main.h" +#include "wmm.h" +#include "11n.h" + +/* Maximum value FW can accept for driver delay in packet transmission */ +#define DRV_PKT_DELAY_TO_FW_MAX 512 + +#define WMM_QUEUED_PACKET_LOWER_LIMIT 180 + +#define WMM_QUEUED_PACKET_UPPER_LIMIT 200 + +/* Offset for TOS field in the IP header */ +#define IPTOS_OFFSET 5 + +static bool disable_tx_amsdu; + +/* This table inverses the tos_to_tid operation to get a priority + * which is in sequential order, and can be compared. + * Use this to compare the priority of two different TIDs. + */ +const u8 tos_to_tid_inv[] = { + 0x02, /* from tos_to_tid[2] = 0 */ + 0x00, /* from tos_to_tid[0] = 1 */ + 0x01, /* from tos_to_tid[1] = 2 */ + 0x03, + 0x04, + 0x05, + 0x06, + 0x07 +}; + +/* WMM information IE */ +static const u8 wmm_info_ie[] = { WLAN_EID_VENDOR_SPECIFIC, 0x07, + 0x00, 0x50, 0xf2, 0x02, + 0x00, 0x01, 0x00 +}; + +static const u8 wmm_aci_to_qidx_map[] = { WMM_AC_BE, + WMM_AC_BK, + WMM_AC_VI, + WMM_AC_VO +}; + +static u8 tos_to_tid[] = { + /* TID DSCP_P2 DSCP_P1 DSCP_P0 WMM_AC */ + 0x01, /* 0 1 0 AC_BK */ + 0x02, /* 0 0 0 AC_BK */ + 0x00, /* 0 0 1 AC_BE */ + 0x03, /* 0 1 1 AC_BE */ + 0x04, /* 1 0 0 AC_VI */ + 0x05, /* 1 0 1 AC_VI */ + 0x06, /* 1 1 0 AC_VO */ + 0x07 /* 1 1 1 AC_VO */ +}; + +static u8 ac_to_tid[4][2] = { {1, 2}, {0, 3}, {4, 5}, {6, 7} }; + +/* This function debug prints the priority parameters for a WMM AC. + */ +static void +nxpwifi_wmm_ac_debug_print(const struct ieee80211_wmm_ac_param *ac_param) +{ + static const char * const ac_str[] = { "BK", "BE", "VI", "VO" }; + + pr_debug("info: WMM AC_%s: ACI=%d, ACM=%d, Aifsn=%d, ", + ac_str[wmm_aci_to_qidx_map[(ac_param->aci_aifsn + & NXPWIFI_ACI) >> 5]], + (ac_param->aci_aifsn & NXPWIFI_ACI) >> 5, + (ac_param->aci_aifsn & NXPWIFI_ACM) >> 4, + ac_param->aci_aifsn & NXPWIFI_AIFSN); + pr_debug("EcwMin=%d, EcwMax=%d, TxopLimit=%d\n", + ac_param->cw & NXPWIFI_ECW_MIN, + (ac_param->cw & NXPWIFI_ECW_MAX) >> 4, + le16_to_cpu(ac_param->txop_limit)); +} + +/* This function allocates a route address list. + * + * The function also initializes the list with the provided RA. + */ +static struct nxpwifi_ra_list_tbl * +nxpwifi_wmm_allocate_ralist_node(struct nxpwifi_adapter *adapter, const u8 *ra) +{ + struct nxpwifi_ra_list_tbl *ra_list; + + ra_list = kzalloc(sizeof(*ra_list), GFP_ATOMIC); + if (!ra_list) + return NULL; + + INIT_LIST_HEAD(&ra_list->list); + skb_queue_head_init(&ra_list->skb_head); + + memcpy(ra_list->ra, ra, ETH_ALEN); + + ra_list->total_pkt_count = 0; + + nxpwifi_dbg(adapter, INFO, "info: allocated ra_list %p\n", ra_list); + + return ra_list; +} + +/* This function returns random no between 16 and 32 to be used as threshold + * for no of packets after which BA setup is initiated. + */ +static u8 nxpwifi_get_random_ba_threshold(void) +{ + u64 ns; + /* setup ba_packet_threshold here random number between + * [BA_SETUP_PACKET_OFFSET, + * BA_SETUP_PACKET_OFFSET+BA_SETUP_MAX_PACKET_THRESHOLD-1] + */ + ns = ktime_get_ns(); + ns += (ns >> 32) + (ns >> 16); + + return ((u8)ns % BA_SETUP_MAX_PACKET_THRESHOLD) + BA_SETUP_PACKET_OFFSET; +} + +/* This function allocates and adds a RA list for all TIDs + * with the given RA. + */ +void nxpwifi_ralist_add(struct nxpwifi_private *priv, const u8 *ra) +{ + int i; + struct nxpwifi_ra_list_tbl *ra_list; + struct nxpwifi_adapter *adapter = priv->adapter; + struct nxpwifi_sta_node *node; + + for (i = 0; i < MAX_NUM_TID; ++i) { + ra_list = nxpwifi_wmm_allocate_ralist_node(adapter, ra); + nxpwifi_dbg(adapter, INFO, + "info: created ra_list %p\n", ra_list); + + if (!ra_list) + break; + + ra_list->is_11n_enabled = 0; + ra_list->ba_status = BA_SETUP_NONE; + ra_list->amsdu_in_ampdu = false; + if (!nxpwifi_queuing_ra_based(priv)) { + ra_list->is_11n_enabled = IS_11N_ENABLED(priv); + } else { + spin_lock_bh(&priv->sta_list_spinlock); + node = nxpwifi_get_sta_entry(priv, ra); + if (node) + ra_list->tx_paused = node->tx_pause; + ra_list->is_11n_enabled = + nxpwifi_is_sta_11n_enabled(priv, node); + if (ra_list->is_11n_enabled) + ra_list->max_amsdu = node->max_amsdu; + spin_unlock_bh(&priv->sta_list_spinlock); + } + + nxpwifi_dbg(adapter, DATA, "data: ralist %p: is_11n_enabled=%d\n", + ra_list, ra_list->is_11n_enabled); + + if (ra_list->is_11n_enabled) { + ra_list->ba_pkt_count = 0; + ra_list->ba_packet_thr = + nxpwifi_get_random_ba_threshold(); + } + list_add_tail(&ra_list->list, + &priv->wmm.tid_tbl_ptr[i].ra_list); + } +} + +/* This function sets the WMM queue priorities to their default values. + */ +static void nxpwifi_wmm_default_queue_priorities(struct nxpwifi_private *priv) +{ + /* Default queue priorities: VO->VI->BE->BK */ + priv->wmm.queue_priority[0] = WMM_AC_VO; + priv->wmm.queue_priority[1] = WMM_AC_VI; + priv->wmm.queue_priority[2] = WMM_AC_BE; + priv->wmm.queue_priority[3] = WMM_AC_BK; +} + +/* This function map ACs to TIDs. + */ +static void +nxpwifi_wmm_queue_priorities_tid(struct nxpwifi_private *priv) +{ + struct nxpwifi_wmm_desc *wmm = &priv->wmm; + u8 *queue_priority = wmm->queue_priority; + int i; + + for (i = 0; i < 4; ++i) { + tos_to_tid[7 - (i * 2)] = ac_to_tid[queue_priority[i]][1]; + tos_to_tid[6 - (i * 2)] = ac_to_tid[queue_priority[i]][0]; + } + + for (i = 0; i < MAX_NUM_TID; ++i) + priv->tos_to_tid_inv[tos_to_tid[i]] = (u8)i; + + atomic_set(&wmm->highest_queued_prio, HIGH_PRIO_TID); +} + +/* This function initializes WMM priority queues. + */ +void +nxpwifi_wmm_setup_queue_priorities(struct nxpwifi_private *priv, + struct ieee80211_wmm_param_ie *wmm_ie) +{ + u16 cw_min, avg_back_off, tmp[4]; + u32 i, j, num_ac; + u8 ac_idx; + + if (!wmm_ie || !priv->wmm_enabled) { + /* WMM is not enabled, just set the defaults and return */ + nxpwifi_wmm_default_queue_priorities(priv); + return; + } + + nxpwifi_dbg(priv->adapter, INFO, + "info: WMM Parameter IE: version=%d,\t" + "qos_info Parameter Set Count=%d, Reserved=%#x\n", + wmm_ie->version, wmm_ie->qos_info & + IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK, + wmm_ie->reserved); + + for (num_ac = 0; num_ac < ARRAY_SIZE(wmm_ie->ac); num_ac++) { + u8 ecw = wmm_ie->ac[num_ac].cw; + u8 aci_aifsn = wmm_ie->ac[num_ac].aci_aifsn; + + cw_min = (1 << (ecw & NXPWIFI_ECW_MIN)) - 1; + avg_back_off = (cw_min >> 1) + (aci_aifsn & NXPWIFI_AIFSN); + + ac_idx = wmm_aci_to_qidx_map[(aci_aifsn & NXPWIFI_ACI) >> 5]; + priv->wmm.queue_priority[ac_idx] = ac_idx; + tmp[ac_idx] = avg_back_off; + + nxpwifi_dbg(priv->adapter, INFO, + "info: WMM: CWmax=%d CWmin=%d Avg Back-off=%d\n", + (1 << ((ecw & NXPWIFI_ECW_MAX) >> 4)) - 1, + cw_min, avg_back_off); + nxpwifi_wmm_ac_debug_print(&wmm_ie->ac[num_ac]); + } + + /* Bubble sort */ + for (i = 0; i < num_ac; i++) { + for (j = 1; j < num_ac - i; j++) { + if (tmp[j - 1] > tmp[j]) { + swap(tmp[j - 1], tmp[j]); + swap(priv->wmm.queue_priority[j - 1], + priv->wmm.queue_priority[j]); + } else if (tmp[j - 1] == tmp[j]) { + if (priv->wmm.queue_priority[j - 1] + < priv->wmm.queue_priority[j]) + swap(priv->wmm.queue_priority[j - 1], + priv->wmm.queue_priority[j]); + } + } + } + + nxpwifi_wmm_queue_priorities_tid(priv); +} + +/* This function evaluates whether or not an AC is to be downgraded. + * + * In case the AC is not enabled, the highest AC is returned that is + * enabled and does not require admission control. + */ +static enum nxpwifi_wmm_ac_e +nxpwifi_wmm_eval_downgrade_ac(struct nxpwifi_private *priv, + enum nxpwifi_wmm_ac_e eval_ac) +{ + int down_ac; + enum nxpwifi_wmm_ac_e ret_ac; + struct nxpwifi_wmm_ac_status *ac_status; + + ac_status = &priv->wmm.ac_status[eval_ac]; + + if (!ac_status->disabled) + /* Okay to use this AC, its enabled */ + return eval_ac; + + /* Setup a default return value of the lowest priority */ + ret_ac = WMM_AC_BK; + + /* Find the highest AC that is enabled and does not require + * admission control. The spec disallows downgrading to an AC, + * which is enabled due to a completed admission control. + * Unadmitted traffic is not to be sent on an AC with admitted + * traffic. + */ + for (down_ac = WMM_AC_BK; down_ac < eval_ac; down_ac++) { + ac_status = &priv->wmm.ac_status[down_ac]; + + if (!ac_status->disabled && !ac_status->flow_required) + /* AC is enabled and does not require admission + * control + */ + ret_ac = (enum nxpwifi_wmm_ac_e)down_ac; + } + + return ret_ac; +} + +/* This function downgrades WMM priority queue. + */ +void +nxpwifi_wmm_setup_ac_downgrade(struct nxpwifi_private *priv) +{ + int ac_val; + + nxpwifi_dbg(priv->adapter, INFO, "info: WMM: AC Priorities:\t" + "BK(0), BE(1), VI(2), VO(3)\n"); + + if (!priv->wmm_enabled) { + /* WMM is not enabled, default priorities */ + for (ac_val = WMM_AC_BK; ac_val <= WMM_AC_VO; ac_val++) + priv->wmm.ac_down_graded_vals[ac_val] = + (enum nxpwifi_wmm_ac_e)ac_val; + } else { + for (ac_val = WMM_AC_BK; ac_val <= WMM_AC_VO; ac_val++) { + priv->wmm.ac_down_graded_vals[ac_val] = + nxpwifi_wmm_eval_downgrade_ac + (priv, (enum nxpwifi_wmm_ac_e)ac_val); + nxpwifi_dbg(priv->adapter, INFO, + "info: WMM: AC PRIO %d maps to %d\n", + ac_val, + priv->wmm.ac_down_graded_vals[ac_val]); + } + } +} + +/* This function converts the IP TOS field to an WMM AC + * Queue assignment. + */ +static enum nxpwifi_wmm_ac_e +nxpwifi_wmm_convert_tos_to_ac(struct nxpwifi_adapter *adapter, u32 tos) +{ + /* Map of TOS UP values to WMM AC */ + static const enum nxpwifi_wmm_ac_e tos_to_ac[] = { + WMM_AC_BE, + WMM_AC_BK, + WMM_AC_BK, + WMM_AC_BE, + WMM_AC_VI, + WMM_AC_VI, + WMM_AC_VO, + WMM_AC_VO + }; + + if (tos >= ARRAY_SIZE(tos_to_ac)) + return WMM_AC_BE; + + return tos_to_ac[tos]; +} + +/* This function evaluates a given TID and downgrades it to a lower + * TID if the WMM Parameter IE received from the AP indicates that the + * AP is disabled (due to call admission control (ACM bit). Mapping + * of TID to AC is taken care of internally. + */ +u8 nxpwifi_wmm_downgrade_tid(struct nxpwifi_private *priv, u32 tid) +{ + enum nxpwifi_wmm_ac_e ac, ac_down; + u8 new_tid; + + ac = nxpwifi_wmm_convert_tos_to_ac(priv->adapter, tid); + ac_down = priv->wmm.ac_down_graded_vals[ac]; + + /* Send the index to tid array, picking from the array will be + * taken care by dequeuing function + */ + new_tid = ac_to_tid[ac_down][tid % 2]; + + return new_tid; +} + +/* This function initializes the WMM state information and the + * WMM data path queues. + */ +void +nxpwifi_wmm_init(struct nxpwifi_adapter *adapter) +{ + int i, j; + struct nxpwifi_private *priv; + + for (j = 0; j < adapter->priv_num; ++j) { + priv = adapter->priv[j]; + if (!priv) + continue; + + for (i = 0; i < MAX_NUM_TID; ++i) { + if (!disable_tx_amsdu && + adapter->tx_buf_size > NXPWIFI_TX_DATA_BUF_SIZE_2K) + priv->aggr_prio_tbl[i].amsdu = + priv->tos_to_tid_inv[i]; + else + priv->aggr_prio_tbl[i].amsdu = + BA_STREAM_NOT_ALLOWED; + priv->aggr_prio_tbl[i].ampdu_ap = + priv->tos_to_tid_inv[i]; + priv->aggr_prio_tbl[i].ampdu_user = + priv->tos_to_tid_inv[i]; + } + + priv->aggr_prio_tbl[6].amsdu = + priv->aggr_prio_tbl[6].ampdu_ap = + priv->aggr_prio_tbl[6].ampdu_user = + BA_STREAM_NOT_ALLOWED; + + priv->aggr_prio_tbl[7].amsdu = + priv->aggr_prio_tbl[7].ampdu_ap = + priv->aggr_prio_tbl[7].ampdu_user = + BA_STREAM_NOT_ALLOWED; + + nxpwifi_set_ba_params(priv); + nxpwifi_reset_11n_rx_seq_num(priv); + + priv->wmm.drv_pkt_delay_max = NXPWIFI_WMM_DRV_DELAY_MAX; + atomic_set(&priv->wmm.tx_pkts_queued, 0); + atomic_set(&priv->wmm.highest_queued_prio, HIGH_PRIO_TID); + } +} + +bool nxpwifi_bypass_txlist_empty(struct nxpwifi_adapter *adapter) +{ + struct nxpwifi_private *priv; + int i; + + for (i = 0; i < adapter->priv_num; i++) { + priv = adapter->priv[i]; + if (!priv) + continue; + if (!skb_queue_empty(&priv->bypass_txq)) + return false; + } + + return true; +} + +/* This function checks if WMM Tx queue is empty. + */ +bool nxpwifi_wmm_lists_empty(struct nxpwifi_adapter *adapter) +{ + int i; + struct nxpwifi_private *priv; + + for (i = 0; i < adapter->priv_num; ++i) { + priv = adapter->priv[i]; + if (!priv) + continue; + if (!priv->port_open) + continue; + if (atomic_read(&priv->wmm.tx_pkts_queued)) + return false; + } + + return true; +} + +/* This function deletes all packets in an RA list node. + * + * The packet sent completion callback handler are called with + * status failure, after they are dequeued to ensure proper + * cleanup. The RA list node itself is freed at the end. + */ +static void +nxpwifi_wmm_del_pkts_in_ralist_node(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ra_list) +{ + struct nxpwifi_adapter *adapter = priv->adapter; + struct sk_buff *skb, *tmp; + + skb_queue_walk_safe(&ra_list->skb_head, skb, tmp) { + skb_unlink(skb, &ra_list->skb_head); + nxpwifi_write_data_complete(adapter, skb, 0, -1); + } +} + +/* This function deletes all packets in an RA list. + * + * Each nodes in the RA list are freed individually first, and then + * the RA list itself is freed. + */ +static void +nxpwifi_wmm_del_pkts_in_ralist(struct nxpwifi_private *priv, + struct list_head *ra_list_head) +{ + struct nxpwifi_ra_list_tbl *ra_list; + + list_for_each_entry(ra_list, ra_list_head, list) + nxpwifi_wmm_del_pkts_in_ralist_node(priv, ra_list); +} + +/* This function deletes all packets in all RA lists. + */ +static void nxpwifi_wmm_cleanup_queues(struct nxpwifi_private *priv) +{ + int i; + + for (i = 0; i < MAX_NUM_TID; i++) + nxpwifi_wmm_del_pkts_in_ralist + (priv, &priv->wmm.tid_tbl_ptr[i].ra_list); + + atomic_set(&priv->wmm.tx_pkts_queued, 0); + atomic_set(&priv->wmm.highest_queued_prio, HIGH_PRIO_TID); +} + +/* This function deletes all route addresses from all RA lists. + */ +static void nxpwifi_wmm_delete_all_ralist(struct nxpwifi_private *priv) +{ + struct nxpwifi_ra_list_tbl *ra_list, *tmp_node; + int i; + + for (i = 0; i < MAX_NUM_TID; ++i) { + nxpwifi_dbg(priv->adapter, INFO, + "info: ra_list: freeing buf for tid %d\n", i); + list_for_each_entry_safe(ra_list, tmp_node, + &priv->wmm.tid_tbl_ptr[i].ra_list, + list) { + list_del(&ra_list->list); + kfree(ra_list); + } + + INIT_LIST_HEAD(&priv->wmm.tid_tbl_ptr[i].ra_list); + } +} + +static int nxpwifi_free_ack_frame(int id, void *p, void *data) +{ + pr_warn("Have pending ack frames!\n"); + kfree_skb(p); + return 0; +} + +/* This function cleans up the Tx and Rx queues. + * + * Cleanup includes - + * - All packets in RA lists + * - All entries in Rx reorder table + * - All entries in Tx BA stream table + * - MPA buffer (if required) + * - All RA lists + */ +void +nxpwifi_clean_txrx(struct nxpwifi_private *priv) +{ + struct sk_buff *skb, *tmp; + + nxpwifi_11n_cleanup_reorder_tbl(priv); + spin_lock_bh(&priv->wmm.ra_list_spinlock); + + nxpwifi_wmm_cleanup_queues(priv); + nxpwifi_11n_delete_all_tx_ba_stream_tbl(priv); + + if (priv->adapter->if_ops.cleanup_mpa_buf) + priv->adapter->if_ops.cleanup_mpa_buf(priv->adapter); + + nxpwifi_wmm_delete_all_ralist(priv); + memcpy(tos_to_tid, ac_to_tid, sizeof(tos_to_tid)); + + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + + skb_queue_walk_safe(&priv->bypass_txq, skb, tmp) { + skb_unlink(skb, &priv->bypass_txq); + nxpwifi_write_data_complete(priv->adapter, skb, 0, -1); + } + atomic_set(&priv->adapter->bypass_tx_pending, 0); + + idr_for_each(&priv->ack_status_frames, nxpwifi_free_ack_frame, NULL); + idr_destroy(&priv->ack_status_frames); +} + +/* This function retrieves a particular RA list node, matching with the + * given TID and RA address. + */ +struct nxpwifi_ra_list_tbl * +nxpwifi_wmm_get_ralist_node(struct nxpwifi_private *priv, u8 tid, + const u8 *ra_addr) +{ + struct nxpwifi_ra_list_tbl *ra_list; + + list_for_each_entry(ra_list, &priv->wmm.tid_tbl_ptr[tid].ra_list, + list) { + if (!memcmp(ra_list->ra, ra_addr, ETH_ALEN)) + return ra_list; + } + + return NULL; +} + +void nxpwifi_update_ralist_tx_pause(struct nxpwifi_private *priv, u8 *mac, + u8 tx_pause) +{ + struct nxpwifi_ra_list_tbl *ra_list; + u32 pkt_cnt = 0, tx_pkts_queued; + int i; + + spin_lock_bh(&priv->wmm.ra_list_spinlock); + + for (i = 0; i < MAX_NUM_TID; ++i) { + ra_list = nxpwifi_wmm_get_ralist_node(priv, i, mac); + if (ra_list && ra_list->tx_paused != tx_pause) { + pkt_cnt += ra_list->total_pkt_count; + ra_list->tx_paused = tx_pause; + if (tx_pause) + priv->wmm.pkts_paused[i] += + ra_list->total_pkt_count; + else + priv->wmm.pkts_paused[i] -= + ra_list->total_pkt_count; + } + } + + if (pkt_cnt) { + tx_pkts_queued = atomic_read(&priv->wmm.tx_pkts_queued); + if (tx_pause) + tx_pkts_queued -= pkt_cnt; + else + tx_pkts_queued += pkt_cnt; + + atomic_set(&priv->wmm.tx_pkts_queued, tx_pkts_queued); + atomic_set(&priv->wmm.highest_queued_prio, HIGH_PRIO_TID); + } + spin_unlock_bh(&priv->wmm.ra_list_spinlock); +} + +/* This function retrieves an RA list node for a given TID and + * RA address pair. + * + * If no such node is found, a new node is added first and then + * retrieved. + */ +struct nxpwifi_ra_list_tbl * +nxpwifi_wmm_get_queue_raptr(struct nxpwifi_private *priv, u8 tid, + const u8 *ra_addr) +{ + struct nxpwifi_ra_list_tbl *ra_list; + + ra_list = nxpwifi_wmm_get_ralist_node(priv, tid, ra_addr); + if (ra_list) + return ra_list; + nxpwifi_ralist_add(priv, ra_addr); + + return nxpwifi_wmm_get_ralist_node(priv, tid, ra_addr); +} + +/* This function deletes RA list nodes for given mac for all TIDs. + * Function also decrements TX pending count accordingly. + */ +void +nxpwifi_wmm_del_peer_ra_list(struct nxpwifi_private *priv, const u8 *ra_addr) +{ + struct nxpwifi_ra_list_tbl *ra_list; + int i; + + spin_lock_bh(&priv->wmm.ra_list_spinlock); + + for (i = 0; i < MAX_NUM_TID; ++i) { + ra_list = nxpwifi_wmm_get_ralist_node(priv, i, ra_addr); + + if (!ra_list) + continue; + nxpwifi_wmm_del_pkts_in_ralist_node(priv, ra_list); + if (ra_list->tx_paused) + priv->wmm.pkts_paused[i] -= ra_list->total_pkt_count; + else + atomic_sub(ra_list->total_pkt_count, + &priv->wmm.tx_pkts_queued); + list_del(&ra_list->list); + kfree(ra_list); + } + spin_unlock_bh(&priv->wmm.ra_list_spinlock); +} + +/* This function checks if a particular RA list node exists in a given TID + * table index. + */ +bool nxpwifi_is_ralist_valid(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ra_list, int ptr_index) +{ + struct nxpwifi_ra_list_tbl *rlist; + + list_for_each_entry(rlist, &priv->wmm.tid_tbl_ptr[ptr_index].ra_list, + list) { + if (rlist == ra_list) + return true; + } + + return false; +} + +/* This function adds a packet to bypass TX queue. + * This is special TX queue for packets which can be sent even when port_open + * is false. + */ +void +nxpwifi_wmm_add_buf_bypass_txqueue(struct nxpwifi_private *priv, + struct sk_buff *skb) +{ + skb_queue_tail(&priv->bypass_txq, skb); +} + +/* This function adds a packet to WMM queue. + * + * In disconnected state the packet is immediately dropped and the + * packet send completion callback is called with status failure. + * + * Otherwise, the correct RA list node is located and the packet + * is queued at the list tail. + */ +void +nxpwifi_wmm_add_buf_txqueue(struct nxpwifi_private *priv, + struct sk_buff *skb) +{ + struct nxpwifi_adapter *adapter = priv->adapter; + u32 tid; + struct nxpwifi_ra_list_tbl *ra_list; + u8 ra[ETH_ALEN], tid_down; + struct ethhdr *eth_hdr = (struct ethhdr *)skb->data; + + memcpy(ra, eth_hdr->h_dest, ETH_ALEN); + + if (!priv->media_connected && !nxpwifi_is_skb_mgmt_frame(skb)) { + nxpwifi_dbg(adapter, DATA, "data: drop packet in disconnect\n"); + nxpwifi_write_data_complete(adapter, skb, 0, -1); + return; + } + + tid = skb->priority; + + spin_lock_bh(&priv->wmm.ra_list_spinlock); + + tid_down = nxpwifi_wmm_downgrade_tid(priv, tid); + + /* In case of infra as we have already created the list during + * association we just don't have to call get_queue_raptr, we will + * have only 1 raptr for a tid in case of infra + */ + memcpy(ra, skb->data, ETH_ALEN); + if (is_multicast_ether_addr(ra) || nxpwifi_is_skb_mgmt_frame(skb)) + eth_broadcast_addr(ra); + ra_list = nxpwifi_wmm_get_queue_raptr(priv, tid_down, ra); + + if (!ra_list) { + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + nxpwifi_write_data_complete(adapter, skb, 0, -1); + return; + } + + skb_queue_tail(&ra_list->skb_head, skb); + + ra_list->ba_pkt_count++; + ra_list->total_pkt_count++; + + if (atomic_read(&priv->wmm.highest_queued_prio) < + priv->tos_to_tid_inv[tid_down]) + atomic_set(&priv->wmm.highest_queued_prio, + priv->tos_to_tid_inv[tid_down]); + + if (ra_list->tx_paused) + priv->wmm.pkts_paused[tid_down]++; + else + atomic_inc(&priv->wmm.tx_pkts_queued); + + spin_unlock_bh(&priv->wmm.ra_list_spinlock); +} + +/* This function processes the get WMM status command response from firmware. + * + * The response may contain multiple TLVs - + * - AC Queue status TLVs + * - Current WMM Parameter IE TLV + * - Admission Control action frame TLVs + * + * This function parses the TLVs and then calls further specific functions + * to process any changes in the queue prioritize or state. + */ +int nxpwifi_ret_wmm_get_status(struct nxpwifi_private *priv, + const struct host_cmd_ds_command *resp) +{ + u8 *curr = (u8 *)&resp->params.get_wmm_status; + u16 resp_len = le16_to_cpu(resp->size), tlv_len; + int mask = IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK; + bool valid = true; + + struct nxpwifi_ie_types_data *tlv_hdr; + struct nxpwifi_ie_types_wmm_queue_status *wmm_qs; + struct ieee80211_wmm_param_ie *wmm_param_ie = NULL; + struct nxpwifi_wmm_ac_status *ac_status; + + nxpwifi_dbg(priv->adapter, INFO, + "info: WMM: WMM_GET_STATUS cmdresp received: %d\n", + resp_len); + + while ((resp_len >= sizeof(tlv_hdr->header)) && valid) { + tlv_hdr = (struct nxpwifi_ie_types_data *)curr; + tlv_len = le16_to_cpu(tlv_hdr->header.len); + + if (resp_len < tlv_len + sizeof(tlv_hdr->header)) + break; + + switch (le16_to_cpu(tlv_hdr->header.type)) { + case TLV_TYPE_WMMQSTATUS: + wmm_qs = (struct nxpwifi_ie_types_wmm_queue_status *) + tlv_hdr; + nxpwifi_dbg(priv->adapter, CMD, + "info: CMD_RESP: WMM_GET_STATUS:\t" + "QSTATUS TLV: %d, %d, %d\n", + wmm_qs->queue_index, + wmm_qs->flow_required, + wmm_qs->disabled); + + ac_status = &priv->wmm.ac_status[wmm_qs->queue_index]; + ac_status->disabled = wmm_qs->disabled; + ac_status->flow_required = wmm_qs->flow_required; + ac_status->flow_created = wmm_qs->flow_created; + break; + + case WLAN_EID_VENDOR_SPECIFIC: + /* Point the regular IEEE IE 2 bytes into the NXP IE + * and setup the IEEE IE type and length byte fields + */ + + wmm_param_ie = + (struct ieee80211_wmm_param_ie *)(curr + 2); + wmm_param_ie->len = (u8)tlv_len; + wmm_param_ie->element_id = WLAN_EID_VENDOR_SPECIFIC; + + nxpwifi_dbg(priv->adapter, CMD, + "info: CMD_RESP: WMM_GET_STATUS:\t" + "WMM Parameter Set Count: %d\n", + wmm_param_ie->qos_info & mask); + + if (wmm_param_ie->len + 2 > + sizeof(struct ieee80211_wmm_param_ie)) + break; + + memcpy(&priv->curr_bss_params.bss_descriptor.wmm_ie, + wmm_param_ie, wmm_param_ie->len + 2); + + break; + + default: + valid = false; + break; + } + + curr += (tlv_len + sizeof(tlv_hdr->header)); + resp_len -= (tlv_len + sizeof(tlv_hdr->header)); + } + + nxpwifi_wmm_setup_queue_priorities(priv, wmm_param_ie); + nxpwifi_wmm_setup_ac_downgrade(priv); + + return 0; +} + +/* Callback handler from the command module to allow insertion of a WMM TLV. + * + * If the BSS we are associating to supports WMM, this function adds the + * required WMM Information IE to the association request command buffer in + * the form of a NXP extended IEEE IE. + */ +u32 +nxpwifi_wmm_process_association_req(struct nxpwifi_private *priv, + u8 **assoc_buf, + struct ieee80211_wmm_param_ie *wmm_ie, + struct ieee80211_ht_cap *ht_cap) +{ + struct nxpwifi_ie_types_wmm_param_set *wmm_tlv; + u32 ret_len = 0; + + /* Null checks */ + if (!assoc_buf) + return 0; + if (!(*assoc_buf)) + return 0; + + if (!wmm_ie) + return 0; + + nxpwifi_dbg(priv->adapter, INFO, + "info: WMM: process assoc req: bss->wmm_ie=%#x\n", + wmm_ie->element_id); + + if ((priv->wmm_required || + (ht_cap && (priv->adapter->config_bands & BAND_GN || + priv->adapter->config_bands & BAND_AN))) && + wmm_ie->element_id == WLAN_EID_VENDOR_SPECIFIC) { + wmm_tlv = (struct nxpwifi_ie_types_wmm_param_set *)*assoc_buf; + wmm_tlv->header.type = cpu_to_le16((u16)wmm_info_ie[0]); + wmm_tlv->header.len = cpu_to_le16((u16)wmm_info_ie[1]); + memcpy(wmm_tlv->wmm_ie, &wmm_info_ie[2], + le16_to_cpu(wmm_tlv->header.len)); + if (wmm_ie->qos_info & IEEE80211_WMM_IE_AP_QOSINFO_UAPSD) + memcpy((u8 *)(wmm_tlv->wmm_ie + + le16_to_cpu(wmm_tlv->header.len) + - sizeof(priv->wmm_qosinfo)), + &priv->wmm_qosinfo, sizeof(priv->wmm_qosinfo)); + + ret_len = sizeof(wmm_tlv->header) + + le16_to_cpu(wmm_tlv->header.len); + + *assoc_buf += ret_len; + } + + return ret_len; +} + +/* This function computes the time delay in the driver queues for a + * given packet. + * + * When the packet is received at the OS/Driver interface, the current + * time is set in the packet structure. The difference between the present + * time and that received time is computed in this function and limited + * based on pre-compiled limits in the driver. + */ +u8 +nxpwifi_wmm_compute_drv_pkt_delay(struct nxpwifi_private *priv, + const struct sk_buff *skb) +{ + u32 queue_delay = ktime_to_ms(net_timedelta(skb->tstamp)); + u8 ret_val; + + /* Queue delay is passed as a uint8 in units of 2ms (ms shifted + * by 1). Min value (other than 0) is therefore 2ms, max is 510ms. + * + * Pass max value if queue_delay is beyond the uint8 range + */ + ret_val = (u8)(min(queue_delay, priv->wmm.drv_pkt_delay_max) >> 1); + + nxpwifi_dbg(priv->adapter, DATA, "data: WMM: Pkt Delay: %d ms,\t" + "%d ms sent to FW\n", queue_delay, ret_val); + + return ret_val; +} + +/* This function retrieves the highest priority RA list table pointer. + */ +static struct nxpwifi_ra_list_tbl * +nxpwifi_wmm_get_highest_priolist_ptr(struct nxpwifi_adapter *adapter, + struct nxpwifi_private **priv, int *tid) +{ + struct nxpwifi_private *priv_tmp; + struct nxpwifi_ra_list_tbl *ptr; + struct nxpwifi_tid_tbl *tid_ptr; + atomic_t *hqp; + int i, j; + u8 to_tid; + + /* check the BSS with highest priority first */ + for (j = adapter->priv_num - 1; j >= 0; --j) { + /* iterate over BSS with the equal priority */ + list_for_each_entry(adapter->bss_prio_tbl[j].bss_prio_cur, + &adapter->bss_prio_tbl[j].bss_prio_head, + list) { +try_again: + priv_tmp = adapter->bss_prio_tbl[j].bss_prio_cur->priv; + + if (!priv_tmp->port_open || + (atomic_read(&priv_tmp->wmm.tx_pkts_queued) == 0)) + continue; + + /* iterate over the WMM queues of the BSS */ + hqp = &priv_tmp->wmm.highest_queued_prio; + for (i = atomic_read(hqp); i >= LOW_PRIO_TID; --i) { + spin_lock_bh(&priv_tmp->wmm.ra_list_spinlock); + + to_tid = tos_to_tid[i]; + tid_ptr = &(priv_tmp)->wmm.tid_tbl_ptr[to_tid]; + + /* iterate over receiver addresses */ + list_for_each_entry(ptr, &tid_ptr->ra_list, + list) { + if (!ptr->tx_paused && + !skb_queue_empty(&ptr->skb_head)) + /* holds both locks */ + goto found; + } + + spin_unlock_bh(&priv_tmp->wmm.ra_list_spinlock); + } + + if (atomic_read(&priv_tmp->wmm.tx_pkts_queued) != 0) { + atomic_set(&priv_tmp->wmm.highest_queued_prio, + HIGH_PRIO_TID); + /* Iterate current private once more, since + * there still exist packets in data queue + */ + goto try_again; + } else { + atomic_set(&priv_tmp->wmm.highest_queued_prio, + NO_PKT_PRIO_TID); + } + } + } + + return NULL; + +found: + /* holds ra_list_spinlock */ + if (atomic_read(hqp) > i) + atomic_set(hqp, i); + spin_unlock_bh(&priv_tmp->wmm.ra_list_spinlock); + + *priv = priv_tmp; + *tid = tos_to_tid[i]; + + return ptr; +} + +/* This functions rotates ra and bss lists so packets are picked round robin. + * + * After a packet is successfully transmitted, rotate the ra list, so the ra + * next to the one transmitted, will come first in the list. This way we pick + * the ra' in a round robin fashion. Same applies to bss nodes of equal + * priority. + * + * Function also increments wmm.packets_out counter. + */ +void nxpwifi_rotate_priolists(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ra, + int tid) +{ + struct nxpwifi_adapter *adapter = priv->adapter; + struct nxpwifi_bss_prio_tbl *tbl = adapter->bss_prio_tbl; + struct nxpwifi_tid_tbl *tid_ptr = &priv->wmm.tid_tbl_ptr[tid]; + + spin_lock_bh(&tbl[priv->bss_priority].bss_prio_lock); + /* dirty trick: we remove 'head' temporarily and reinsert it after + * curr bss node. imagine list to stay fixed while head is moved + */ + list_move(&tbl[priv->bss_priority].bss_prio_head, + &tbl[priv->bss_priority].bss_prio_cur->list); + spin_unlock_bh(&tbl[priv->bss_priority].bss_prio_lock); + + spin_lock_bh(&priv->wmm.ra_list_spinlock); + if (nxpwifi_is_ralist_valid(priv, ra, tid)) { + priv->wmm.packets_out[tid]++; + /* same as above */ + list_move(&tid_ptr->ra_list, &ra->list); + } + spin_unlock_bh(&priv->wmm.ra_list_spinlock); +} + +/* This function checks if 11n aggregation is possible. + */ +static bool +nxpwifi_is_11n_aggragation_possible(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ptr, + int max_buf_size) +{ + int count = 0, total_size = 0; + struct sk_buff *skb, *tmp; + int max_amsdu_size; + + if (priv->bss_role == NXPWIFI_BSS_ROLE_UAP && priv->ap_11n_enabled && + ptr->is_11n_enabled) + max_amsdu_size = min_t(int, ptr->max_amsdu, max_buf_size); + else + max_amsdu_size = max_buf_size; + + skb_queue_walk_safe(&ptr->skb_head, skb, tmp) { + total_size += skb->len; + if (total_size >= max_amsdu_size) + break; + if (++count >= MIN_NUM_AMSDU) + return true; + } + + return false; +} + +/* This function sends a single packet to firmware for transmission. + */ +static void +nxpwifi_send_single_packet(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ptr, int ptr_index) +__releases(&priv->wmm.ra_list_spinlock) +{ + struct sk_buff *skb, *skb_next; + struct nxpwifi_tx_param tx_param; + struct nxpwifi_adapter *adapter = priv->adapter; + struct nxpwifi_txinfo *tx_info; + + if (skb_queue_empty(&ptr->skb_head)) { + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + nxpwifi_dbg(adapter, DATA, "data: nothing to send\n"); + return; + } + + skb = skb_dequeue(&ptr->skb_head); + + tx_info = NXPWIFI_SKB_TXCB(skb); + nxpwifi_dbg(adapter, DATA, + "data: dequeuing the packet %p %p\n", ptr, skb); + + ptr->total_pkt_count--; + + if (!skb_queue_empty(&ptr->skb_head)) + skb_next = skb_peek(&ptr->skb_head); + else + skb_next = NULL; + + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + + tx_param.next_pkt_len = ((skb_next) ? skb_next->len + + sizeof(struct txpd) : 0); + + if (nxpwifi_process_tx(priv, skb, &tx_param) == -EBUSY) { + /* Queue the packet back at the head */ + spin_lock_bh(&priv->wmm.ra_list_spinlock); + + if (!nxpwifi_is_ralist_valid(priv, ptr, ptr_index)) { + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + nxpwifi_write_data_complete(adapter, skb, 0, -1); + return; + } + + skb_queue_tail(&ptr->skb_head, skb); + + ptr->total_pkt_count++; + ptr->ba_pkt_count++; + tx_info->flags |= NXPWIFI_BUF_FLAG_REQUEUED_PKT; + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + } else { + nxpwifi_rotate_priolists(priv, ptr, ptr_index); + atomic_dec(&priv->wmm.tx_pkts_queued); + } +} + +/* This function checks if the first packet in the given RA list + * is already processed or not. + */ +static bool +nxpwifi_is_ptr_processed(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ptr) +{ + struct sk_buff *skb; + struct nxpwifi_txinfo *tx_info; + + if (skb_queue_empty(&ptr->skb_head)) + return false; + + skb = skb_peek(&ptr->skb_head); + + tx_info = NXPWIFI_SKB_TXCB(skb); + if (tx_info->flags & NXPWIFI_BUF_FLAG_REQUEUED_PKT) + return true; + + return false; +} + +/* This function sends a single processed packet to firmware for + * transmission. + */ +static void +nxpwifi_send_processed_packet(struct nxpwifi_private *priv, + struct nxpwifi_ra_list_tbl *ptr, int ptr_index) + __releases(&priv->wmm.ra_list_spinlock) +{ + struct nxpwifi_tx_param tx_param; + struct nxpwifi_adapter *adapter = priv->adapter; + int ret; + struct sk_buff *skb, *skb_next; + struct nxpwifi_txinfo *tx_info; + + if (skb_queue_empty(&ptr->skb_head)) { + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + return; + } + + skb = skb_dequeue(&ptr->skb_head); + + if (adapter->data_sent || adapter->tx_lock_flag) { + ptr->total_pkt_count--; + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + skb_queue_tail(&adapter->tx_data_q, skb); + atomic_dec(&priv->wmm.tx_pkts_queued); + atomic_inc(&adapter->tx_queued); + return; + } + + if (!skb_queue_empty(&ptr->skb_head)) + skb_next = skb_peek(&ptr->skb_head); + else + skb_next = NULL; + + tx_info = NXPWIFI_SKB_TXCB(skb); + + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + + tx_param.next_pkt_len = + ((skb_next) ? skb_next->len + + sizeof(struct txpd) : 0); + + ret = adapter->if_ops.host_to_card(adapter, NXPWIFI_TYPE_DATA, + skb, &tx_param); + + switch (ret) { + case -EBUSY: + nxpwifi_dbg(adapter, ERROR, "data: -EBUSY is returned\n"); + spin_lock_bh(&priv->wmm.ra_list_spinlock); + + if (!nxpwifi_is_ralist_valid(priv, ptr, ptr_index)) { + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + nxpwifi_write_data_complete(adapter, skb, 0, -1); + return; + } + + skb_queue_tail(&ptr->skb_head, skb); + + tx_info->flags |= NXPWIFI_BUF_FLAG_REQUEUED_PKT; + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + break; + case -EINPROGRESS: + break; + case 0: + nxpwifi_write_data_complete(adapter, skb, 0, ret); + break; + default: + nxpwifi_dbg(adapter, ERROR, "host_to_card failed: %#x\n", ret); + adapter->dbg.num_tx_host_to_card_failure++; + nxpwifi_write_data_complete(adapter, skb, 0, ret); + break; + } + + if (ret != -EBUSY) { + nxpwifi_rotate_priolists(priv, ptr, ptr_index); + atomic_dec(&priv->wmm.tx_pkts_queued); + spin_lock_bh(&priv->wmm.ra_list_spinlock); + ptr->total_pkt_count--; + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + } +} + +/* This function dequeues a packet from the highest priority list + * and transmits it. + */ +static int +nxpwifi_dequeue_tx_packet(struct nxpwifi_adapter *adapter) +{ + struct nxpwifi_ra_list_tbl *ptr; + struct nxpwifi_private *priv = NULL; + int ptr_index = 0; + u8 ra[ETH_ALEN]; + int tid_del = 0, tid = 0; + + ptr = nxpwifi_wmm_get_highest_priolist_ptr(adapter, &priv, &ptr_index); + if (!ptr) + return -ENOENT; + + tid = nxpwifi_get_tid(ptr); + + nxpwifi_dbg(adapter, DATA, "data: tid=%d\n", tid); + + spin_lock_bh(&priv->wmm.ra_list_spinlock); + if (!nxpwifi_is_ralist_valid(priv, ptr, ptr_index)) { + spin_unlock_bh(&priv->wmm.ra_list_spinlock); + return -EINVAL; + } + + if (nxpwifi_is_ptr_processed(priv, ptr)) { + nxpwifi_send_processed_packet(priv, ptr, ptr_index); + /* ra_list_spinlock has been freed in + * nxpwifi_send_processed_packet() + */ + return 0; + } + + if (!ptr->is_11n_enabled || + ptr->ba_status || + priv->wps.session_enable) { + if (ptr->is_11n_enabled && + ptr->ba_status && + ptr->amsdu_in_ampdu && + nxpwifi_is_amsdu_allowed(priv, tid) && + nxpwifi_is_11n_aggragation_possible(priv, ptr, + adapter->tx_buf_size)) + nxpwifi_11n_aggregate_pkt(priv, ptr, ptr_index); + /* ra_list_spinlock has been freed in + * nxpwifi_11n_aggregate_pkt() + */ + else + nxpwifi_send_single_packet(priv, ptr, ptr_index); + /* ra_list_spinlock has been freed in + * nxpwifi_send_single_packet() + */ + } else { + if (nxpwifi_is_ampdu_allowed(priv, ptr, tid) && + ptr->ba_pkt_count > ptr->ba_packet_thr) { + if (nxpwifi_space_avail_for_new_ba_stream(adapter)) { + nxpwifi_create_ba_tbl(priv, ptr->ra, tid, + BA_SETUP_INPROGRESS); + nxpwifi_send_addba(priv, tid, ptr->ra); + } else if (nxpwifi_find_stream_to_delete + (priv, tid, &tid_del, ra)) { + nxpwifi_create_ba_tbl(priv, ptr->ra, tid, + BA_SETUP_INPROGRESS); + nxpwifi_send_delba(priv, tid_del, ra, 1); + } + } + if (nxpwifi_is_amsdu_allowed(priv, tid) && + nxpwifi_is_11n_aggragation_possible(priv, ptr, + adapter->tx_buf_size)) + nxpwifi_11n_aggregate_pkt(priv, ptr, ptr_index); + /* ra_list_spinlock has been freed in + * nxpwifi_11n_aggregate_pkt() + */ + else + nxpwifi_send_single_packet(priv, ptr, ptr_index); + /* ra_list_spinlock has been freed in + * nxpwifi_send_single_packet() + */ + } + return 0; +} + +void nxpwifi_process_bypass_tx(struct nxpwifi_adapter *adapter) +{ + struct nxpwifi_tx_param tx_param; + struct sk_buff *skb; + struct nxpwifi_txinfo *tx_info; + struct nxpwifi_private *priv; + int i; + + if (adapter->data_sent || adapter->tx_lock_flag) + return; + + for (i = 0; i < adapter->priv_num; ++i) { + priv = adapter->priv[i]; + + if (!priv) + continue; + + if (skb_queue_empty(&priv->bypass_txq)) + continue; + + skb = skb_dequeue(&priv->bypass_txq); + tx_info = NXPWIFI_SKB_TXCB(skb); + + /* no aggregation for bypass packets */ + tx_param.next_pkt_len = 0; + + if (nxpwifi_process_tx(priv, skb, &tx_param) == -EBUSY) { + skb_queue_head(&priv->bypass_txq, skb); + tx_info->flags |= NXPWIFI_BUF_FLAG_REQUEUED_PKT; + } else { + atomic_dec(&adapter->bypass_tx_pending); + } + } +} + +/* This function transmits the highest priority packet awaiting in the + * WMM Queues. + */ +void +nxpwifi_wmm_process_tx(struct nxpwifi_adapter *adapter) +{ + do { + if (nxpwifi_dequeue_tx_packet(adapter)) + break; + if (adapter->iface_type != NXPWIFI_SDIO) { + if (adapter->data_sent || + adapter->tx_lock_flag) + break; + } else { + if (atomic_read(&adapter->tx_queued) >= + NXPWIFI_MAX_PKTS_TXQ) + break; + } + } while (!nxpwifi_wmm_lists_empty(adapter)); +}