From patchwork Thu Jun 17 15:59:28 2021 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Viktor Barna X-Patchwork-Id: 12328449 X-Patchwork-Delegate: kvalo@adurom.com Return-Path: X-Spam-Checker-Version: SpamAssassin 3.4.0 (2014-02-07) on aws-us-west-2-korg-lkml-1.web.codeaurora.org X-Spam-Level: X-Spam-Status: No, score=-18.8 required=3.0 tests=BAYES_00,DKIM_SIGNED, DKIM_VALID,DKIM_VALID_AU,HEADER_FROM_DIFFERENT_DOMAINS,INCLUDES_CR_TRAILER, INCLUDES_PATCH,MAILING_LIST_MULTI,MSGID_FROM_MTA_HEADER,SPF_HELO_NONE, SPF_PASS,USER_AGENT_GIT autolearn=ham autolearn_force=no version=3.4.0 Received: from mail.kernel.org (mail.kernel.org [198.145.29.99]) by smtp.lore.kernel.org (Postfix) with ESMTP id D6D85C49361 for ; Thu, 17 Jun 2021 16:05:58 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [23.128.96.18]) by mail.kernel.org (Postfix) with ESMTP id B50AA61405 for ; Thu, 17 Jun 2021 16:05:58 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S233651AbhFQQIF (ORCPT ); Thu, 17 Jun 2021 12:08:05 -0400 Received: from mail-db8eur05on2082.outbound.protection.outlook.com ([40.107.20.82]:7360 "EHLO EUR05-DB8-obe.outbound.protection.outlook.com" rhost-flags-OK-OK-OK-FAIL) by vger.kernel.org with ESMTP id S232173AbhFQQHZ (ORCPT ); Thu, 17 Jun 2021 12:07:25 -0400 ARC-Seal: i=1; a=rsa-sha256; s=arcselector9901; d=microsoft.com; cv=none; b=mvmNfc65manZ2G6E56hqHmewdc5Q3+0P0mxSs+Yn+bMRkHoKQYe+iAEv0bn6FNtw92WAsjjlfHJcHpkewqlSBB46El907JReZV3Nqw67MtwkbJRB8+ElczNKHSY2HlXV3nRXv1Jp/IHaqyEWDV6waekoWKjtp+yam0q3UPrmSTcC38Bj5DlsHe6JvIbz50HCD385AdvfQ3peTwnlyBPG6FFFS+HG6/Mic4BUWSdeMuT8q1oLTpqq2YKOXsZGk+W/Ma7jEqJq4ed7wqoKgsFOi7duGq6plnbZrNQWwzsMgJwwayG+HfDfcV//6+EB52taf4jWAKmCtaYUq9xU5xDaHA== ARC-Message-Signature: i=1; a=rsa-sha256; c=relaxed/relaxed; d=microsoft.com; s=arcselector9901; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-SenderADCheck; bh=IOAWgpwNu7L3cmaTieRwbtEGfwuiSBFRwutyjFL1ksU=; b=UAAuW40tJ5PxBmetOpzWAhQlRJfFq65XLV6ijJp6wNxtom9nN3PK0QB+uUQjDjOcxKh63Fnhqz4lyiJ8R59qZHqiNHE2clXqLyypVYj0KlZ3WggWoyNbfHLqzGYj4quqXIOu+cQFWzdLxl/+jhHC4HQorKd8GKhFikPBa7bkQYHK3GJz7F03GnGzWDANhlzNH+96p4uoSLDtptE0j25q1SqK8ALNAJipfNjkfpkJuVam750QXh6q130UhYoel5zxPPt2XEiqAryE4Z6UmNxYPIn5cTVVILPuXDvkCD2ex0WD63jMRp85DRyGaz2Rh+hxWm2C0kgY1zm8avRfmTVaQw== ARC-Authentication-Results: i=1; mx.microsoft.com 1; spf=pass smtp.mailfrom=celeno.com; dmarc=pass action=none header.from=celeno.com; dkim=pass header.d=celeno.com; arc=none DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=celeno.com; s=selector2; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-SenderADCheck; bh=IOAWgpwNu7L3cmaTieRwbtEGfwuiSBFRwutyjFL1ksU=; b=CVV4KArlxOK7PwAef7AdAddzZfZsmbxfe9hBCygkan9d/bTbGk2n8ylOJHtCNyWQ6QOqSLMMbT3ee7Vc8BMqqmYe1GVTQ6+rsGjKVv5hjgBfRUqC2aB7SiKtI8QEGaZ54vlyUuIm+V+uCX7/qAKVCEnipyLEh9olBrPVC1PkqUI= Authentication-Results: vger.kernel.org; dkim=none (message not signed) header.d=none;vger.kernel.org; dmarc=none action=none header.from=celeno.com; Received: from AM9P192MB1412.EURP192.PROD.OUTLOOK.COM (2603:10a6:20b:38b::16) by AM9P192MB0871.EURP192.PROD.OUTLOOK.COM (2603:10a6:20b:1fb::6) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4242.21; Thu, 17 Jun 2021 16:04:26 +0000 Received: from AM9P192MB1412.EURP192.PROD.OUTLOOK.COM ([fe80::1847:5583:4db7:102f]) by AM9P192MB1412.EURP192.PROD.OUTLOOK.COM ([fe80::1847:5583:4db7:102f%4]) with mapi id 15.20.4242.021; Thu, 17 Jun 2021 16:04:26 +0000 From: viktor.barna@celeno.com To: linux-wireless@vger.kernel.org Cc: Kalle Valo , "David S . Miller" , Jakub Kicinski , Aviad Brikman , Eliav Farber , Oleksandr Savchenko , Shay Bar , Viktor Barna Subject: [RFC v1 081/256] cl8k: add ext/vlan_dscp.c Date: Thu, 17 Jun 2021 15:59:28 +0000 Message-Id: <20210617160223.160998-82-viktor.barna@celeno.com> X-Mailer: git-send-email 2.27.0 In-Reply-To: <20210617160223.160998-1-viktor.barna@celeno.com> References: <20210617160223.160998-1-viktor.barna@celeno.com> X-Originating-IP: [62.216.42.54] X-ClientProxiedBy: PR3PR09CA0018.eurprd09.prod.outlook.com (2603:10a6:102:b7::23) To AM9P192MB1412.EURP192.PROD.OUTLOOK.COM (2603:10a6:20b:38b::16) MIME-Version: 1.0 X-MS-Exchange-MessageSentRepresentingType: 1 Received: from localhost.localdomain (62.216.42.54) by PR3PR09CA0018.eurprd09.prod.outlook.com (2603:10a6:102:b7::23) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4242.18 via Frontend Transport; Thu, 17 Jun 2021 16:04:01 +0000 X-MS-PublicTrafficType: Email X-MS-Office365-Filtering-Correlation-Id: 719eccc2-ade0-43de-77e1-08d931a9864d X-MS-TrafficTypeDiagnostic: AM9P192MB0871: X-MS-Exchange-Transport-Forked: True X-Microsoft-Antispam-PRVS: X-MS-Oob-TLC-OOBClassifiers: OLM:126; X-MS-Exchange-SenderADCheck: 1 X-Microsoft-Antispam: BCL:0; X-Microsoft-Antispam-Message-Info: IFwQrDI4WNjBOS4iHl7PYuhsnBW3w1vh9xJmyU5Do+3IyUOyZHCyBFBnYh9EsbNV3YOSUM6/qXn8d7rIo2m3AvOK+2TpjKIXLy+b5NH2sUtu+tb0V+MrIKHEG/txkbuTZjrxLsZp77pg/skkYOzwtGavTiElRXWNVDQBletReH1A5B+FIBShy02pRqqhvvD7jCMrPiQ1jC4FkwvvMNtBf57OIBOXefpmt+l/Yv/+agIU5edIV9koA75yaKflEd7LQ1uWOixMGIG47F+QmCsBO2VcfhYVG96ujZXA3q+2VGZZx1wdeEADwhECwWegwy9dvh/NS0skOtCzuIJBBTLej7E57i7qXnoA5PB1jcdoBSy02DX+VnirfvXJCqC0fqp2dK2A58DHkDgvgsqj0RdmMbNiRz/jOxb2E1JeG/oaUm6Yh7stCsWPNpoqp5ddvLKLOTR4esrVlL5sbIf7LXZ6Q/1xt1C93O1n667oMVv9T3jzSebwf/4hAYshnHMLYOy8MPp/9C/BknwqZ6BRyvdDF4hz1wCdch0/xVE7cgSIYtfWQgy2Cde7UuJU2F1On4u4hCgwI/9KaywJqf0DiyVmsFSNgQ0/meNbNxd0QHpaVJMmg7ozLJZ6xOoCa0Sh3xS27vVXWs6Bf7RbxvNfDamf8kgCIhiySYKhogvYS99ZIE4QPaiJWDUo1JiMb8XqavWtKdxf9ZBd9TWvWLNnSXUZvQ== X-Forefront-Antispam-Report: CIP:255.255.255.255;CTRY:;LANG:en;SCL:1;SRV:;IPV:NLI;SFV:NSPM;H:AM9P192MB1412.EURP192.PROD.OUTLOOK.COM;PTR:;CAT:NONE;SFS:(136003)(366004)(376002)(346002)(396003)(39840400004)(16526019)(66946007)(478600001)(26005)(66476007)(30864003)(6486002)(186003)(66556008)(9686003)(316002)(86362001)(6506007)(6666004)(6512007)(55236004)(38350700002)(1076003)(38100700002)(4326008)(8676002)(83380400001)(5660300002)(52116002)(2616005)(2906002)(6916009)(956004)(8936002)(107886003)(54906003)(36756003)(69590400013)(32563001);DIR:OUT;SFP:1101; X-MS-Exchange-AntiSpam-MessageData-ChunkCount: 1 X-MS-Exchange-AntiSpam-MessageData-0: KBRrPd4OX249zFCcfRJtVsMrONfIWxFqPgmEHoS6c9y0jfZBGNTN7SWZFgxcl2xDMMLeVBzatSER/7BgUAiVeFx+EkoDWiV7gBckPZDIoHCX05P9lTavvpT6VJULnOfFdoxqBCmbvupR44WIHq/kku21e0Lg0/whp01qnOvPU5eVNMF2NwU3sWZ8shiVzRCQYRhHnk9B85T0FZRxdxPUhSfSDBFFZB0VEKzpnulk0zWxiUJ1RO2MYpuk1kq2a+iEWEHbu0hO2VVuYJPC1NfymHEJm+ztIomEWB68dnJdSFDLhgbcFeSYzm6rAmAs+XSXOO+3X3fwFNB9kR1JsSm/ZUbhDbK1PcL7qJI45pFfuv/BtRj0R6PkIVGsA+KwKkrfwP7M7Bc88/0OIGPKLb2OVKgdIcpa+xdYf7dz5g/ycw7+6Boq4Y/3gUnLzovOxQwA5tRIUn/Oo4G1m5cxAz7croqPTiOkAGZ+XFTpFAat831s4OT3A7EHzgteZ8Ob9/QdOd5xSdZO4v01BQtEhQVEhLdoRVbTKL4wmwwn67mxuzaAXUZyQV983YWT5m7gnzSsV2PPwZbxdA2pnsrZN4t3ZGgYWZEIpvKL+oIEKZRIpeNHyvEgSlTVEYVLnZhHLsmR2lRho9E75zeFrYc1eu3sst5B6u9TNacB8BQKZDdCtfg4MSWFU2P9gnP97JT5SWIHnWypgA6TVfY7KLwgFGNYw5rfYBgXgzXpN1VnxZosjByUU1neV9dAxt2Z+nsC1wlfUIYJl0PruSuxnc2QZX+fcT9P7bnDg3Er6HI4Rk7v7RaVh09HIFG0z1Lr6yJFn71qJmjcSZU6hpssQhYmR+FvepG5uQQUEQqDb/UDTLoe1Kt3k9aS6bUvjdxZhMmuqBA0gyLVZMlGAmBHx5wlpOnYa/rahIZ1+n226ZFQcVcysUQ+27LC2OacuMvk248yDTEpO0j9sGcYILtjE3XWJg8Inq0rJDgs+obY0Qb1y1IAAueezzVwPqkMX+YxE/J4iHpOauoE9I6An2kXYL6sPPbYcfBSx3845obPYVcGrJSFIkjumPcmn/czYgvAU3RThj1/xXQKfKW/Qjk/u9H9oXLCjjNJTmmBJEYg8MpjHezJpvgsgMYyjBa5Rx5IIqyQRjr43dmDkC0+NYIk11KogRnXDDpZpgwUi+uyMoLJ361QAD5qIv0Dxn6Y97JjdBv/ZIcgnE5qhuFQCmf8P0SQWyKQdtSZ9gS++yBEwubPESR1YMB/CRJJqnwXOQJN78I/qMw7fllxJ0XK5j8Pj/ZrE//k9wJ845iBl9wjm++vFqA/HDu8YpHwIQXbYjRBF2onMX2v X-OriginatorOrg: celeno.com X-MS-Exchange-CrossTenant-Network-Message-Id: 719eccc2-ade0-43de-77e1-08d931a9864d X-MS-Exchange-CrossTenant-AuthSource: AM9P192MB1412.EURP192.PROD.OUTLOOK.COM X-MS-Exchange-CrossTenant-AuthAs: Internal X-MS-Exchange-CrossTenant-OriginalArrivalTime: 17 Jun 2021 16:04:02.4503 (UTC) X-MS-Exchange-CrossTenant-FromEntityHeader: Hosted X-MS-Exchange-CrossTenant-Id: f313103b-4c9f-4fd3-b5cf-b97f91c4afa8 X-MS-Exchange-CrossTenant-MailboxType: HOSTED X-MS-Exchange-CrossTenant-UserPrincipalName: LXW7JnLcuPl685Nl/2C5baipqgHGGFTXagFT7EhQH6O8Aejt1r2ovmSp4/1j/dlp25XNuPNklm53Nu1OfJWTEA== X-MS-Exchange-Transport-CrossTenantHeadersStamped: AM9P192MB0871 Precedence: bulk List-ID: X-Mailing-List: linux-wireless@vger.kernel.org From: Viktor Barna (Part of the split. Please, take a look at the cover letter for more details). Signed-off-by: Viktor Barna --- .../net/wireless/celeno/cl8k/ext/vlan_dscp.c | 658 ++++++++++++++++++ 1 file changed, 658 insertions(+) create mode 100644 drivers/net/wireless/celeno/cl8k/ext/vlan_dscp.c -- 2.30.0 diff --git a/drivers/net/wireless/celeno/cl8k/ext/vlan_dscp.c b/drivers/net/wireless/celeno/cl8k/ext/vlan_dscp.c new file mode 100644 index 000000000000..9942d5599ffb --- /dev/null +++ b/drivers/net/wireless/celeno/cl8k/ext/vlan_dscp.c @@ -0,0 +1,658 @@ +// SPDX-License-Identifier: MIT +/* Copyright(c) 2019-2021, Celeno Communications Ltd. */ + +#include +#include + +#include "ext/vlan_dscp.h" +#include "utils/ip.h" +#include "vif.h" +#include "hw.h" +#include "utils/utils.h" + +#define VLAN_DSCP_DBG(...) \ + do { \ + if (cl_hw->vlan_dscp.debug) \ + pr_debug(__VA_ARGS__); \ + } while (0) + +static u8 get_vlan_pbit_up(struct cl_hw *cl_hw, u8 pbit_val, u16 vlan_id_val, u8 ap_idx) +{ + /* Get UP from VID + priority bits */ + u8 user_priority = 0; + u8 pbit_found = 0; + struct cl_vid_user *vid_user; + struct cl_vlan_dhcp_params *vlan_dhcp_params = + &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + int j; + + for (j = 0; j < CL_USER_PRIO_VALS; j++) { + vid_user = &vlan_dhcp_params->vlan_pbit_to_up[pbit_val][j]; + if (vid_user->vid == 0) + break; /* End of vid list in this Pbit raw */ + + if (vlan_id_val == vid_user->vid) { + user_priority = vid_user->user_prio; + pbit_found = 1; + break; + } + } + + /* Use vlan_to_up mapping */ + if (pbit_found == 0) + user_priority = vlan_dhcp_params->vlan_to_up[pbit_val]; + + VLAN_DSCP_DBG("[%s] vlan_id_val=%u, pbit_val=%u, ap_idx=%u, user_priority=%u\n", + __func__, vlan_id_val, pbit_val, ap_idx, user_priority); + + return user_priority; +} + +static u8 get_ip_user_priority(struct cl_hw *cl_hw, u8 *src_buf, u16 protocol, u8 ap_idx) +{ + /* Get UP from DSCP value */ + u8 user_priority = cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].dscp_to_up[0]; + u8 dscp_val = 0; + + if (protocol == ETH_P_IP) /* IPv4 */ + dscp_val = (*(src_buf + 1) & 0xfc) >> 2; + else + dscp_val = (ntohs(*(u16 *)src_buf) >> 6) & 0x3f; + + if (dscp_val > 63) { + VLAN_DSCP_DBG("[%s] bad DSCP value = %u\n", __func__, dscp_val); + return user_priority; + } + + /* Look for priority in the DSCP array */ + user_priority = cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].dscp_to_up[dscp_val]; + if (user_priority == U8_MAX) { + /* Value wasn't found in the array. use regular parsing */ + if (protocol == ETH_P_IP) /* IPv4 */ + user_priority = (*(src_buf + 1) & 0xe0) >> 5; + else /* IPv6 */ + user_priority = ((*src_buf) & 0x0e) >> 1; + } + + VLAN_DSCP_DBG("[%s] dscp_val=%u, user_priority=%u, ap_idx=%u\n", + __func__, dscp_val, user_priority, ap_idx); + + return user_priority; +} + +static int print_configuration(struct cl_hw *cl_hw, u8 ap_idx) +{ + /* Print all the configuration parameters for specific AP */ + struct cl_vlan_dscp *dscp = &cl_hw->vlan_dscp; + struct cl_vlan_dhcp_params *param = &dscp->vlan_dhcp_params[ap_idx]; + u8 i, j; + char *buf = NULL; + ssize_t buf_size; + int err = 0; + int len = 0; + + cl_snprintf(&buf, &len, &buf_size, + "=====================================\n"); + cl_snprintf(&buf, &len, &buf_size, + "AP %u\n", ap_idx); + cl_snprintf(&buf, &len, &buf_size, + "=====================================\n"); + cl_snprintf(&buf, &len, &buf_size, + "enable = %s\n", dscp->enable[ap_idx] ? "True" : "False"); + cl_snprintf(&buf, &len, &buf_size, + "up_layer_based = %u [0-AUTO 2-VLAN 3-DSCP]\n", param->up_layer_based); + cl_snprintf(&buf, &len, &buf_size, + "default_vlan_up = %u\n", param->default_vlan_up); + + cl_snprintf(&buf, &len, &buf_size, "\ndscp_to_up:\n"); + + for (i = 0; i < CL_USER_DSCP_VALS; i++) { + cl_snprintf(&buf, &len, &buf_size, + "dscp=%u up=%u", i, param->dscp_to_up[i]); + if ((i + 1) % 8 == 0) + cl_snprintf(&buf, &len, &buf_size, "\n"); + else + cl_snprintf(&buf, &len, &buf_size, "\t"); + } + + cl_snprintf(&buf, &len, &buf_size, "\n\nvlan_to_up:\n"); + for (i = 0; i < CL_USER_PRIO_VALS; i++) + cl_snprintf(&buf, &len, &buf_size, + "%3u ", param->vlan_to_up[i]); + + cl_snprintf(&buf, &len, &buf_size, "\n\nvlan_pbit_to_up:\n"); + for (i = 0; i < CL_USER_PRIO_VALS; i++) { + cl_snprintf(&buf, &len, &buf_size, "pbit %u\t", i); + for (j = 0; j < CL_USER_PRIO_VALS; j++) + cl_snprintf(&buf, &len, &buf_size, + "v=%u,up=%u\t", + param->vlan_pbit_to_up[i][j].vid, + param->vlan_pbit_to_up[i][j].user_prio); + + cl_snprintf(&buf, &len, &buf_size, "\n"); + } + + err = cl_vendor_reply(cl_hw, buf, len); + kfree(buf); + + return err; +} + +static void read_dscp_parms_format1(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + int i; + int num_of_str; + char *tok_ptr; + char *mac_ptr; + char *str_ptr[CL_USER_DSCP_VALS]; + u8 dscp; + u8 user_prio; + u32 val; + struct cl_vlan_dhcp_params *vlan_dhcp_params = + &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + + mac_ptr = strsep(&buffer, ";"); + for (num_of_str = 0; mac_ptr && (num_of_str < ARRAY_SIZE(str_ptr)); num_of_str++) { + str_ptr[num_of_str] = mac_ptr; + mac_ptr = strsep(&buffer, ";"); + } + + for (i = 0; i < num_of_str; i++) { + val = U32_MAX; + dscp = U8_MAX; + user_prio = U8_MAX; + + /* Get DSCP 0-63 */ + tok_ptr = strsep(&str_ptr[i], ","); + + if (!tok_ptr) { + pr_err("dscp_to_up0_7dec: dscp null\n"); + continue; + } + if (kstrtou32(tok_ptr, 10, &val) != 0) { + pr_err("dscp_to_up0_7dec: invalid dscp (%s)\n", tok_ptr); + continue; + } + if (val > 63) { + pr_err("dscp_to_up0_7dec: dscp exceeds 63 (%u)\n", val); + continue; + } + + dscp = (u8)val; + + /* Get user priority */ + val = U32_MAX; + tok_ptr = strsep(&str_ptr[i], ","); + + if (!tok_ptr) { + pr_err("dscp_to_up0_7dec: up null\n"); + continue; + } + if (kstrtou32(tok_ptr, 10, &val) != 0) { + pr_err("dscp_to_up0_7dec: invalid up (%s)\n", tok_ptr); + continue; + } + if (val > 7) { + pr_err("dscp_to_up0_7dec: up exceeds 7 (%u)\n", val); + continue; + } + + user_prio = (u8)val; + + /* At this point the 2 parameters are valid: dscp, and user_prio */ + if (vlan_dhcp_params->dscp_to_up[dscp] == U8_MAX) + /* First time initialization for this dscp */ + vlan_dhcp_params->dscp_to_up[dscp] = user_prio; + else + pr_warn("dscp_to_up0_7dec[%u] is already set with user_prio %u." + "Will not set it with a different user_prio %u\n", + dscp, vlan_dhcp_params->dscp_to_up[dscp], user_prio); + } +} + +static void read_dscp_parms_format2(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + int i; + char *mac_ptr; + u8 dscp_val; + struct cl_vlan_dhcp_params *vlan_dhcp_params = + &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + + for (i = 0, mac_ptr = strsep(&buffer, ","); mac_ptr; mac_ptr = strsep(&buffer, ","), i++) { + if (i > (CL_USER_PRIO_VALS - 1)) { + pr_err("dscp_to_up0_7dec: DSCP exceeds %d entries!" + "Remaining DSCP entries ignored\n", CL_USER_PRIO_VALS); + break; + } + + if (kstrtou8(mac_ptr, 10, &dscp_val) != 0) { + pr_err("dscp_to_up0_7dec: Invalid dscp (%s)\n", mac_ptr); + continue; + } + + if (dscp_val > 63) { + pr_err("dscp_to_up0_7dec: illegal dscp value %u." + "Converted to %u\n", dscp_val, (dscp_val & 0x3F)); + dscp_val &= 0x3F; + } + + vlan_dhcp_params->dscp_to_up[dscp_val] = i; + } + + if (i < (CL_USER_PRIO_VALS - 1)) + pr_warn("dscp_to_up0_7dec: only %d first DSCP entries filled in." + "Remaining DSCP entries will remain unassigned\n", i + 1); +} + +static void read_dscp_parms_from_buffer(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + /* + * Initiate configuration per AP index by buffer by + * configuration parameter dscp_to_up0_7dec + */ + char *pch; + struct cl_vlan_dhcp_params *vlan_dhcp_params = + &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + + if (!buffer || (strlen(buffer) == 0)) + return; + + /* Initially fill with illegal dscp value */ + memset(vlan_dhcp_params->dscp_to_up, U8_MAX, CL_USER_DSCP_VALS); + + pch = strchr(buffer, ';'); + if (pch) + /* Handle the follow format dscp_val1,up;dscp_val2,up;dscp_val3,up;... */ + read_dscp_parms_format1(cl_hw, buffer, ap_idx); + else + /* Handle the follow format dscp_to_up_0,dscp_to_up_1,dscp_to_up_2,... */ + read_dscp_parms_format2(cl_hw, buffer, ap_idx); +} + +static void read_vlan_parms_format1(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + int i, j, num_of_str; + char *mac_ptr, *tok_ptr; + char *str_ptr[CL_USER_PRIO_VALS * CL_USER_PRIO_VALS]; + u8 pbit, user_prio; + u8 helper[CL_USER_PRIO_VALS]; + u16 vid; + u32 val; + struct cl_vlan_dhcp_params *vlan_dhcp_params = &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + + /*Initialize array */ + for (i = 0; i < CL_USER_PRIO_VALS; i++) { + for (j = 0; j < CL_USER_PRIO_VALS; j++) { + vlan_dhcp_params->vlan_pbit_to_up[i][j].vid = 0; + vlan_dhcp_params->vlan_pbit_to_up[i][j].user_prio = 0; + } + helper[i] = 0; + } + + mac_ptr = strsep(&buffer, ";"); + + for (num_of_str = 0; mac_ptr && (num_of_str < ARRAY_SIZE(str_ptr)); num_of_str++) { + str_ptr[num_of_str] = mac_ptr; + mac_ptr = strsep(&buffer, ";"); + } + + for (i = 0; i < num_of_str; i++) { + val = U32_MAX; + pbit = U8_MAX; + vid = U16_MAX; + user_prio = U8_MAX; + + /* Get vid */ + tok_ptr = strsep(&str_ptr[i], ","); + if (!tok_ptr) { + pr_err("vlan_to_up0_7dec: vid null\n"); + continue; + } + if (kstrtou32(tok_ptr, 10, &val) != 0) { + pr_err("vlan_to_up0_7dec: invalid vid (%s)\n", tok_ptr); + continue; + } + if (val > 4095) { + pr_err("vlan_to_up0_7dec: vid exceeds 4095 (%u)\n", val); + continue; + } + + vid = (u16)val; + + /* Get p-bit */ + val = U32_MAX; + tok_ptr = strsep(&str_ptr[i], ","); + + if (!tok_ptr) { + pr_err("vlan_to_up0_7dec: pbit null\n"); + continue; + } + if (kstrtou32(tok_ptr, 10, &val) != 0) { + pr_err("vlan_to_up0_7dec: invalid pbit (%s)\n", tok_ptr); + continue; + } + if (val > 7) { + pr_err("vlan_to_up0_7dec: pbit exceeds 7 (%u)\n", val); + continue; + } + + pbit = (u8)val; + + /* Get user priority */ + val = U32_MAX; + tok_ptr = strsep(&str_ptr[i], ","); + + if (!tok_ptr) { + pr_err("vlan_to_up0_7dec: up null\n"); + continue; + } + if (kstrtou32(tok_ptr, 10, &val) != 0) { + pr_err("vlan_to_up0_7dec: invalid up (%s)\n", tok_ptr); + continue; + } + if (val > 7) { + pr_err("vlan_to_up0_7dec: up exceeds 7 (%u)\n", val); + continue; + } + + user_prio = (u8)val; + + /* At this point all 3 parameters are valid: p-bit, vid and user_prio */ + if (vid != 0) { + if (helper[pbit] < CL_USER_PRIO_VALS) { + struct cl_vid_user *vid_user = + &vlan_dhcp_params->vlan_pbit_to_up[pbit][helper[pbit]]; + + vid_user->vid = vid; + vid_user->user_prio = user_prio; + helper[pbit]++; + + /* + * Mark the fact that vlan_pbit_to_up + * array has been initialized + */ + if (vlan_dhcp_params->vlan_pbit_to_up[0][0].user_prio == (u8)(-1)) + vlan_dhcp_params->vlan_pbit_to_up[0][0].user_prio = 0; + } + } else { + vlan_dhcp_params->vlan_to_up[pbit] = user_prio; + } + } +} + +static void read_vlan_parms_format2(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + struct cl_vlan_dhcp_params *vlan_dhcp_params = &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + int i; + char *mac_ptr; + u8 user_prio; + + for (i = 0, mac_ptr = strsep(&buffer, ","); mac_ptr; mac_ptr = strsep(&buffer, ","), i++) { + if (i > (CL_USER_PRIO_VALS - 1)) { + pr_warn("vlan_to_up0_7dec: Priority bit exceeds %d entries!" + "Rest of Priority bits ignored\n", CL_USER_PRIO_VALS); + break; + } + + if (kstrtou8(mac_ptr, 10, &user_prio) != 0) { + pr_err("dscp_to_up0_7dec: Invalid up (%s)\n", mac_ptr); + continue; + } + + if (user_prio > 7) { + pr_err("vlan_to_up0_7dec[%d]: illegal user priority %u." + "Set it to %u\n", i, user_prio, (user_prio & 0x7)); + user_prio = (user_prio & 0x7); + } + + vlan_dhcp_params->vlan_to_up[i] = user_prio; + } + + if (i < (CL_USER_PRIO_VALS - 1)) + pr_warn("vlan_to_up0_7dec: only %d first priority bits entries filled in." + "Remaining priority bits will remain unassigned\n", i + 1); +} + +static void read_vlan_parms_from_buffer(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + /* + * Initiate configuration per AP index by buffer + * by configuration parameter vlan_to_up0_7dec + */ + struct cl_vlan_dhcp_params *vlan_dhcp_params = + &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + int i; + char *pch; + + if (!buffer || (strlen(buffer) == 0)) + return; + + /* Assume initially no change in vlan user priority */ + for (i = 0; i < CL_USER_PRIO_VALS; i++) + vlan_dhcp_params->vlan_to_up[i] = i; + + /* Assume initially vid_pbit NVRAM parameter is not initialized */ + vlan_dhcp_params->vlan_pbit_to_up[0][0].user_prio = (u8)(-1); + + pch = strchr(buffer, ';'); + if (pch) + /* Handle format vlan_id1,pbit,up;vlan_id2,pbit,up;vlan_id3,pbit,up.. */ + read_vlan_parms_format1(cl_hw, buffer, ap_idx); + else + /* Handle the follow format dscp_to_up_0,dscp_to_up_1,dscp_to_up_2,... */ + read_vlan_parms_format2(cl_hw, buffer, ap_idx); +} + +static void read_layer_based_parms_from_buffer(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + if (!buffer || (strlen(buffer) == 0)) + return; + + if ((strcmp(buffer, "AUTO") == 0) || (strcmp(buffer, "0") == 0)) + cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].up_layer_based = 0; + else if ((strcmp(buffer, "VLAN") == 0) || (strcmp(buffer, "2") == 0)) + cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].up_layer_based = 2; + else if ((strcmp(buffer, "DSCP") == 0) || (strcmp(buffer, "3") == 0)) + cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].up_layer_based = 3; + else + pr_err("%s: invalid input [%s]\n", __func__, buffer); +} + +static void read_default_vlan_up_parms_from_buffer(struct cl_hw *cl_hw, char *buffer, u8 ap_idx) +{ + u8 default_vlan_up; + + if (!buffer || (strlen(buffer) == 0)) + return; + + if (kstrtou8(buffer, 10, &default_vlan_up) != 0) { + pr_err("invalid param - %s\n", buffer); + return; + } + + if (default_vlan_up < 8) + cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].default_vlan_up = default_vlan_up; +} + +static int cl_vlan_dscp_cli_help(struct cl_hw *cl_hw) +{ + char *buf = kzalloc(PAGE_SIZE, GFP_KERNEL); + int err = 0; + + if (!buf) + return -ENOMEM; + + snprintf(buf, PAGE_SIZE, + "qos usage:\n" + "-a : Set ce_dscp_to_up0_7dec0 [val]\n" + "-b : Set ce_vlan_to_up0_7dec0 [val]\n" + "-c : Set ce_up0_7_layer_based [val]\n" + "-d : Set ce_up0_7_default_vlan_user_prio [val]\n" + "-e : Enable/Disable QOS functionality [1/0]\n" + "-s : Show current QoS configuration\n" + "-v : Enable/Disable QOS debug [1/0]\n"); + + err = cl_vendor_reply(cl_hw, buf, strlen(buf)); + kfree(buf); + + return err; +} + +void cl_vlan_dscp_init(struct cl_hw *cl_hw) +{ + struct cl_tcv_conf *conf = cl_hw->conf; + u8 ap_idx; + + for (ap_idx = 0; ap_idx < MAX_BSS_NUM; ap_idx++) { + cl_hw->vlan_dscp.enable[ap_idx] = conf->ce_dscp_vlan_enable[ap_idx]; + cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].up_layer_based = + conf->ce_up0_7_layer_based[ap_idx]; + cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx].default_vlan_up = + conf->ce_up0_7_default_vlan_user_prio[ap_idx]; + } + + /* Ce_dscp_to_up0_7decX */ + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec0, 0); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec1, 1); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec2, 2); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec3, 3); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec4, 4); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec5, 5); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec6, 6); + read_dscp_parms_from_buffer(cl_hw, conf->ce_dscp_to_up0_7dec7, 7); + + /* Ce_vlan_to_up0_7decX */ + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec0, 0); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec1, 1); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec2, 2); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec3, 3); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec4, 4); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec5, 5); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec6, 6); + read_vlan_parms_from_buffer(cl_hw, conf->ce_vlan_to_up0_7dec7, 7); +} + +bool cl_vlan_dscp_is_enabled(struct cl_hw *cl_hw, struct cl_vif *cl_vif) +{ + return cl_hw->vlan_dscp.enable[cl_vif->vif_index]; +} + +u8 cl_vlan_dscp_check_ether_type(struct cl_hw *cl_hw, struct sk_buff *skb, u8 ap_idx) +{ + /* Calculate the user priority according to the celeno logic and configuration */ + struct cl_vlan_dhcp_params *vlan_dhcp_params = &cl_hw->vlan_dscp.vlan_dhcp_params[ap_idx]; + u8 user_priority = 0; + u16 ether_type; + u8 *src_buf = skb->data; + + /* VLAN TCI: 3-bit UP + 1-bit CFI + 12-bit VLAN ID */ + u16 vlan_id_val = 0; + u8 pbit_val = 0; + + /* Get Ethernet protocol field*/ + ether_type = get_ether_type(2 * ETH_ALEN, src_buf); + /* Skip the Ethernet Header.*/ + src_buf += ETH_HLEN; + + /* + * handel LLC packets + * if it's a LLC packet we should move the data pointer and to + * get the ether type from the llc header add support to other LLC + */ + if (ether_type <= 1500) { + /* 802.3, 802.3 LLC + * DestMAC(6) + SrcMAC(6) + Length(2) + + * DSAP(1) + SSAP(1) + Control(1) + + * if the DSAP = 0xAA, SSAP=0xAA, Contorl = 0x03, it has a 5-bytes SNAP header. + * => + SNAP (5, OriginationID(3) + etherType(2)) + */ + if (PKT_HAS_LLC_HDR(src_buf)) { + /* Get new ether_type */ + ether_type = get_ether_type(LENGTH_LLC + LENGTH_SSNAP - 2, src_buf); + src_buf += LENGTH_LLC + LENGTH_SSNAP; /* Skip this LLC/SNAP header */ + } + } + + if (ether_type == ETH_VLAN || skb->vlan_tci != 0) { + if (skb->vlan_tci != 0) { + pbit_val = (skb->vlan_tci >> PBIT_OFFSET); + vlan_id_val = skb->vlan_tci & VID_MASK; + } else { + /* VLAN tag: 3-bit UP + 1-bit CFI + 12-bit VLAN ID */ + vlan_id_val = *(u16 *)src_buf; + vlan_id_val = htons(vlan_id_val); + vlan_id_val = vlan_id_val & VID_MASK; + ether_type = get_ether_type(LENGTH_VLAN_HDR - 2, src_buf); + pbit_val = (*(src_buf) & 0xe0) >> 5; + } + + /* Only use VLAN tag */ + if (vlan_dhcp_params->up_layer_based != CL_UP_BY_L3) { + user_priority = get_vlan_pbit_up(cl_hw, pbit_val, vlan_id_val, ap_idx); + goto out; + } + + if (ether_type == ETH_VLAN) + src_buf += LENGTH_VLAN_HDR; /* Skip the VLAN Header.*/ + } + + ether_type = htons(*(u16 *)(src_buf - 2)); + + if (vlan_dhcp_params->up_layer_based != CL_UP_BY_L2) { + /* + * GET user priority by DSCP + * If it is an IP packet and up_layer_based == 3 (DSCP), + * we will check it's DSCP parameter + */ + if (ether_type == ETH_P_IP || ether_type == ETH_P_IPV6) { + u8 version = (*src_buf & 0xf0); + + if ((version == IP_V_IPV6 || version == IP_V_IPV4)) + user_priority = get_ip_user_priority(cl_hw, src_buf, + ether_type, ap_idx); + } + } + +out: + VLAN_DSCP_DBG("ether_type=0x%X, vlan_tci=0x%X, pbit_val=%u, vlan_id_val=%u, priority=%u\n", + ether_type, skb->vlan_tci, pbit_val, vlan_id_val, user_priority); + + return user_priority; +} + +int cl_vlan_dscp_cli(struct cl_hw *cl_hw, struct cl_vif *cl_vif, char *data) +{ + u8 ap_idx = cl_vif->vif_index; + char *payload_start = data + 3; + + if (!(payload_start && strlen(payload_start))) + return cl_vlan_dscp_cli_help(cl_hw); + + /* + * workaround: delete last 'space' symbol if it is present in the payload + * this is caused by Celeno iw command convention - automatically added after all payload + */ + if (payload_start[strlen(payload_start) - 1] == ' ') + payload_start[strlen(payload_start) - 1] = '\0'; + + if (!strncmp(data, "-a.", 3)) { + read_dscp_parms_from_buffer(cl_hw, payload_start, ap_idx); + } else if (!strncmp(data, "-b.", 3)) { + read_vlan_parms_from_buffer(cl_hw, payload_start, ap_idx); + } else if (!strncmp(data, "-c.", 3)) { + read_layer_based_parms_from_buffer(cl_hw, payload_start, ap_idx); + } else if (!strncmp(data, "-d.", 3)) { + read_default_vlan_up_parms_from_buffer(cl_hw, payload_start, ap_idx); + } else if (!strncmp(data, "-e.", 3)) { + if (kstrtobool(payload_start, &cl_hw->vlan_dscp.enable[ap_idx]) != 0) + pr_err("invalid data - %s\n", payload_start); + } else if (!strncmp(data, "-v.", 3)) { + if (kstrtobool(payload_start, &cl_hw->vlan_dscp.debug) != 0) + pr_err("invalid data - %s\n", payload_start); + } else if (!strncmp(data, "-s.?", 4)) { + return print_configuration(cl_hw, ap_idx); + } + + pr_err("%s(): error in parameters\n", __func__); + return cl_vlan_dscp_cli_help(cl_hw); +}