diff --git a/techpack/datarmnet/core/Android.mk b/techpack/datarmnet/core/Android.mk new file mode 100644 index 000000000000..9aad807d7d38 --- /dev/null +++ b/techpack/datarmnet/core/Android.mk @@ -0,0 +1,62 @@ +ifneq ($(TARGET_PRODUCT),qssi) +RMNET_CORE_DLKM_PLATFORMS_LIST := lahaina +RMNET_CORE_DLKM_PLATFORMS_LIST += holi +RMNET_CORE_DLKM_PLATFORMS_LIST += monaco +RMNET_CORE_DLKM_PLATFORMS_LIST += monaco_go +RMNET_CORE_DLKM_PLATFORMS_LIST += monaco_go_aon + +ifeq ($(call is-board-platform-in-list, $(RMNET_CORE_DLKM_PLATFORMS_LIST)),true) +#Make file to create RMNET_CORE DLKM +LOCAL_PATH := $(call my-dir) +include $(CLEAR_VARS) + +LOCAL_CFLAGS := -Wno-macro-redefined -Wno-unused-function -Wall -Werror +LOCAL_CLANG :=true + +LOCAL_MODULE_PATH := $(KERNEL_MODULES_OUT) +LOCAL_MODULE := rmnet_core.ko + +LOCAL_SRC_FILES := \ + rmnet_config.c \ + rmnet_descriptor.c \ + rmnet_genl.c \ + rmnet_handlers.c \ + rmnet_map_command.c \ + rmnet_map_data.c \ + rmnet_vnd.c \ + dfc_qmap.c \ + dfc_qmi.c \ + qmi_rmnet.c \ + wda_qmi.c + +RMNET_BLD_DIR := ../../vendor/qcom/opensource/datarmnet/core +DLKM_DIR := $(TOP)/device/qcom/common/dlkm + +KBUILD_OPTIONS := $(RMNET_BLD_DIR) + +$(warning $(DLKM_DIR)) +include $(DLKM_DIR)/AndroidKernelModule.mk + +######## Create RMNET_CTL DLKM ######## +include $(CLEAR_VARS) + +LOCAL_CFLAGS := -Wno-macro-redefined -Wno-unused-function -Wall -Werror +LOCAL_CLANG :=true + +LOCAL_MODULE_PATH := $(KERNEL_MODULES_OUT) +LOCAL_MODULE := rmnet_ctl.ko + +LOCAL_SRC_FILES := \ + rmnet_ctl_client.c \ + rmnet_ctl_ipa.c + +RMNET_BLD_DIR := ../../vendor/qcom/opensource/datarmnet/core +DLKM_DIR := $(TOP)/device/qcom/common/dlkm + +KBUILD_OPTIONS := $(RMNET_BLD_DIR) + +$(warning $(DLKM_DIR)) +include $(DLKM_DIR)/AndroidKernelModule.mk + +endif #End of Check for target +endif #End of Check for qssi target diff --git a/techpack/datarmnet/core/Kbuild b/techpack/datarmnet/core/Kbuild new file mode 100644 index 000000000000..51ac345578dc --- /dev/null +++ b/techpack/datarmnet/core/Kbuild @@ -0,0 +1,8 @@ +obj-m += rmnet_core.o +obj-m += rmnet_ctl.o +rmnet_core-y := rmnet_config.o rmnet_handlers.o rmnet_descriptor.o \ + rmnet_genl.o rmnet_map_command.o rmnet_map_data.o rmnet_vnd.o\ + qmi_rmnet.o wda_qmi.o dfc_qmi.o dfc_qmap.o +rmnet_ctl-y := rmnet_ctl_client.o rmnet_ctl_ipa.o + + diff --git a/techpack/datarmnet/core/Kconfig b/techpack/datarmnet/core/Kconfig new file mode 100644 index 000000000000..0fe2530349a4 --- /dev/null +++ b/techpack/datarmnet/core/Kconfig @@ -0,0 +1,18 @@ +# +# RMNET MAP driver +# + +menuconfig RMNET_CORE + default m + select GRO_CELLS + ---help--- + If you select this, you will enable the RMNET module which is used + for handling data in the multiplexing and aggregation protocol (MAP) + format in the embedded data path. RMNET devices can be attached to + any IP mode physical device. + +menuconfig RMNET_CTL + default m + ---help--- + Enable the RMNET CTL module which is used for handling QMAP commands + for flow control purposes. diff --git a/techpack/datarmnet/core/Makefile b/techpack/datarmnet/core/Makefile new file mode 100644 index 000000000000..ed85c84a4124 --- /dev/null +++ b/techpack/datarmnet/core/Makefile @@ -0,0 +1,13 @@ +KERNEL_SRC ?= /lib/modules/$(shell uname -r)/build + +KBUILD_OPTIONS := RMNET_CORE_ROOT=$(PWD) +KBUILD_OPTIONS += MODNAME?=rmnet_core + +all: + $(MAKE) -C $(KERNEL_SRC) M=$(shell pwd) modules $(KBUILD_OPTIONS) + +modules_install: + $(MAKE) INSTALL_MOD_STRIP=1 -C $(KERNEL_SRC) M=$(shell pwd) modules_install + +clean: + $(MAKE) -C $(KERNEL_SRC) M=$(PWD) clean diff --git a/techpack/datarmnet/core/dfc.h b/techpack/datarmnet/core/dfc.h new file mode 100644 index 000000000000..bb3631d343ee --- /dev/null +++ b/techpack/datarmnet/core/dfc.h @@ -0,0 +1,320 @@ +/* Copyright (c) 2018-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#undef TRACE_SYSTEM +#define TRACE_SYSTEM dfc +#undef TRACE_INCLUDE_PATH +#define TRACE_INCLUDE_PATH ../../../../vendor/qcom/opensource/datarmnet/core +#define TRACE_INCLUDE_FILE dfc + +#if !defined(_TRACE_DFC_H) || defined(TRACE_HEADER_MULTI_READ) +#define _TRACE_DFC_H + +#include + +TRACE_EVENT(dfc_qmi_tc, + + TP_PROTO(const char *name, u32 txq, int enable), + + TP_ARGS(name, txq, enable), + + TP_STRUCT__entry( + __string(dev_name, name) + __field(u32, txq) + __field(int, enable) + ), + + TP_fast_assign( + __assign_str(dev_name, name); + __entry->txq = txq; + __entry->enable = enable; + ), + + TP_printk("dev=%s txq=%u %s", + __get_str(dev_name), + __entry->txq, + __entry->enable ? "enable" : "disable") +); + +TRACE_EVENT(dfc_flow_ind, + + TP_PROTO(int src, int idx, u8 mux_id, u8 bearer_id, u32 grant, + u16 seq_num, u8 ack_req, u32 ancillary), + + TP_ARGS(src, idx, mux_id, bearer_id, grant, seq_num, ack_req, + ancillary), + + TP_STRUCT__entry( + __field(int, src) + __field(int, idx) + __field(u8, mid) + __field(u8, bid) + __field(u32, grant) + __field(u16, seq) + __field(u8, ack_req) + __field(u32, ancillary) + ), + + TP_fast_assign( + __entry->src = src; + __entry->idx = idx; + __entry->mid = mux_id; + __entry->bid = bearer_id; + __entry->grant = grant; + __entry->seq = seq_num; + __entry->ack_req = ack_req; + __entry->ancillary = ancillary; + ), + + TP_printk("src=%d [%d]: mid=%u bid=%u grant=%u seq=%u ack=%u anc=%u", + __entry->src, __entry->idx, __entry->mid, __entry->bid, + __entry->grant, __entry->seq, __entry->ack_req, + __entry->ancillary) +); + +TRACE_EVENT(dfc_flow_check, + + TP_PROTO(const char *name, u8 bearer_id, unsigned int len, + u32 mark, u32 grant), + + TP_ARGS(name, bearer_id, len, mark, grant), + + TP_STRUCT__entry( + __string(dev_name, name) + __field(u8, bearer_id) + __field(unsigned int, len) + __field(u32, mark) + __field(u32, grant) + ), + + TP_fast_assign( + __assign_str(dev_name, name) + __entry->bearer_id = bearer_id; + __entry->len = len; + __entry->mark = mark; + __entry->grant = grant; + ), + + TP_printk("dev=%s bearer_id=%u skb_len=%u mark=%u current_grant=%u", + __get_str(dev_name), __entry->bearer_id, + __entry->len, __entry->mark, __entry->grant) +); + +TRACE_EVENT(dfc_flow_info, + + TP_PROTO(const char *name, u8 bearer_id, u32 flow_id, int ip_type, + u32 handle, int add), + + TP_ARGS(name, bearer_id, flow_id, ip_type, handle, add), + + TP_STRUCT__entry( + __string(dev_name, name) + __field(u8, bid) + __field(u32, fid) + __field(int, ip) + __field(u32, handle) + __field(int, action) + ), + + TP_fast_assign( + __assign_str(dev_name, name) + __entry->bid = bearer_id; + __entry->fid = flow_id; + __entry->ip = ip_type; + __entry->handle = handle; + __entry->action = add; + ), + + TP_printk("%s: dev=%s bearer_id=%u flow_id=%u ip_type=%d txq=%d", + __entry->action ? "add flow" : "delete flow", + __get_str(dev_name), + __entry->bid, __entry->fid, __entry->ip, __entry->handle) +); + +TRACE_EVENT(dfc_client_state_up, + + TP_PROTO(int idx, u32 instance, u32 ep_type, u32 iface), + + TP_ARGS(idx, instance, ep_type, iface), + + TP_STRUCT__entry( + __field(int, idx) + __field(u32, instance) + __field(u32, ep_type) + __field(u32, iface) + ), + + TP_fast_assign( + __entry->idx = idx; + __entry->instance = instance; + __entry->ep_type = ep_type; + __entry->iface = iface; + ), + + TP_printk("DFC Client[%d] connect: instance=%u ep_type=%u iface_id=%u", + __entry->idx, __entry->instance, + __entry->ep_type, __entry->iface) +); + +TRACE_EVENT(dfc_client_state_down, + + TP_PROTO(int idx, int from_cb), + + TP_ARGS(idx, from_cb), + + TP_STRUCT__entry( + __field(int, idx) + __field(int, from_cb) + ), + + TP_fast_assign( + __entry->idx = idx; + __entry->from_cb = from_cb; + ), + + TP_printk("DFC Client[%d] exit: callback %d", + __entry->idx, __entry->from_cb) +); + +TRACE_EVENT(dfc_qmap_cmd, + + TP_PROTO(u8 mux_id, u8 bearer_id, u16 seq_num, u8 type, u32 tran), + + TP_ARGS(mux_id, bearer_id, seq_num, type, tran), + + TP_STRUCT__entry( + __field(u8, mid) + __field(u8, bid) + __field(u16, seq) + __field(u8, type) + __field(u32, tran) + ), + + TP_fast_assign( + __entry->mid = mux_id; + __entry->bid = bearer_id; + __entry->seq = seq_num; + __entry->type = type; + __entry->tran = tran; + ), + + TP_printk("mux_id=%u bearer_id=%u seq_num=%u type=%u tran=%u", + __entry->mid, __entry->bid, __entry->seq, + __entry->type, __entry->tran) +); + +TRACE_EVENT(dfc_tx_link_status_ind, + + TP_PROTO(int src, int idx, u8 status, u8 mux_id, u8 bearer_id), + + TP_ARGS(src, idx, status, mux_id, bearer_id), + + TP_STRUCT__entry( + __field(int, src) + __field(int, idx) + __field(u8, status) + __field(u8, mid) + __field(u8, bid) + ), + + TP_fast_assign( + __entry->src = src; + __entry->idx = idx; + __entry->status = status; + __entry->mid = mux_id; + __entry->bid = bearer_id; + ), + + TP_printk("src=%d [%d]: status=%u mux_id=%u bearer_id=%u", + __entry->src, __entry->idx, __entry->status, + __entry->mid, __entry->bid) +); + +TRACE_EVENT(dfc_qmap, + + TP_PROTO(const void *data, size_t len, bool in), + + TP_ARGS(data, len, in), + + TP_STRUCT__entry( + __field(bool, in) + __field(size_t, len) + __dynamic_array(u8, data, len) + ), + + TP_fast_assign( + __entry->in = in; + __entry->len = len; + memcpy(__get_dynamic_array(data), data, len); + ), + + TP_printk("%s [%s]", + __entry->in ? "<--" : "-->", + __print_hex(__get_dynamic_array(data), __entry->len)) +); + +TRACE_EVENT(dfc_adjust_grant, + + TP_PROTO(u8 mux_id, u8 bearer_id, u32 grant, u32 rx_bytes, + u32 inflight, u32 a_grant), + + TP_ARGS(mux_id, bearer_id, grant, rx_bytes, inflight, a_grant), + + TP_STRUCT__entry( + __field(u8, mux_id) + __field(u8, bearer_id) + __field(u32, grant) + __field(u32, rx_bytes) + __field(u32, inflight) + __field(u32, a_grant) + ), + + TP_fast_assign( + __entry->mux_id = mux_id; + __entry->bearer_id = bearer_id; + __entry->grant = grant; + __entry->rx_bytes = rx_bytes; + __entry->inflight = inflight; + __entry->a_grant = a_grant; + ), + + TP_printk("mid=%u bid=%u grant=%u rx=%u inflight=%u adjusted_grant=%u", + __entry->mux_id, __entry->bearer_id, __entry->grant, + __entry->rx_bytes, __entry->inflight, __entry->a_grant) +); + +TRACE_EVENT(dfc_watchdog, + + TP_PROTO(u8 mux_id, u8 bearer_id, u8 event), + + TP_ARGS(mux_id, bearer_id, event), + + TP_STRUCT__entry( + __field(u8, mux_id) + __field(u8, bearer_id) + __field(u8, event) + ), + + TP_fast_assign( + __entry->mux_id = mux_id; + __entry->bearer_id = bearer_id; + __entry->event = event; + ), + + TP_printk("mid=%u bid=%u event=%u", + __entry->mux_id, __entry->bearer_id, __entry->event) +); + +#endif /* _TRACE_DFC_H */ + +/* This part must be outside protection */ +#include diff --git a/techpack/datarmnet/core/dfc_defs.h b/techpack/datarmnet/core/dfc_defs.h new file mode 100644 index 000000000000..2a30a29f903b --- /dev/null +++ b/techpack/datarmnet/core/dfc_defs.h @@ -0,0 +1,99 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * Copyright (c) 2019-2020, The Linux Foundation. All rights reserved. + */ + +#ifndef _DFC_DEFS_H +#define _DFC_DEFS_H + +#include +#include "qmi_rmnet_i.h" + +#define DFC_ACK_TYPE_DISABLE 1 +#define DFC_ACK_TYPE_THRESHOLD 2 + +#define DFC_MASK_TCP_BIDIR 0x1 +#define DFC_MASK_RAT_SWITCH 0x2 +#define DFC_IS_TCP_BIDIR(r) (bool)((r) & DFC_MASK_TCP_BIDIR) +#define DFC_IS_RAT_SWITCH(r) (bool)((r) & DFC_MASK_RAT_SWITCH) + +#define DFC_MAX_QOS_ID_V01 2 + +struct dfc_qmi_data { + void *rmnet_port; + struct workqueue_struct *dfc_wq; + struct work_struct svc_arrive; + struct qmi_handle handle; + struct sockaddr_qrtr ssctl; + struct svc_info svc; + struct work_struct qmi_ind_work; + struct list_head qmi_ind_q; + spinlock_t qmi_ind_lock; + int index; + int restart_state; +}; + +enum dfc_ip_type_enum_v01 { + DFC_IP_TYPE_ENUM_MIN_ENUM_VAL_V01 = -2147483647, + DFC_IPV4_TYPE_V01 = 0x4, + DFC_IPV6_TYPE_V01 = 0x6, + DFC_IP_TYPE_ENUM_MAX_ENUM_VAL_V01 = 2147483647 +}; + +struct dfc_qos_id_type_v01 { + u32 qos_id; + enum dfc_ip_type_enum_v01 ip_type; +}; + +struct dfc_flow_status_info_type_v01 { + u8 subs_id; + u8 mux_id; + u8 bearer_id; + u32 num_bytes; + u16 seq_num; + u8 qos_ids_len; + struct dfc_qos_id_type_v01 qos_ids[DFC_MAX_QOS_ID_V01]; + u8 rx_bytes_valid; + u32 rx_bytes; +}; + +struct dfc_ancillary_info_type_v01 { + u8 subs_id; + u8 mux_id; + u8 bearer_id; + u32 reserved; +}; + +struct dfc_flow_status_ind_msg_v01 { + u8 flow_status_valid; + u8 flow_status_len; + struct dfc_flow_status_info_type_v01 flow_status[DFC_MAX_BEARERS_V01]; + u8 eod_ack_reqd_valid; + u8 eod_ack_reqd; + u8 ancillary_info_valid; + u8 ancillary_info_len; + struct dfc_ancillary_info_type_v01 ancillary_info[DFC_MAX_BEARERS_V01]; +}; + +struct dfc_bearer_info_type_v01 { + u8 subs_id; + u8 mux_id; + u8 bearer_id; + enum dfc_ip_type_enum_v01 ip_type; +}; + +struct dfc_tx_link_status_ind_msg_v01 { + u8 tx_status; + u8 bearer_info_valid; + u8 bearer_info_len; + struct dfc_bearer_info_type_v01 bearer_info[DFC_MAX_BEARERS_V01]; +}; + +void dfc_do_burst_flow_control(struct dfc_qmi_data *dfc, + struct dfc_flow_status_ind_msg_v01 *ind, + bool is_query); + +void dfc_handle_tx_link_status_ind(struct dfc_qmi_data *dfc, + struct dfc_tx_link_status_ind_msg_v01 *ind); + +#endif /* _DFC_DEFS_H */ diff --git a/techpack/datarmnet/core/dfc_qmap.c b/techpack/datarmnet/core/dfc_qmap.c new file mode 100644 index 000000000000..f371a0f6e7bf --- /dev/null +++ b/techpack/datarmnet/core/dfc_qmap.c @@ -0,0 +1,564 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2019-2020, The Linux Foundation. All rights reserved. + */ + +#include +#include +#include "rmnet_ctl.h" +#include "dfc_defs.h" +#include "rmnet_qmi.h" +#include "qmi_rmnet.h" +#include "dfc.h" + +#define QMAP_DFC_VER 1 + +#define QMAP_CMD_DONE -1 + +#define QMAP_CMD_REQUEST 0 +#define QMAP_CMD_ACK 1 +#define QMAP_CMD_UNSUPPORTED 2 +#define QMAP_CMD_INVALID 3 + +#define QMAP_DFC_CONFIG 10 +#define QMAP_DFC_IND 11 +#define QMAP_DFC_QUERY 12 +#define QMAP_DFC_END_MARKER 13 + +struct qmap_hdr { + u8 cd_pad; + u8 mux_id; + __be16 pkt_len; +} __aligned(1); + +#define QMAP_HDR_LEN sizeof(struct qmap_hdr) + +struct qmap_cmd_hdr { + u8 pad_len:6; + u8 reserved_bit:1; + u8 cd_bit:1; + u8 mux_id; + __be16 pkt_len; + u8 cmd_name; + u8 cmd_type:2; + u8 reserved:6; + u16 reserved2; + __be32 tx_id; +} __aligned(1); + +struct qmap_dfc_config { + struct qmap_cmd_hdr hdr; + u8 cmd_ver; + u8 cmd_id; + u8 reserved; + u8 tx_info:1; + u8 reserved2:7; + __be32 ep_type; + __be32 iface_id; + u32 reserved3; +} __aligned(1); + +struct qmap_dfc_ind { + struct qmap_cmd_hdr hdr; + u8 cmd_ver; + u8 reserved; + __be16 seq_num; + u8 reserved2; + u8 tx_info_valid:1; + u8 tx_info:1; + u8 rx_bytes_valid:1; + u8 reserved3:5; + u8 bearer_id; + u8 tcp_bidir:1; + u8 bearer_status:3; + u8 reserved4:4; + __be32 grant; + __be32 rx_bytes; + u32 reserved6; +} __aligned(1); + +struct qmap_dfc_query { + struct qmap_cmd_hdr hdr; + u8 cmd_ver; + u8 reserved; + u8 bearer_id; + u8 reserved2; + u32 reserved3; +} __aligned(1); + +struct qmap_dfc_query_resp { + struct qmap_cmd_hdr hdr; + u8 cmd_ver; + u8 bearer_id; + u8 tcp_bidir:1; + u8 rx_bytes_valid:1; + u8 reserved:6; + u8 invalid:1; + u8 reserved2:7; + __be32 grant; + __be32 rx_bytes; + u32 reserved4; +} __aligned(1); + +struct qmap_dfc_end_marker_req { + struct qmap_cmd_hdr hdr; + u8 cmd_ver; + u8 reserved; + u8 bearer_id; + u8 reserved2; + u16 reserved3; + __be16 seq_num; + u32 reserved4; +} __aligned(1); + +struct qmap_dfc_end_marker_cnf { + struct qmap_cmd_hdr hdr; + u8 cmd_ver; + u8 reserved; + u8 bearer_id; + u8 reserved2; + u16 reserved3; + __be16 seq_num; + u32 reserved4; +} __aligned(1); + +static struct dfc_flow_status_ind_msg_v01 qmap_flow_ind; +static struct dfc_tx_link_status_ind_msg_v01 qmap_tx_ind; +static struct dfc_qmi_data __rcu *qmap_dfc_data; +static atomic_t qmap_txid; +static void *rmnet_ctl_handle; +static bool dfc_config_acked; + +static struct rmnet_ctl_client_if *rmnet_ctl; + +static void dfc_qmap_send_config(struct dfc_qmi_data *data); +static void dfc_qmap_send_end_marker_cnf(struct qos_info *qos, + u8 bearer_id, u16 seq, u32 tx_id); + +static void dfc_qmap_send_cmd(struct sk_buff *skb) +{ + trace_dfc_qmap(skb->data, skb->len, false); + + if (unlikely(!rmnet_ctl || !rmnet_ctl->send) || + rmnet_ctl->send(rmnet_ctl_handle, skb)) { + pr_err("Failed to send to rmnet ctl\n"); + kfree_skb(skb); + } +} + +static void dfc_qmap_send_inband_ack(struct dfc_qmi_data *dfc, + struct sk_buff *skb) +{ + struct qmap_cmd_hdr *cmd; + + cmd = (struct qmap_cmd_hdr *)skb->data; + + skb->protocol = htons(ETH_P_MAP); + skb->dev = rmnet_get_real_dev(dfc->rmnet_port); + + if (likely(rmnet_ctl && rmnet_ctl->log)) + rmnet_ctl->log(RMNET_CTL_LOG_DEBUG, "TXI", 0, + skb->data, skb->len); + trace_dfc_qmap(skb->data, skb->len, false); + dev_queue_xmit(skb); +} + +static int dfc_qmap_handle_ind(struct dfc_qmi_data *dfc, + struct sk_buff *skb) +{ + struct qmap_dfc_ind *cmd; + + if (skb->len < sizeof(struct qmap_dfc_ind)) + return QMAP_CMD_INVALID; + + cmd = (struct qmap_dfc_ind *)skb->data; + + if (cmd->tx_info_valid) { + memset(&qmap_tx_ind, 0, sizeof(qmap_tx_ind)); + qmap_tx_ind.tx_status = cmd->tx_info; + qmap_tx_ind.bearer_info_valid = 1; + qmap_tx_ind.bearer_info_len = 1; + qmap_tx_ind.bearer_info[0].mux_id = cmd->hdr.mux_id; + qmap_tx_ind.bearer_info[0].bearer_id = cmd->bearer_id; + + dfc_handle_tx_link_status_ind(dfc, &qmap_tx_ind); + + /* Ignore grant since it is always 0 */ + goto done; + } + + memset(&qmap_flow_ind, 0, sizeof(qmap_flow_ind)); + qmap_flow_ind.flow_status_valid = 1; + qmap_flow_ind.flow_status_len = 1; + qmap_flow_ind.flow_status[0].mux_id = cmd->hdr.mux_id; + qmap_flow_ind.flow_status[0].bearer_id = cmd->bearer_id; + qmap_flow_ind.flow_status[0].num_bytes = ntohl(cmd->grant); + qmap_flow_ind.flow_status[0].seq_num = ntohs(cmd->seq_num); + + if (cmd->rx_bytes_valid) { + qmap_flow_ind.flow_status[0].rx_bytes_valid = 1; + qmap_flow_ind.flow_status[0].rx_bytes = ntohl(cmd->rx_bytes); + } + + if (cmd->tcp_bidir) { + qmap_flow_ind.ancillary_info_valid = 1; + qmap_flow_ind.ancillary_info_len = 1; + qmap_flow_ind.ancillary_info[0].mux_id = cmd->hdr.mux_id; + qmap_flow_ind.ancillary_info[0].bearer_id = cmd->bearer_id; + qmap_flow_ind.ancillary_info[0].reserved = DFC_MASK_TCP_BIDIR; + } + + dfc_do_burst_flow_control(dfc, &qmap_flow_ind, false); + +done: + return QMAP_CMD_ACK; +} + +static int dfc_qmap_handle_query_resp(struct dfc_qmi_data *dfc, + struct sk_buff *skb) +{ + struct qmap_dfc_query_resp *cmd; + + if (skb->len < sizeof(struct qmap_dfc_query_resp)) + return QMAP_CMD_DONE; + + cmd = (struct qmap_dfc_query_resp *)skb->data; + + if (cmd->invalid) + return QMAP_CMD_DONE; + + memset(&qmap_flow_ind, 0, sizeof(qmap_flow_ind)); + qmap_flow_ind.flow_status_valid = 1; + qmap_flow_ind.flow_status_len = 1; + + qmap_flow_ind.flow_status[0].mux_id = cmd->hdr.mux_id; + qmap_flow_ind.flow_status[0].bearer_id = cmd->bearer_id; + qmap_flow_ind.flow_status[0].num_bytes = ntohl(cmd->grant); + qmap_flow_ind.flow_status[0].seq_num = 0xFFFF; + + if (cmd->rx_bytes_valid) { + qmap_flow_ind.flow_status[0].rx_bytes_valid = 1; + qmap_flow_ind.flow_status[0].rx_bytes = ntohl(cmd->rx_bytes); + } + + if (cmd->tcp_bidir) { + qmap_flow_ind.ancillary_info_valid = 1; + qmap_flow_ind.ancillary_info_len = 1; + qmap_flow_ind.ancillary_info[0].mux_id = cmd->hdr.mux_id; + qmap_flow_ind.ancillary_info[0].bearer_id = cmd->bearer_id; + qmap_flow_ind.ancillary_info[0].reserved = DFC_MASK_TCP_BIDIR; + } + + dfc_do_burst_flow_control(dfc, &qmap_flow_ind, true); + + return QMAP_CMD_DONE; +} + +static void dfc_qmap_set_end_marker(struct dfc_qmi_data *dfc, u8 mux_id, + u8 bearer_id, u16 seq_num, u32 tx_id) +{ + struct net_device *dev; + struct qos_info *qos; + struct rmnet_bearer_map *bearer; + + dev = rmnet_get_rmnet_dev(dfc->rmnet_port, mux_id); + if (!dev) + return; + + qos = (struct qos_info *)rmnet_get_qos_pt(dev); + if (!qos) + return; + + spin_lock_bh(&qos->qos_lock); + + bearer = qmi_rmnet_get_bearer_map(qos, bearer_id); + + if (bearer && bearer->last_seq == seq_num && bearer->grant_size) { + bearer->ack_req = 1; + bearer->ack_txid = tx_id; + } else { + dfc_qmap_send_end_marker_cnf(qos, bearer_id, seq_num, tx_id); + } + + spin_unlock_bh(&qos->qos_lock); +} + +static int dfc_qmap_handle_end_marker_req(struct dfc_qmi_data *dfc, + struct sk_buff *skb) +{ + struct qmap_dfc_end_marker_req *cmd; + + if (skb->len < sizeof(struct qmap_dfc_end_marker_req)) + return QMAP_CMD_INVALID; + + cmd = (struct qmap_dfc_end_marker_req *)skb->data; + + dfc_qmap_set_end_marker(dfc, cmd->hdr.mux_id, cmd->bearer_id, + ntohs(cmd->seq_num), ntohl(cmd->hdr.tx_id)); + + return QMAP_CMD_DONE; +} + +static void dfc_qmap_cmd_handler(struct sk_buff *skb) +{ + struct qmap_cmd_hdr *cmd; + struct dfc_qmi_data *dfc; + int rc = QMAP_CMD_DONE; + + if (!skb) + return; + + trace_dfc_qmap(skb->data, skb->len, true); + + if (skb->len < sizeof(struct qmap_cmd_hdr)) + goto free_skb; + + cmd = (struct qmap_cmd_hdr *)skb->data; + if (!cmd->cd_bit || skb->len != ntohs(cmd->pkt_len) + QMAP_HDR_LEN) + goto free_skb; + + if (cmd->cmd_name == QMAP_DFC_QUERY) { + if (cmd->cmd_type != QMAP_CMD_ACK) + goto free_skb; + } else if (cmd->cmd_type != QMAP_CMD_REQUEST) { + if (cmd->cmd_type == QMAP_CMD_ACK && + cmd->cmd_name == QMAP_DFC_CONFIG) + dfc_config_acked = true; + goto free_skb; + } + + rcu_read_lock(); + + dfc = rcu_dereference(qmap_dfc_data); + if (!dfc || READ_ONCE(dfc->restart_state)) { + rcu_read_unlock(); + goto free_skb; + } + + /* Re-send DFC config once if needed */ + if (unlikely(!dfc_config_acked)) { + dfc_qmap_send_config(dfc); + dfc_config_acked = true; + } + + switch (cmd->cmd_name) { + case QMAP_DFC_IND: + rc = dfc_qmap_handle_ind(dfc, skb); + qmi_rmnet_set_dl_msg_active(dfc->rmnet_port); + break; + + case QMAP_DFC_QUERY: + rc = dfc_qmap_handle_query_resp(dfc, skb); + break; + + case QMAP_DFC_END_MARKER: + rc = dfc_qmap_handle_end_marker_req(dfc, skb); + break; + + default: + rc = QMAP_CMD_UNSUPPORTED; + } + + /* Send ack */ + if (rc != QMAP_CMD_DONE) { + cmd->cmd_type = rc; + if (cmd->cmd_name == QMAP_DFC_IND) + dfc_qmap_send_inband_ack(dfc, skb); + else + dfc_qmap_send_cmd(skb); + + rcu_read_unlock(); + return; + } + + rcu_read_unlock(); + +free_skb: + kfree_skb(skb); +} + +static void dfc_qmap_send_config(struct dfc_qmi_data *data) +{ + struct sk_buff *skb; + struct qmap_dfc_config *dfc_config; + unsigned int len = sizeof(struct qmap_dfc_config); + + skb = alloc_skb(len, GFP_ATOMIC); + if (!skb) + return; + + skb->protocol = htons(ETH_P_MAP); + dfc_config = (struct qmap_dfc_config *)skb_put(skb, len); + memset(dfc_config, 0, len); + + dfc_config->hdr.cd_bit = 1; + dfc_config->hdr.mux_id = 0; + dfc_config->hdr.pkt_len = htons(len - QMAP_HDR_LEN); + dfc_config->hdr.cmd_name = QMAP_DFC_CONFIG; + dfc_config->hdr.cmd_type = QMAP_CMD_REQUEST; + dfc_config->hdr.tx_id = htonl(atomic_inc_return(&qmap_txid)); + + dfc_config->cmd_ver = QMAP_DFC_VER; + dfc_config->cmd_id = QMAP_DFC_IND; + dfc_config->tx_info = 1; + dfc_config->ep_type = htonl(data->svc.ep_type); + dfc_config->iface_id = htonl(data->svc.iface_id); + + dfc_qmap_send_cmd(skb); +} + +static void dfc_qmap_send_query(u8 mux_id, u8 bearer_id) +{ + struct sk_buff *skb; + struct qmap_dfc_query *dfc_query; + unsigned int len = sizeof(struct qmap_dfc_query); + + skb = alloc_skb(len, GFP_ATOMIC); + if (!skb) + return; + + skb->protocol = htons(ETH_P_MAP); + dfc_query = (struct qmap_dfc_query *)skb_put(skb, len); + memset(dfc_query, 0, len); + + dfc_query->hdr.cd_bit = 1; + dfc_query->hdr.mux_id = mux_id; + dfc_query->hdr.pkt_len = htons(len - QMAP_HDR_LEN); + dfc_query->hdr.cmd_name = QMAP_DFC_QUERY; + dfc_query->hdr.cmd_type = QMAP_CMD_REQUEST; + dfc_query->hdr.tx_id = htonl(atomic_inc_return(&qmap_txid)); + + dfc_query->cmd_ver = QMAP_DFC_VER; + dfc_query->bearer_id = bearer_id; + + dfc_qmap_send_cmd(skb); +} + +static void dfc_qmap_send_end_marker_cnf(struct qos_info *qos, + u8 bearer_id, u16 seq, u32 tx_id) +{ + struct sk_buff *skb; + struct qmap_dfc_end_marker_cnf *em_cnf; + unsigned int len = sizeof(struct qmap_dfc_end_marker_cnf); + + skb = alloc_skb(len, GFP_ATOMIC); + if (!skb) + return; + + em_cnf = (struct qmap_dfc_end_marker_cnf *)skb_put(skb, len); + memset(em_cnf, 0, len); + + em_cnf->hdr.cd_bit = 1; + em_cnf->hdr.mux_id = qos->mux_id; + em_cnf->hdr.pkt_len = htons(len - QMAP_HDR_LEN); + em_cnf->hdr.cmd_name = QMAP_DFC_END_MARKER; + em_cnf->hdr.cmd_type = QMAP_CMD_ACK; + em_cnf->hdr.tx_id = htonl(tx_id); + + em_cnf->cmd_ver = QMAP_DFC_VER; + em_cnf->bearer_id = bearer_id; + em_cnf->seq_num = htons(seq); + + skb->protocol = htons(ETH_P_MAP); + skb->dev = qos->real_dev; + + /* This cmd needs to be sent in-band */ + if (likely(rmnet_ctl && rmnet_ctl->log)) + rmnet_ctl->log(RMNET_CTL_LOG_INFO, "TXI", 0, + skb->data, skb->len); + trace_dfc_qmap(skb->data, skb->len, false); + rmnet_map_tx_qmap_cmd(skb); +} + +void dfc_qmap_send_ack(struct qos_info *qos, u8 bearer_id, u16 seq, u8 type) +{ + struct rmnet_bearer_map *bearer; + + if (type == DFC_ACK_TYPE_DISABLE) { + bearer = qmi_rmnet_get_bearer_map(qos, bearer_id); + if (bearer) + dfc_qmap_send_end_marker_cnf(qos, bearer_id, + seq, bearer->ack_txid); + } else if (type == DFC_ACK_TYPE_THRESHOLD) { + dfc_qmap_send_query(qos->mux_id, bearer_id); + } +} + +static struct rmnet_ctl_client_hooks cb = { + .ctl_dl_client_hook = dfc_qmap_cmd_handler, +}; + +int dfc_qmap_client_init(void *port, int index, struct svc_info *psvc, + struct qmi_info *qmi) +{ + struct dfc_qmi_data *data; + + if (!port || !qmi) + return -EINVAL; + + /* Prevent double init */ + data = rcu_dereference(qmap_dfc_data); + if (data) + return -EINVAL; + + data = kzalloc(sizeof(struct dfc_qmi_data), GFP_KERNEL); + if (!data) + return -ENOMEM; + + data->rmnet_port = port; + data->index = index; + memcpy(&data->svc, psvc, sizeof(data->svc)); + + qmi->dfc_clients[index] = (void *)data; + rcu_assign_pointer(qmap_dfc_data, data); + + atomic_set(&qmap_txid, 0); + + rmnet_ctl = rmnet_ctl_if(); + if (!rmnet_ctl) { + pr_err("rmnet_ctl module not loaded\n"); + goto out; + } + + if (rmnet_ctl->reg) + rmnet_ctl_handle = rmnet_ctl->reg(&cb); + + if (!rmnet_ctl_handle) + pr_err("Failed to register with rmnet ctl\n"); + + trace_dfc_client_state_up(data->index, data->svc.instance, + data->svc.ep_type, data->svc.iface_id); + + pr_info("DFC QMAP init\n"); + + dfc_config_acked = false; + dfc_qmap_send_config(data); + +out: + return 0; +} + +void dfc_qmap_client_exit(void *dfc_data) +{ + struct dfc_qmi_data *data = (struct dfc_qmi_data *)dfc_data; + + if (!data) { + pr_err("%s() data is null\n", __func__); + return; + } + + trace_dfc_client_state_down(data->index, 0); + + if (rmnet_ctl && rmnet_ctl->dereg) + rmnet_ctl->dereg(rmnet_ctl_handle); + rmnet_ctl_handle = NULL; + + WRITE_ONCE(data->restart_state, 1); + RCU_INIT_POINTER(qmap_dfc_data, NULL); + synchronize_rcu(); + + kfree(data); + rmnet_ctl = NULL; + + pr_info("DFC QMAP exit\n"); +} diff --git a/techpack/datarmnet/core/dfc_qmi.c b/techpack/datarmnet/core/dfc_qmi.c new file mode 100644 index 000000000000..f52d9e3e032f --- /dev/null +++ b/techpack/datarmnet/core/dfc_qmi.c @@ -0,0 +1,1568 @@ +/* + * Copyright (c) 2018-2021, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include "rmnet_qmi.h" +#include "qmi_rmnet.h" +#include "dfc_defs.h" + +#define CREATE_TRACE_POINTS +#include "dfc.h" + +struct dfc_qmap_header { + u8 pad_len:6; + u8 reserved_bit:1; + u8 cd_bit:1; + u8 mux_id; + __be16 pkt_len; +} __aligned(1); + +struct dfc_ack_cmd { + struct dfc_qmap_header header; + u8 command_name; + u8 cmd_type:2; + u8 reserved:6; + u16 reserved2; + u32 transaction_id; + u8 ver:2; + u8 reserved3:6; + u8 type:2; + u8 reserved4:6; + u16 dfc_seq; + u8 reserved5[3]; + u8 bearer_id; +} __aligned(1); + +static void dfc_svc_init(struct work_struct *work); + +/* **************************************************** */ +#define DFC_SERVICE_ID_V01 0x4E +#define DFC_SERVICE_VERS_V01 0x01 +#define DFC_TIMEOUT_JF msecs_to_jiffies(1000) + +#define QMI_DFC_BIND_CLIENT_REQ_V01 0x0020 +#define QMI_DFC_BIND_CLIENT_RESP_V01 0x0020 +#define QMI_DFC_BIND_CLIENT_REQ_V01_MAX_MSG_LEN 11 +#define QMI_DFC_BIND_CLIENT_RESP_V01_MAX_MSG_LEN 7 + +#define QMI_DFC_INDICATION_REGISTER_REQ_V01 0x0001 +#define QMI_DFC_INDICATION_REGISTER_RESP_V01 0x0001 +#define QMI_DFC_INDICATION_REGISTER_REQ_V01_MAX_MSG_LEN 8 +#define QMI_DFC_INDICATION_REGISTER_RESP_V01_MAX_MSG_LEN 7 + +#define QMI_DFC_FLOW_STATUS_IND_V01 0x0022 +#define QMI_DFC_TX_LINK_STATUS_IND_V01 0x0024 + +#define QMI_DFC_GET_FLOW_STATUS_REQ_V01 0x0023 +#define QMI_DFC_GET_FLOW_STATUS_RESP_V01 0x0023 +#define QMI_DFC_GET_FLOW_STATUS_REQ_V01_MAX_MSG_LEN 20 +#define QMI_DFC_GET_FLOW_STATUS_RESP_V01_MAX_MSG_LEN 543 + +struct dfc_bind_client_req_msg_v01 { + u8 ep_id_valid; + struct data_ep_id_type_v01 ep_id; +}; + +struct dfc_bind_client_resp_msg_v01 { + struct qmi_response_type_v01 resp; +}; + +struct dfc_indication_register_req_msg_v01 { + u8 report_flow_status_valid; + u8 report_flow_status; + u8 report_tx_link_status_valid; + u8 report_tx_link_status; +}; + +struct dfc_indication_register_resp_msg_v01 { + struct qmi_response_type_v01 resp; +}; + +static struct qmi_elem_info dfc_qos_id_type_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_4_BYTE, + .elem_len = 1, + .elem_size = sizeof(u32), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct dfc_qos_id_type_v01, + qos_id), + .ei_array = NULL, + }, + { + .data_type = QMI_SIGNED_4_BYTE_ENUM, + .elem_len = 1, + .elem_size = sizeof(enum dfc_ip_type_enum_v01), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct dfc_qos_id_type_v01, + ip_type), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_flow_status_info_type_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + subs_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + mux_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + bearer_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_4_BYTE, + .elem_len = 1, + .elem_size = sizeof(u32), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + num_bytes), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_2_BYTE, + .elem_len = 1, + .elem_size = sizeof(u16), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + seq_num), + .ei_array = NULL, + }, + { + .data_type = QMI_DATA_LEN, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + qos_ids_len), + .ei_array = NULL, + }, + { + .data_type = QMI_STRUCT, + .elem_len = DFC_MAX_QOS_ID_V01, + .elem_size = sizeof(struct dfc_qos_id_type_v01), + .array_type = VAR_LEN_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_flow_status_info_type_v01, + qos_ids), + .ei_array = dfc_qos_id_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_ancillary_info_type_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_ancillary_info_type_v01, + subs_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_ancillary_info_type_v01, + mux_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_ancillary_info_type_v01, + bearer_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_4_BYTE, + .elem_len = 1, + .elem_size = sizeof(u32), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_ancillary_info_type_v01, + reserved), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +struct dfc_get_flow_status_req_msg_v01 { + u8 bearer_id_list_valid; + u8 bearer_id_list_len; + u8 bearer_id_list[DFC_MAX_BEARERS_V01]; +}; + +struct dfc_get_flow_status_resp_msg_v01 { + struct qmi_response_type_v01 resp; + u8 flow_status_valid; + u8 flow_status_len; + struct dfc_flow_status_info_type_v01 flow_status[DFC_MAX_BEARERS_V01]; +}; + +struct dfc_svc_ind { + struct list_head list; + u16 msg_id; + union { + struct dfc_flow_status_ind_msg_v01 dfc_info; + struct dfc_tx_link_status_ind_msg_v01 tx_status; + } d; +}; + +static struct qmi_elem_info dfc_bind_client_req_msg_v01_ei[] = { + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct dfc_bind_client_req_msg_v01, + ep_id_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct data_ep_id_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct dfc_bind_client_req_msg_v01, + ep_id), + .ei_array = data_ep_id_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_bind_client_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct dfc_bind_client_resp_msg_v01, + resp), + .ei_array = qmi_response_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_indication_register_req_msg_v01_ei[] = { + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_indication_register_req_msg_v01, + report_flow_status_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_indication_register_req_msg_v01, + report_flow_status), + .ei_array = NULL, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x11, + .offset = offsetof(struct + dfc_indication_register_req_msg_v01, + report_tx_link_status_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x11, + .offset = offsetof(struct + dfc_indication_register_req_msg_v01, + report_tx_link_status), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_indication_register_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct + dfc_indication_register_resp_msg_v01, + resp), + .ei_array = qmi_response_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_flow_status_ind_v01_ei[] = { + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + flow_status_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_DATA_LEN, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + flow_status_len), + .ei_array = NULL, + }, + { + .data_type = QMI_STRUCT, + .elem_len = DFC_MAX_BEARERS_V01, + .elem_size = sizeof(struct + dfc_flow_status_info_type_v01), + .array_type = VAR_LEN_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + flow_status), + .ei_array = dfc_flow_status_info_type_v01_ei, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x11, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + eod_ack_reqd_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x11, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + eod_ack_reqd), + .ei_array = NULL, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x12, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + ancillary_info_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_DATA_LEN, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x12, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + ancillary_info_len), + .ei_array = NULL, + }, + { + .data_type = QMI_STRUCT, + .elem_len = DFC_MAX_BEARERS_V01, + .elem_size = sizeof(struct + dfc_ancillary_info_type_v01), + .array_type = VAR_LEN_ARRAY, + .tlv_type = 0x12, + .offset = offsetof(struct + dfc_flow_status_ind_msg_v01, + ancillary_info), + .ei_array = dfc_ancillary_info_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_get_flow_status_req_msg_v01_ei[] = { + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_get_flow_status_req_msg_v01, + bearer_id_list_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_DATA_LEN, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_get_flow_status_req_msg_v01, + bearer_id_list_len), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = DFC_MAX_BEARERS_V01, + .elem_size = sizeof(u8), + .array_type = VAR_LEN_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_get_flow_status_req_msg_v01, + bearer_id_list), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_get_flow_status_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct + dfc_get_flow_status_resp_msg_v01, + resp), + .ei_array = qmi_response_type_v01_ei, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_get_flow_status_resp_msg_v01, + flow_status_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_DATA_LEN, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_get_flow_status_resp_msg_v01, + flow_status_len), + .ei_array = NULL, + }, + { + .data_type = QMI_STRUCT, + .elem_len = DFC_MAX_BEARERS_V01, + .elem_size = sizeof(struct + dfc_flow_status_info_type_v01), + .array_type = VAR_LEN_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_get_flow_status_resp_msg_v01, + flow_status), + .ei_array = dfc_flow_status_info_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_bearer_info_type_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_bearer_info_type_v01, + subs_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_bearer_info_type_v01, + mux_id), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_bearer_info_type_v01, + bearer_id), + .ei_array = NULL, + }, + { + .data_type = QMI_SIGNED_4_BYTE_ENUM, + .elem_len = 1, + .elem_size = sizeof(enum dfc_ip_type_enum_v01), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct + dfc_bearer_info_type_v01, + ip_type), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info dfc_tx_link_status_ind_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x01, + .offset = offsetof(struct + dfc_tx_link_status_ind_msg_v01, + tx_status), + .ei_array = NULL, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_tx_link_status_ind_msg_v01, + bearer_info_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_DATA_LEN, + .elem_len = 1, + .elem_size = sizeof(u8), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_tx_link_status_ind_msg_v01, + bearer_info_len), + .ei_array = NULL, + }, + { + .data_type = QMI_STRUCT, + .elem_len = DFC_MAX_BEARERS_V01, + .elem_size = sizeof(struct + dfc_bearer_info_type_v01), + .array_type = VAR_LEN_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + dfc_tx_link_status_ind_msg_v01, + bearer_info), + .ei_array = dfc_bearer_info_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static int +dfc_bind_client_req(struct qmi_handle *dfc_handle, + struct sockaddr_qrtr *ssctl, struct svc_info *svc) +{ + struct dfc_bind_client_resp_msg_v01 *resp; + struct dfc_bind_client_req_msg_v01 *req; + struct qmi_txn txn; + int ret; + + req = kzalloc(sizeof(*req), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + resp = kzalloc(sizeof(*resp), GFP_ATOMIC); + if (!resp) { + kfree(req); + return -ENOMEM; + } + + ret = qmi_txn_init(dfc_handle, &txn, + dfc_bind_client_resp_msg_v01_ei, resp); + if (ret < 0) { + pr_err("%s() Failed init for response, err: %d\n", + __func__, ret); + goto out; + } + + req->ep_id_valid = 1; + req->ep_id.ep_type = svc->ep_type; + req->ep_id.iface_id = svc->iface_id; + ret = qmi_send_request(dfc_handle, ssctl, &txn, + QMI_DFC_BIND_CLIENT_REQ_V01, + QMI_DFC_BIND_CLIENT_REQ_V01_MAX_MSG_LEN, + dfc_bind_client_req_msg_v01_ei, req); + if (ret < 0) { + qmi_txn_cancel(&txn); + pr_err("%s() Failed sending request, err: %d\n", + __func__, ret); + goto out; + } + + ret = qmi_txn_wait(&txn, DFC_TIMEOUT_JF); + if (ret < 0) { + pr_err("%s() Response waiting failed, err: %d\n", + __func__, ret); + } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) { + pr_err("%s() Request rejected, result: %d, err: %d\n", + __func__, resp->resp.result, resp->resp.error); + ret = -resp->resp.result; + } + +out: + kfree(resp); + kfree(req); + return ret; +} + +static int +dfc_indication_register_req(struct qmi_handle *dfc_handle, + struct sockaddr_qrtr *ssctl, u8 reg) +{ + struct dfc_indication_register_resp_msg_v01 *resp; + struct dfc_indication_register_req_msg_v01 *req; + struct qmi_txn txn; + int ret; + + req = kzalloc(sizeof(*req), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + resp = kzalloc(sizeof(*resp), GFP_ATOMIC); + if (!resp) { + kfree(req); + return -ENOMEM; + } + + ret = qmi_txn_init(dfc_handle, &txn, + dfc_indication_register_resp_msg_v01_ei, resp); + if (ret < 0) { + pr_err("%s() Failed init for response, err: %d\n", + __func__, ret); + goto out; + } + + req->report_flow_status_valid = 1; + req->report_flow_status = reg; + req->report_tx_link_status_valid = 1; + req->report_tx_link_status = reg; + + ret = qmi_send_request(dfc_handle, ssctl, &txn, + QMI_DFC_INDICATION_REGISTER_REQ_V01, + QMI_DFC_INDICATION_REGISTER_REQ_V01_MAX_MSG_LEN, + dfc_indication_register_req_msg_v01_ei, req); + if (ret < 0) { + qmi_txn_cancel(&txn); + pr_err("%s() Failed sending request, err: %d\n", + __func__, ret); + goto out; + } + + ret = qmi_txn_wait(&txn, DFC_TIMEOUT_JF); + if (ret < 0) { + pr_err("%s() Response waiting failed, err: %d\n", + __func__, ret); + } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) { + pr_err("%s() Request rejected, result: %d, err: %d\n", + __func__, resp->resp.result, resp->resp.error); + ret = -resp->resp.result; + } + +out: + kfree(resp); + kfree(req); + return ret; +} + +static int +dfc_get_flow_status_req(struct qmi_handle *dfc_handle, + struct sockaddr_qrtr *ssctl, + struct dfc_get_flow_status_resp_msg_v01 *resp) +{ + struct dfc_get_flow_status_req_msg_v01 *req; + struct qmi_txn *txn; + int ret; + + req = kzalloc(sizeof(*req), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + txn = kzalloc(sizeof(*txn), GFP_ATOMIC); + if (!txn) { + kfree(req); + return -ENOMEM; + } + + ret = qmi_txn_init(dfc_handle, txn, + dfc_get_flow_status_resp_msg_v01_ei, resp); + if (ret < 0) { + pr_err("%s() Failed init for response, err: %d\n", + __func__, ret); + goto out; + } + + ret = qmi_send_request(dfc_handle, ssctl, txn, + QMI_DFC_GET_FLOW_STATUS_REQ_V01, + QMI_DFC_GET_FLOW_STATUS_REQ_V01_MAX_MSG_LEN, + dfc_get_flow_status_req_msg_v01_ei, req); + if (ret < 0) { + qmi_txn_cancel(txn); + pr_err("%s() Failed sending request, err: %d\n", + __func__, ret); + goto out; + } + + ret = qmi_txn_wait(txn, DFC_TIMEOUT_JF); + if (ret < 0) { + pr_err("%s() Response waiting failed, err: %d\n", + __func__, ret); + } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) { + pr_err("%s() Request rejected, result: %d, err: %d\n", + __func__, resp->resp.result, resp->resp.error); + ret = -resp->resp.result; + } + +out: + kfree(txn); + kfree(req); + return ret; +} + +static int dfc_init_service(struct dfc_qmi_data *data) +{ + int rc; + + rc = dfc_bind_client_req(&data->handle, &data->ssctl, &data->svc); + if (rc < 0) + return rc; + + return dfc_indication_register_req(&data->handle, &data->ssctl, 1); +} + +static void +dfc_send_ack(struct net_device *dev, u8 bearer_id, u16 seq, u8 mux_id, u8 type) +{ + struct qos_info *qos = rmnet_get_qos_pt(dev); + struct sk_buff *skb; + struct dfc_ack_cmd *msg; + int data_size = sizeof(struct dfc_ack_cmd); + int header_size = sizeof(struct dfc_qmap_header); + + if (!qos) + return; + + if (dfc_qmap) { + dfc_qmap_send_ack(qos, bearer_id, seq, type); + return; + } + + skb = alloc_skb(data_size, GFP_ATOMIC); + if (!skb) + return; + + msg = (struct dfc_ack_cmd *)skb_put(skb, data_size); + memset(msg, 0, data_size); + + msg->header.cd_bit = 1; + msg->header.mux_id = mux_id; + msg->header.pkt_len = htons(data_size - header_size); + + msg->bearer_id = bearer_id; + msg->command_name = 4; + msg->cmd_type = 0; + msg->dfc_seq = htons(seq); + msg->type = type; + msg->ver = 2; + msg->transaction_id = htonl(qos->tran_num); + + skb->dev = qos->real_dev; + skb->protocol = htons(ETH_P_MAP); + + trace_dfc_qmap_cmd(mux_id, bearer_id, seq, type, qos->tran_num); + qos->tran_num++; + + rmnet_map_tx_qmap_cmd(skb); +} + +int dfc_bearer_flow_ctl(struct net_device *dev, + struct rmnet_bearer_map *bearer, + struct qos_info *qos) +{ + bool enable; + + enable = bearer->grant_size ? true : false; + + /* Do not flow disable tcp ack q in tcp bidir + * ACK queue opened first to drain ACKs faster + * Although since tcp ancillary is true most of the time, + * this shouldn't really make a difference + * If there is non zero grant but tcp ancillary is false, + * send out ACKs anyway + */ + if (bearer->ack_mq_idx != INVALID_MQ) + qmi_rmnet_flow_control(dev, bearer->ack_mq_idx, + enable || bearer->tcp_bidir); + + qmi_rmnet_flow_control(dev, bearer->mq_idx, enable); + + if (!enable && bearer->ack_req) + dfc_send_ack(dev, bearer->bearer_id, + bearer->seq, qos->mux_id, + DFC_ACK_TYPE_DISABLE); + + return 0; +} + +static int dfc_all_bearer_flow_ctl(struct net_device *dev, + struct qos_info *qos, u8 ack_req, u32 ancillary, + struct dfc_flow_status_info_type_v01 *fc_info) +{ + struct rmnet_bearer_map *bearer; + + list_for_each_entry(bearer, &qos->bearer_head, list) { + bearer->grant_size = fc_info->num_bytes; + bearer->grant_thresh = + qmi_rmnet_grant_per(bearer->grant_size); + bearer->seq = fc_info->seq_num; + bearer->ack_req = ack_req; + bearer->tcp_bidir = DFC_IS_TCP_BIDIR(ancillary); + bearer->last_grant = fc_info->num_bytes; + bearer->last_seq = fc_info->seq_num; + bearer->last_adjusted_grant = fc_info->num_bytes; + + dfc_bearer_flow_ctl(dev, bearer, qos); + } + + return 0; +} + +static u32 dfc_adjust_grant(struct rmnet_bearer_map *bearer, + struct dfc_flow_status_info_type_v01 *fc_info) +{ + u32 grant; + + if (!fc_info->rx_bytes_valid) + return fc_info->num_bytes; + + if (bearer->bytes_in_flight > fc_info->rx_bytes) + bearer->bytes_in_flight -= fc_info->rx_bytes; + else + bearer->bytes_in_flight = 0; + + /* Adjusted grant = grant - bytes_in_flight */ + if (fc_info->num_bytes > bearer->bytes_in_flight) + grant = fc_info->num_bytes - bearer->bytes_in_flight; + else + grant = 0; + + trace_dfc_adjust_grant(fc_info->mux_id, fc_info->bearer_id, + fc_info->num_bytes, fc_info->rx_bytes, + bearer->bytes_in_flight, grant); + return grant; +} + +static int dfc_update_fc_map(struct net_device *dev, struct qos_info *qos, + u8 ack_req, u32 ancillary, + struct dfc_flow_status_info_type_v01 *fc_info, + bool is_query) +{ + struct rmnet_bearer_map *itm = NULL; + int rc = 0; + bool action = false; + u32 adjusted_grant; + + itm = qmi_rmnet_get_bearer_map(qos, fc_info->bearer_id); + if (!itm) + itm = qmi_rmnet_get_bearer_noref(qos, fc_info->bearer_id); + + if (itm) { + /* The RAT switch flag indicates the start and end of + * the switch. Ignore indications in between. + */ + if (DFC_IS_RAT_SWITCH(ancillary)) + itm->rat_switch = !fc_info->num_bytes; + else + if (itm->rat_switch) + return 0; + + /* If TX is OFF but we received grant, ignore it */ + if (itm->tx_off && fc_info->num_bytes > 0) + return 0; + + /* Adjuste grant for query */ + if (dfc_qmap && is_query) { + adjusted_grant = dfc_adjust_grant(itm, fc_info); + } else { + adjusted_grant = fc_info->num_bytes; + itm->bytes_in_flight = 0; + } + + /* update queue state only if there is a change in grant + * or change in ancillary tcp state + */ + if ((itm->grant_size == 0 && adjusted_grant > 0) || + (itm->grant_size > 0 && adjusted_grant == 0) || + (itm->tcp_bidir ^ DFC_IS_TCP_BIDIR(ancillary))) + action = true; + + /* This is needed by qmap */ + if (dfc_qmap && itm->ack_req && !ack_req && itm->grant_size) + dfc_qmap_send_ack(qos, itm->bearer_id, + itm->seq, DFC_ACK_TYPE_DISABLE); + + itm->grant_size = adjusted_grant; + + /* No further query if the adjusted grant is less + * than 20% of the original grant. Add to watch to + * recover if no indication is received. + */ + if (dfc_qmap && is_query && + itm->grant_size < (fc_info->num_bytes / 5)) { + itm->grant_thresh = itm->grant_size; + qmi_rmnet_watchdog_add(itm); + } else { + itm->grant_thresh = + qmi_rmnet_grant_per(itm->grant_size); + qmi_rmnet_watchdog_remove(itm); + } + + itm->seq = fc_info->seq_num; + itm->ack_req = ack_req; + itm->tcp_bidir = DFC_IS_TCP_BIDIR(ancillary); + itm->last_grant = fc_info->num_bytes; + itm->last_seq = fc_info->seq_num; + itm->last_adjusted_grant = adjusted_grant; + + if (action) + rc = dfc_bearer_flow_ctl(dev, itm, qos); + } + + return rc; +} + +void dfc_do_burst_flow_control(struct dfc_qmi_data *dfc, + struct dfc_flow_status_ind_msg_v01 *ind, + bool is_query) +{ + struct net_device *dev; + struct qos_info *qos; + struct dfc_flow_status_info_type_v01 *flow_status; + struct dfc_ancillary_info_type_v01 *ai; + u8 ack_req = ind->eod_ack_reqd_valid ? ind->eod_ack_reqd : 0; + u32 ancillary; + int i, j; + + rcu_read_lock(); + + for (i = 0; i < ind->flow_status_len; i++) { + flow_status = &ind->flow_status[i]; + + ancillary = 0; + if (ind->ancillary_info_valid) { + for (j = 0; j < ind->ancillary_info_len; j++) { + ai = &ind->ancillary_info[j]; + if (ai->mux_id == flow_status->mux_id && + ai->bearer_id == flow_status->bearer_id) { + ancillary = ai->reserved; + break; + } + } + } + + trace_dfc_flow_ind(dfc->index, + i, flow_status->mux_id, + flow_status->bearer_id, + flow_status->num_bytes, + flow_status->seq_num, + ack_req, + ancillary); + + dev = rmnet_get_rmnet_dev(dfc->rmnet_port, + flow_status->mux_id); + if (!dev) + goto clean_out; + + qos = (struct qos_info *)rmnet_get_qos_pt(dev); + if (!qos) + continue; + + spin_lock_bh(&qos->qos_lock); + + if (qmi_rmnet_ignore_grant(dfc->rmnet_port)) { + spin_unlock_bh(&qos->qos_lock); + continue; + } + + if (unlikely(flow_status->bearer_id == 0xFF)) + dfc_all_bearer_flow_ctl( + dev, qos, ack_req, ancillary, flow_status); + else + dfc_update_fc_map( + dev, qos, ack_req, ancillary, flow_status, + is_query); + + spin_unlock_bh(&qos->qos_lock); + } + +clean_out: + rcu_read_unlock(); +} + +static void dfc_update_tx_link_status(struct net_device *dev, + struct qos_info *qos, u8 tx_status, + struct dfc_bearer_info_type_v01 *binfo) +{ + struct rmnet_bearer_map *itm = NULL; + + itm = qmi_rmnet_get_bearer_map(qos, binfo->bearer_id); + if (!itm) + return; + + /* If no change in tx status, ignore */ + if (itm->tx_off == !tx_status) + return; + + if (itm->grant_size && !tx_status) { + itm->grant_size = 0; + itm->tcp_bidir = false; + itm->bytes_in_flight = 0; + qmi_rmnet_watchdog_remove(itm); + dfc_bearer_flow_ctl(dev, itm, qos); + } else if (itm->grant_size == 0 && tx_status && !itm->rat_switch) { + itm->grant_size = DEFAULT_GRANT; + itm->grant_thresh = qmi_rmnet_grant_per(DEFAULT_GRANT); + itm->seq = 0; + itm->ack_req = 0; + dfc_bearer_flow_ctl(dev, itm, qos); + } + + itm->tx_off = !tx_status; +} + +void dfc_handle_tx_link_status_ind(struct dfc_qmi_data *dfc, + struct dfc_tx_link_status_ind_msg_v01 *ind) +{ + struct net_device *dev; + struct qos_info *qos; + struct dfc_bearer_info_type_v01 *bearer_info; + int i; + + rcu_read_lock(); + + for (i = 0; i < ind->bearer_info_len; i++) { + bearer_info = &ind->bearer_info[i]; + + trace_dfc_tx_link_status_ind(dfc->index, i, + ind->tx_status, + bearer_info->mux_id, + bearer_info->bearer_id); + + dev = rmnet_get_rmnet_dev(dfc->rmnet_port, + bearer_info->mux_id); + if (!dev) + goto clean_out; + + qos = (struct qos_info *)rmnet_get_qos_pt(dev); + if (!qos) + continue; + + spin_lock_bh(&qos->qos_lock); + + dfc_update_tx_link_status( + dev, qos, ind->tx_status, bearer_info); + + spin_unlock_bh(&qos->qos_lock); + } + +clean_out: + rcu_read_unlock(); +} + +static void dfc_qmi_ind_work(struct work_struct *work) +{ + struct dfc_qmi_data *dfc = container_of(work, struct dfc_qmi_data, + qmi_ind_work); + struct dfc_svc_ind *svc_ind; + unsigned long flags; + + if (!dfc) + return; + + local_bh_disable(); + + do { + spin_lock_irqsave(&dfc->qmi_ind_lock, flags); + svc_ind = list_first_entry_or_null(&dfc->qmi_ind_q, + struct dfc_svc_ind, list); + if (svc_ind) + list_del(&svc_ind->list); + spin_unlock_irqrestore(&dfc->qmi_ind_lock, flags); + + if (!svc_ind) + break; + + if (!dfc->restart_state) { + if (svc_ind->msg_id == QMI_DFC_FLOW_STATUS_IND_V01) + dfc_do_burst_flow_control( + dfc, &svc_ind->d.dfc_info, + false); + else if (svc_ind->msg_id == + QMI_DFC_TX_LINK_STATUS_IND_V01) + dfc_handle_tx_link_status_ind( + dfc, &svc_ind->d.tx_status); + } + kfree(svc_ind); + } while (1); + + local_bh_enable(); + + qmi_rmnet_set_dl_msg_active(dfc->rmnet_port); +} + +static void dfc_clnt_ind_cb(struct qmi_handle *qmi, struct sockaddr_qrtr *sq, + struct qmi_txn *txn, const void *data) +{ + struct dfc_qmi_data *dfc = container_of(qmi, struct dfc_qmi_data, + handle); + struct dfc_flow_status_ind_msg_v01 *ind_msg; + struct dfc_svc_ind *svc_ind; + unsigned long flags; + + if (qmi != &dfc->handle) + return; + + ind_msg = (struct dfc_flow_status_ind_msg_v01 *)data; + if (ind_msg->flow_status_valid) { + if (ind_msg->flow_status_len > DFC_MAX_BEARERS_V01) { + pr_err("%s() Invalid fc info len: %d\n", + __func__, ind_msg->flow_status_len); + return; + } + + svc_ind = kzalloc(sizeof(struct dfc_svc_ind), GFP_ATOMIC); + if (!svc_ind) + return; + + svc_ind->msg_id = QMI_DFC_FLOW_STATUS_IND_V01; + memcpy(&svc_ind->d.dfc_info, ind_msg, sizeof(*ind_msg)); + + spin_lock_irqsave(&dfc->qmi_ind_lock, flags); + list_add_tail(&svc_ind->list, &dfc->qmi_ind_q); + spin_unlock_irqrestore(&dfc->qmi_ind_lock, flags); + + queue_work(dfc->dfc_wq, &dfc->qmi_ind_work); + } +} + +static void dfc_tx_link_status_ind_cb(struct qmi_handle *qmi, + struct sockaddr_qrtr *sq, + struct qmi_txn *txn, const void *data) +{ + struct dfc_qmi_data *dfc = container_of(qmi, struct dfc_qmi_data, + handle); + struct dfc_tx_link_status_ind_msg_v01 *ind_msg; + struct dfc_svc_ind *svc_ind; + unsigned long flags; + + if (qmi != &dfc->handle) + return; + + ind_msg = (struct dfc_tx_link_status_ind_msg_v01 *)data; + if (ind_msg->bearer_info_valid) { + if (ind_msg->bearer_info_len > DFC_MAX_BEARERS_V01) { + pr_err("%s() Invalid bearer info len: %d\n", + __func__, ind_msg->bearer_info_len); + return; + } + + svc_ind = kzalloc(sizeof(struct dfc_svc_ind), GFP_ATOMIC); + if (!svc_ind) + return; + + svc_ind->msg_id = QMI_DFC_TX_LINK_STATUS_IND_V01; + memcpy(&svc_ind->d.tx_status, ind_msg, sizeof(*ind_msg)); + + spin_lock_irqsave(&dfc->qmi_ind_lock, flags); + list_add_tail(&svc_ind->list, &dfc->qmi_ind_q); + spin_unlock_irqrestore(&dfc->qmi_ind_lock, flags); + + queue_work(dfc->dfc_wq, &dfc->qmi_ind_work); + } +} + +static void dfc_svc_init(struct work_struct *work) +{ + int rc = 0; + struct dfc_qmi_data *data = container_of(work, struct dfc_qmi_data, + svc_arrive); + struct qmi_info *qmi; + + if (data->restart_state == 1) + return; + + rc = dfc_init_service(data); + if (rc < 0) { + pr_err("%s Failed to init service, err[%d]\n", __func__, rc); + return; + } + + if (data->restart_state == 1) + return; + while (!rtnl_trylock()) { + if (!data->restart_state) + cond_resched(); + else + return; + } + qmi = (struct qmi_info *)rmnet_get_qmi_pt(data->rmnet_port); + if (!qmi) { + rtnl_unlock(); + return; + } + + qmi->dfc_pending[data->index] = NULL; + qmi->dfc_clients[data->index] = (void *)data; + trace_dfc_client_state_up(data->index, + data->svc.instance, + data->svc.ep_type, + data->svc.iface_id); + + rtnl_unlock(); + + pr_info("Connection established with the DFC Service\n"); +} + +static int dfc_svc_arrive(struct qmi_handle *qmi, struct qmi_service *svc) +{ + struct dfc_qmi_data *data = container_of(qmi, struct dfc_qmi_data, + handle); + + data->ssctl.sq_family = AF_QIPCRTR; + data->ssctl.sq_node = svc->node; + data->ssctl.sq_port = svc->port; + + queue_work(data->dfc_wq, &data->svc_arrive); + + return 0; +} + +static void dfc_svc_exit(struct qmi_handle *qmi, struct qmi_service *svc) +{ + struct dfc_qmi_data *data = container_of(qmi, struct dfc_qmi_data, + handle); + + if (!data) + pr_debug("%s() data is null\n", __func__); +} + +static struct qmi_ops server_ops = { + .new_server = dfc_svc_arrive, + .del_server = dfc_svc_exit, +}; + +static struct qmi_msg_handler qmi_indication_handler[] = { + { + .type = QMI_INDICATION, + .msg_id = QMI_DFC_FLOW_STATUS_IND_V01, + .ei = dfc_flow_status_ind_v01_ei, + .decoded_size = sizeof(struct dfc_flow_status_ind_msg_v01), + .fn = dfc_clnt_ind_cb, + }, + { + .type = QMI_INDICATION, + .msg_id = QMI_DFC_TX_LINK_STATUS_IND_V01, + .ei = dfc_tx_link_status_ind_v01_ei, + .decoded_size = sizeof(struct dfc_tx_link_status_ind_msg_v01), + .fn = dfc_tx_link_status_ind_cb, + }, + {}, +}; + +int dfc_qmi_client_init(void *port, int index, struct svc_info *psvc, + struct qmi_info *qmi) +{ + struct dfc_qmi_data *data; + int rc = -ENOMEM; + + if (!port || !qmi) + return -EINVAL; + + data = kzalloc(sizeof(struct dfc_qmi_data), GFP_KERNEL); + if (!data) + return -ENOMEM; + + data->rmnet_port = port; + data->index = index; + data->restart_state = 0; + memcpy(&data->svc, psvc, sizeof(data->svc)); + + INIT_WORK(&data->qmi_ind_work, dfc_qmi_ind_work); + INIT_LIST_HEAD(&data->qmi_ind_q); + spin_lock_init(&data->qmi_ind_lock); + + data->dfc_wq = create_singlethread_workqueue("dfc_wq"); + if (!data->dfc_wq) { + pr_err("%s Could not create workqueue\n", __func__); + goto err0; + } + + INIT_WORK(&data->svc_arrive, dfc_svc_init); + rc = qmi_handle_init(&data->handle, + QMI_DFC_GET_FLOW_STATUS_RESP_V01_MAX_MSG_LEN, + &server_ops, qmi_indication_handler); + if (rc < 0) { + pr_err("%s: failed qmi_handle_init - rc[%d]\n", __func__, rc); + goto err1; + } + + rc = qmi_add_lookup(&data->handle, DFC_SERVICE_ID_V01, + DFC_SERVICE_VERS_V01, + psvc->instance); + if (rc < 0) { + pr_err("%s: failed qmi_add_lookup - rc[%d]\n", __func__, rc); + goto err2; + } + + qmi->dfc_pending[index] = (void *)data; + + return 0; + +err2: + qmi_handle_release(&data->handle); +err1: + destroy_workqueue(data->dfc_wq); +err0: + kfree(data); + return rc; +} + +void dfc_qmi_client_exit(void *dfc_data) +{ + struct dfc_qmi_data *data = (struct dfc_qmi_data *)dfc_data; + + if (!data) { + pr_err("%s() data is null\n", __func__); + return; + } + + data->restart_state = 1; + trace_dfc_client_state_down(data->index, 0); + qmi_handle_release(&data->handle); + + drain_workqueue(data->dfc_wq); + destroy_workqueue(data->dfc_wq); + kfree(data); +} + +void dfc_qmi_burst_check(struct net_device *dev, struct qos_info *qos, + int ip_type, u32 mark, unsigned int len) +{ + struct rmnet_bearer_map *bearer = NULL; + struct rmnet_flow_map *itm; + u32 start_grant; + + spin_lock_bh(&qos->qos_lock); + + /* Mark is flow_id */ + itm = qmi_rmnet_get_flow_map(qos, mark, ip_type); + if (likely(itm)) + bearer = itm->bearer; + + if (unlikely(!bearer)) + goto out; + + trace_dfc_flow_check(dev->name, bearer->bearer_id, + len, mark, bearer->grant_size); + + bearer->bytes_in_flight += len; + + if (!bearer->grant_size) + goto out; + + start_grant = bearer->grant_size; + if (len >= bearer->grant_size) + bearer->grant_size = 0; + else + bearer->grant_size -= len; + + if (start_grant > bearer->grant_thresh && + bearer->grant_size <= bearer->grant_thresh) { + dfc_send_ack(dev, bearer->bearer_id, + bearer->seq, qos->mux_id, + DFC_ACK_TYPE_THRESHOLD); + } + + if (!bearer->grant_size) + dfc_bearer_flow_ctl(dev, bearer, qos); + +out: + spin_unlock_bh(&qos->qos_lock); +} + +void dfc_qmi_query_flow(void *dfc_data) +{ + struct dfc_qmi_data *data = (struct dfc_qmi_data *)dfc_data; + struct dfc_get_flow_status_resp_msg_v01 *resp; + struct dfc_svc_ind *svc_ind; + int rc; + + resp = kzalloc(sizeof(*resp), GFP_ATOMIC); + if (!resp) + return; + + svc_ind = kzalloc(sizeof(*svc_ind), GFP_ATOMIC); + if (!svc_ind) { + kfree(resp); + return; + } + + if (!data) + goto done; + + rc = dfc_get_flow_status_req(&data->handle, &data->ssctl, resp); + + if (rc < 0 || !resp->flow_status_valid || resp->flow_status_len < 1 || + resp->flow_status_len > DFC_MAX_BEARERS_V01) + goto done; + + svc_ind->d.dfc_info.flow_status_valid = resp->flow_status_valid; + svc_ind->d.dfc_info.flow_status_len = resp->flow_status_len; + memcpy(&svc_ind->d.dfc_info.flow_status, resp->flow_status, + sizeof(resp->flow_status[0]) * resp->flow_status_len); + dfc_do_burst_flow_control(data, &svc_ind->d.dfc_info, true); + +done: + kfree(svc_ind); + kfree(resp); +} diff --git a/techpack/datarmnet/core/qmi_rmnet.c b/techpack/datarmnet/core/qmi_rmnet.c new file mode 100644 index 000000000000..84818c9b8f57 --- /dev/null +++ b/techpack/datarmnet/core/qmi_rmnet.c @@ -0,0 +1,1371 @@ +/* +* Copyright (c) 2018-2021, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ +#include + +#include "qmi_rmnet_i.h" +#include "qmi_rmnet.h" +#include "rmnet_qmi.h" +#include "dfc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define NLMSG_FLOW_ACTIVATE 1 +#define NLMSG_FLOW_DEACTIVATE 2 +#define NLMSG_CLIENT_SETUP 4 +#define NLMSG_CLIENT_DELETE 5 +#define NLMSG_SCALE_FACTOR 6 +#define NLMSG_WQ_FREQUENCY 7 + +#define FLAG_DFC_MASK 0x000F +#define FLAG_POWERSAVE_MASK 0x0010 +#define FLAG_QMAP_MASK 0x0020 + +#define FLAG_TO_MODE(f) ((f) & FLAG_DFC_MASK) + +#define DFC_SUPPORTED_MODE(m) \ + ((m) == DFC_MODE_SA) + +#define FLAG_TO_QMAP(f) ((f) & FLAG_QMAP_MASK) + +int dfc_mode; +int dfc_qmap; + +unsigned int rmnet_wq_frequency __read_mostly = 1000; + +#define PS_WORK_ACTIVE_BIT 0 +#define PS_INTERVAL (((!rmnet_wq_frequency) ? \ + 1 : rmnet_wq_frequency/10) * (HZ/100)) +#define NO_DELAY (0x0000 * HZ) +#define PS_INTERVAL_MS (1000) +#define PS_INTERVAL_KT (ms_to_ktime(PS_INTERVAL_MS)) +#define WATCHDOG_EXPIRE_JF (msecs_to_jiffies(50)) + +#ifdef CONFIG_QTI_QMI_DFC +static unsigned int qmi_rmnet_scale_factor = 5; +static LIST_HEAD(qos_cleanup_list); +#endif + +static int +qmi_rmnet_del_flow(struct net_device *dev, struct tcmsg *tcm, + struct qmi_info *qmi); + +struct qmi_elem_info data_ep_id_type_v01_ei[] = { + { + .data_type = QMI_SIGNED_4_BYTE_ENUM, + .elem_len = 1, + .elem_size = sizeof(enum data_ep_type_enum_v01), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct data_ep_id_type_v01, + ep_type), + .ei_array = NULL, + }, + { + .data_type = QMI_UNSIGNED_4_BYTE, + .elem_len = 1, + .elem_size = sizeof(u32), + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = offsetof(struct data_ep_id_type_v01, + iface_id), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .elem_len = 0, + .elem_size = 0, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + .offset = 0, + .ei_array = NULL, + }, +}; +EXPORT_SYMBOL(data_ep_id_type_v01_ei); + +void *qmi_rmnet_has_dfc_client(struct qmi_info *qmi) +{ + int i; + + if (!qmi) + return NULL; + + for (i = 0; i < MAX_CLIENT_NUM; i++) { + if (qmi->dfc_clients[i]) + return qmi->dfc_clients[i]; + } + + return NULL; +} + +static inline int +qmi_rmnet_has_client(struct qmi_info *qmi) +{ + if (qmi->wda_client) + return 1; + + return qmi_rmnet_has_dfc_client(qmi) ? 1 : 0; +} + +static int +qmi_rmnet_has_pending(struct qmi_info *qmi) +{ + int i; + + if (qmi->wda_pending) + return 1; + + for (i = 0; i < MAX_CLIENT_NUM; i++) { + if (qmi->dfc_pending[i]) + return 1; + } + + return 0; +} + +#ifdef CONFIG_QTI_QMI_DFC +static void +qmi_rmnet_clean_flow_list(struct qos_info *qos) +{ + struct rmnet_bearer_map *bearer, *br_tmp; + struct rmnet_flow_map *itm, *fl_tmp; + + ASSERT_RTNL(); + + list_for_each_entry_safe(itm, fl_tmp, &qos->flow_head, list) { + list_del(&itm->list); + kfree(itm); + } + + list_for_each_entry_safe(bearer, br_tmp, &qos->bearer_head, list) { + list_del(&bearer->list); + kfree(bearer); + } + + memset(qos->mq, 0, sizeof(qos->mq)); +} + +struct rmnet_flow_map * +qmi_rmnet_get_flow_map(struct qos_info *qos, u32 flow_id, int ip_type) +{ + struct rmnet_flow_map *itm; + + if (!qos) + return NULL; + + list_for_each_entry(itm, &qos->flow_head, list) { + if ((itm->flow_id == flow_id) && (itm->ip_type == ip_type)) + return itm; + } + return NULL; +} + +struct rmnet_bearer_map * +qmi_rmnet_get_bearer_map(struct qos_info *qos, uint8_t bearer_id) +{ + struct rmnet_bearer_map *itm; + + if (!qos) + return NULL; + + list_for_each_entry(itm, &qos->bearer_head, list) { + if (itm->bearer_id == bearer_id) + return itm; + } + return NULL; +} + +static void qmi_rmnet_update_flow_map(struct rmnet_flow_map *itm, + struct rmnet_flow_map *new_map) +{ + itm->bearer_id = new_map->bearer_id; + itm->flow_id = new_map->flow_id; + itm->ip_type = new_map->ip_type; + itm->mq_idx = new_map->mq_idx; +} + +int qmi_rmnet_flow_control(struct net_device *dev, u32 mq_idx, int enable) +{ + struct netdev_queue *q; + + if (unlikely(mq_idx >= dev->num_tx_queues)) + return 0; + + q = netdev_get_tx_queue(dev, mq_idx); + if (unlikely(!q)) + return 0; + + if (enable) + netif_tx_wake_queue(q); + else + netif_tx_stop_queue(q); + + trace_dfc_qmi_tc(dev->name, mq_idx, enable); + + return 0; +} + +static void qmi_rmnet_reset_txq(struct net_device *dev, unsigned int txq) +{ + struct Qdisc *qdisc; + + if (unlikely(txq >= dev->num_tx_queues)) + return; + + qdisc = rtnl_dereference(netdev_get_tx_queue(dev, txq)->qdisc); + if (qdisc) { + spin_lock_bh(qdisc_lock(qdisc)); + qdisc_reset(qdisc); + spin_unlock_bh(qdisc_lock(qdisc)); + } +} + +/** + * qmi_rmnet_watchdog_fn - watchdog timer func + */ +static void qmi_rmnet_watchdog_fn(struct timer_list *t) +{ + struct rmnet_bearer_map *bearer; + + bearer = container_of(t, struct rmnet_bearer_map, watchdog); + + trace_dfc_watchdog(bearer->qos->mux_id, bearer->bearer_id, 2); + + spin_lock_bh(&bearer->qos->qos_lock); + + if (bearer->watchdog_quit) + goto done; + + /* + * Possible stall, try to recover. Enable 80% query and jumpstart + * the bearer if disabled. + */ + bearer->watchdog_expire_cnt++; + bearer->bytes_in_flight = 0; + if (!bearer->grant_size) { + bearer->grant_size = DEFAULT_CALL_GRANT; + bearer->grant_thresh = qmi_rmnet_grant_per(bearer->grant_size); + dfc_bearer_flow_ctl(bearer->qos->vnd_dev, bearer, bearer->qos); + } else { + bearer->grant_thresh = qmi_rmnet_grant_per(bearer->grant_size); + } + +done: + bearer->watchdog_started = false; + spin_unlock_bh(&bearer->qos->qos_lock); +} + +/** + * qmi_rmnet_watchdog_add - add the bearer to watch + * Needs to be called with qos_lock + */ +void qmi_rmnet_watchdog_add(struct rmnet_bearer_map *bearer) +{ + bearer->watchdog_quit = false; + + if (bearer->watchdog_started) + return; + + bearer->watchdog_started = true; + mod_timer(&bearer->watchdog, jiffies + WATCHDOG_EXPIRE_JF); + + trace_dfc_watchdog(bearer->qos->mux_id, bearer->bearer_id, 1); +} + +/** + * qmi_rmnet_watchdog_remove - remove the bearer from watch + * Needs to be called with qos_lock + */ +void qmi_rmnet_watchdog_remove(struct rmnet_bearer_map *bearer) +{ + bearer->watchdog_quit = true; + + if (!bearer->watchdog_started) + return; + + del_timer(&bearer->watchdog); + bearer->watchdog_started = false; + + trace_dfc_watchdog(bearer->qos->mux_id, bearer->bearer_id, 0); +} + +/** + * qmi_rmnet_bearer_clean - clean the removed bearer + * Needs to be called with rtn_lock but not qos_lock + */ +static void qmi_rmnet_bearer_clean(struct qos_info *qos) +{ + if (qos->removed_bearer) { + qos->removed_bearer->watchdog_quit = true; + del_timer_sync(&qos->removed_bearer->watchdog); + kfree(qos->removed_bearer); + qos->removed_bearer = NULL; + } +} + +static struct rmnet_bearer_map *__qmi_rmnet_bearer_get( + struct qos_info *qos_info, u8 bearer_id) +{ + struct rmnet_bearer_map *bearer; + + bearer = qmi_rmnet_get_bearer_map(qos_info, bearer_id); + if (bearer) { + bearer->flow_ref++; + } else { + bearer = kzalloc(sizeof(*bearer), GFP_ATOMIC); + if (!bearer) + return NULL; + + bearer->bearer_id = bearer_id; + bearer->flow_ref = 1; + bearer->grant_size = DEFAULT_CALL_GRANT; + bearer->grant_thresh = qmi_rmnet_grant_per(bearer->grant_size); + bearer->mq_idx = INVALID_MQ; + bearer->ack_mq_idx = INVALID_MQ; + bearer->qos = qos_info; + timer_setup(&bearer->watchdog, qmi_rmnet_watchdog_fn, 0); + list_add(&bearer->list, &qos_info->bearer_head); + } + + return bearer; +} + +static void __qmi_rmnet_bearer_put(struct net_device *dev, + struct qos_info *qos_info, + struct rmnet_bearer_map *bearer, + bool reset) +{ + struct mq_map *mq; + int i, j; + + if (bearer && --bearer->flow_ref == 0) { + for (i = 0; i < MAX_MQ_NUM; i++) { + mq = &qos_info->mq[i]; + if (mq->bearer != bearer) + continue; + + mq->bearer = NULL; + if (reset) { + qmi_rmnet_reset_txq(dev, i); + qmi_rmnet_flow_control(dev, i, 1); + + if (dfc_mode == DFC_MODE_SA) { + j = i + ACK_MQ_OFFSET; + qmi_rmnet_reset_txq(dev, j); + qmi_rmnet_flow_control(dev, j, 1); + } + } + } + + /* Remove from bearer map */ + list_del(&bearer->list); + qos_info->removed_bearer = bearer; + } +} + +static void __qmi_rmnet_update_mq(struct net_device *dev, + struct qos_info *qos_info, + struct rmnet_bearer_map *bearer, + struct rmnet_flow_map *itm) +{ + struct mq_map *mq; + + /* In SA mode default mq is not associated with any bearer */ + if (dfc_mode == DFC_MODE_SA && itm->mq_idx == DEFAULT_MQ_NUM) + return; + + mq = &qos_info->mq[itm->mq_idx]; + if (!mq->bearer) { + mq->bearer = bearer; + + if (dfc_mode == DFC_MODE_SA) { + bearer->mq_idx = itm->mq_idx; + bearer->ack_mq_idx = itm->mq_idx + ACK_MQ_OFFSET; + } else { + bearer->mq_idx = itm->mq_idx; + } + + qmi_rmnet_flow_control(dev, itm->mq_idx, + bearer->grant_size > 0 ? 1 : 0); + + if (dfc_mode == DFC_MODE_SA) + qmi_rmnet_flow_control(dev, bearer->ack_mq_idx, + bearer->grant_size > 0 ? 1 : 0); + } +} + +static int __qmi_rmnet_rebind_flow(struct net_device *dev, + struct qos_info *qos_info, + struct rmnet_flow_map *itm, + struct rmnet_flow_map *new_map) +{ + struct rmnet_bearer_map *bearer; + + __qmi_rmnet_bearer_put(dev, qos_info, itm->bearer, false); + + bearer = __qmi_rmnet_bearer_get(qos_info, new_map->bearer_id); + if (!bearer) + return -ENOMEM; + + qmi_rmnet_update_flow_map(itm, new_map); + itm->bearer = bearer; + + __qmi_rmnet_update_mq(dev, qos_info, bearer, itm); + + return 0; +} + +static int qmi_rmnet_add_flow(struct net_device *dev, struct tcmsg *tcm, + struct qmi_info *qmi) +{ + struct qos_info *qos_info = (struct qos_info *)rmnet_get_qos_pt(dev); + struct rmnet_flow_map new_map, *itm; + struct rmnet_bearer_map *bearer; + struct tcmsg tmp_tcm; + int rc = 0; + + if (!qos_info || !tcm || tcm->tcm_handle >= MAX_MQ_NUM) + return -EINVAL; + + ASSERT_RTNL(); + + /* flow activate + * tcm->tcm__pad1 - bearer_id, tcm->tcm_parent - flow_id, + * tcm->tcm_ifindex - ip_type, tcm->tcm_handle - mq_idx + */ + + new_map.bearer_id = tcm->tcm__pad1; + new_map.flow_id = tcm->tcm_parent; + new_map.ip_type = tcm->tcm_ifindex; + new_map.mq_idx = tcm->tcm_handle; + trace_dfc_flow_info(dev->name, new_map.bearer_id, new_map.flow_id, + new_map.ip_type, new_map.mq_idx, 1); + +again: + spin_lock_bh(&qos_info->qos_lock); + + itm = qmi_rmnet_get_flow_map(qos_info, new_map.flow_id, + new_map.ip_type); + if (itm) { + if (itm->bearer_id != new_map.bearer_id) { + rc = __qmi_rmnet_rebind_flow( + dev, qos_info, itm, &new_map); + goto done; + } else if (itm->mq_idx != new_map.mq_idx) { + tmp_tcm.tcm__pad1 = itm->bearer_id; + tmp_tcm.tcm_parent = itm->flow_id; + tmp_tcm.tcm_ifindex = itm->ip_type; + tmp_tcm.tcm_handle = itm->mq_idx; + spin_unlock_bh(&qos_info->qos_lock); + qmi_rmnet_del_flow(dev, &tmp_tcm, qmi); + goto again; + } else { + goto done; + } + } + + /* Create flow map */ + itm = kzalloc(sizeof(*itm), GFP_ATOMIC); + if (!itm) { + spin_unlock_bh(&qos_info->qos_lock); + return -ENOMEM; + } + + qmi_rmnet_update_flow_map(itm, &new_map); + list_add(&itm->list, &qos_info->flow_head); + + /* Create or update bearer map */ + bearer = __qmi_rmnet_bearer_get(qos_info, new_map.bearer_id); + if (!bearer) { + rc = -ENOMEM; + goto done; + } + + itm->bearer = bearer; + + __qmi_rmnet_update_mq(dev, qos_info, bearer, itm); + +done: + spin_unlock_bh(&qos_info->qos_lock); + + qmi_rmnet_bearer_clean(qos_info); + + return rc; +} + +static int +qmi_rmnet_del_flow(struct net_device *dev, struct tcmsg *tcm, + struct qmi_info *qmi) +{ + struct qos_info *qos_info = (struct qos_info *)rmnet_get_qos_pt(dev); + struct rmnet_flow_map new_map, *itm; + + if (!qos_info) + return -EINVAL; + + ASSERT_RTNL(); + + /* flow deactivate + * tcm->tcm__pad1 - bearer_id, tcm->tcm_parent - flow_id, + * tcm->tcm_ifindex - ip_type + */ + + spin_lock_bh(&qos_info->qos_lock); + + new_map.bearer_id = tcm->tcm__pad1; + new_map.flow_id = tcm->tcm_parent; + new_map.ip_type = tcm->tcm_ifindex; + itm = qmi_rmnet_get_flow_map(qos_info, new_map.flow_id, + new_map.ip_type); + if (itm) { + trace_dfc_flow_info(dev->name, new_map.bearer_id, + new_map.flow_id, new_map.ip_type, + itm->mq_idx, 0); + + __qmi_rmnet_bearer_put(dev, qos_info, itm->bearer, true); + + /* Remove from flow map */ + list_del(&itm->list); + kfree(itm); + } + + if (list_empty(&qos_info->flow_head)) + netif_tx_wake_all_queues(dev); + + spin_unlock_bh(&qos_info->qos_lock); + + qmi_rmnet_bearer_clean(qos_info); + + return 0; +} + +static void qmi_rmnet_query_flows(struct qmi_info *qmi) +{ + int i; + + for (i = 0; i < MAX_CLIENT_NUM; i++) { + if (qmi->dfc_clients[i] && !dfc_qmap && + !qmi->dfc_client_exiting[i]) + dfc_qmi_query_flow(qmi->dfc_clients[i]); + } +} + +struct rmnet_bearer_map *qmi_rmnet_get_bearer_noref(struct qos_info *qos_info, + u8 bearer_id) +{ + struct rmnet_bearer_map *bearer; + + bearer = __qmi_rmnet_bearer_get(qos_info, bearer_id); + if (bearer) + bearer->flow_ref--; + + return bearer; +} + +#else +static inline void +qmi_rmnet_update_flow_map(struct rmnet_flow_map *itm, + struct rmnet_flow_map *new_map) +{ +} + +static inline int +qmi_rmnet_add_flow(struct net_device *dev, struct tcmsg *tcm, + struct qmi_info *qmi) +{ + return -EINVAL; +} + +static inline int +qmi_rmnet_del_flow(struct net_device *dev, struct tcmsg *tcm, + struct qmi_info *qmi) +{ + return -EINVAL; +} + +static inline void qmi_rmnet_query_flows(struct qmi_info *qmi) +{ +} +#endif + +static int +qmi_rmnet_setup_client(void *port, struct qmi_info *qmi, struct tcmsg *tcm) +{ + int idx, err = 0; + struct svc_info svc; + + ASSERT_RTNL(); + + /* client setup + * tcm->tcm_handle - instance, tcm->tcm_info - ep_type, + * tcm->tcm_parent - iface_id, tcm->tcm_ifindex - flags + */ + idx = (tcm->tcm_handle == 0) ? 0 : 1; + + if (!qmi) { + qmi = kzalloc(sizeof(struct qmi_info), GFP_ATOMIC); + if (!qmi) + return -ENOMEM; + + qmi->ws = wakeup_source_register(NULL, "RMNET_DFC"); + rmnet_init_qmi_pt(port, qmi); + } + + qmi->flag = tcm->tcm_ifindex; + svc.instance = tcm->tcm_handle; + svc.ep_type = tcm->tcm_info; + svc.iface_id = tcm->tcm_parent; + + if (DFC_SUPPORTED_MODE(dfc_mode) && + !qmi->dfc_clients[idx] && !qmi->dfc_pending[idx]) { + if (dfc_qmap) + err = dfc_qmap_client_init(port, idx, &svc, qmi); + else + err = dfc_qmi_client_init(port, idx, &svc, qmi); + qmi->dfc_client_exiting[idx] = false; + } + + if ((tcm->tcm_ifindex & FLAG_POWERSAVE_MASK) && + (idx == 0) && !qmi->wda_client && !qmi->wda_pending) { + err = wda_qmi_client_init(port, &svc, qmi); + } + + return err; +} + +static int +__qmi_rmnet_delete_client(void *port, struct qmi_info *qmi, int idx) +{ + void *data = NULL; + + ASSERT_RTNL(); + + if (qmi->dfc_clients[idx]) + data = qmi->dfc_clients[idx]; + else if (qmi->dfc_pending[idx]) + data = qmi->dfc_pending[idx]; + + if (data) { + if (dfc_qmap) + dfc_qmap_client_exit(data); + else + dfc_qmi_client_exit(data); + qmi->dfc_clients[idx] = NULL; + qmi->dfc_pending[idx] = NULL; + } + + if (!qmi_rmnet_has_client(qmi) && !qmi_rmnet_has_pending(qmi)) { + rmnet_reset_qmi_pt(port); + wakeup_source_unregister(qmi->ws); + kfree(qmi); + return 0; + } + + return 1; +} + +static void +qmi_rmnet_delete_client(void *port, struct qmi_info *qmi, struct tcmsg *tcm) +{ + int idx; + void *data = NULL; + + /* client delete: tcm->tcm_handle - instance*/ + idx = (tcm->tcm_handle == 0) ? 0 : 1; + + ASSERT_RTNL(); + if (qmi->wda_client) + data = qmi->wda_client; + else if (qmi->wda_pending) + data = qmi->wda_pending; + + if ((idx == 0) && data) { + wda_qmi_client_exit(data); + qmi->wda_client = NULL; + qmi->wda_pending = NULL; + } else { + qmi->dfc_client_exiting[idx] = true; + qmi_rmnet_flush_ps_wq(); + } + + __qmi_rmnet_delete_client(port, qmi, idx); +} + +void qmi_rmnet_change_link(struct net_device *dev, void *port, void *tcm_pt) +{ + struct qmi_info *qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + struct tcmsg *tcm = (struct tcmsg *)tcm_pt; + void *wda_data = NULL; + + switch (tcm->tcm_family) { + case NLMSG_FLOW_ACTIVATE: + if (!qmi || !DFC_SUPPORTED_MODE(dfc_mode) || + !qmi_rmnet_has_dfc_client(qmi)) + return; + + qmi_rmnet_add_flow(dev, tcm, qmi); + break; + case NLMSG_FLOW_DEACTIVATE: + if (!qmi || !DFC_SUPPORTED_MODE(dfc_mode)) + return; + + qmi_rmnet_del_flow(dev, tcm, qmi); + break; + case NLMSG_CLIENT_SETUP: + dfc_mode = FLAG_TO_MODE(tcm->tcm_ifindex); + dfc_qmap = FLAG_TO_QMAP(tcm->tcm_ifindex); + + if (!DFC_SUPPORTED_MODE(dfc_mode) && + !(tcm->tcm_ifindex & FLAG_POWERSAVE_MASK)) + return; + + if (qmi_rmnet_setup_client(port, qmi, tcm) < 0) { + /* retrieve qmi again as it could have been changed */ + qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + if (qmi && + !qmi_rmnet_has_client(qmi) && + !qmi_rmnet_has_pending(qmi)) { + rmnet_reset_qmi_pt(port); + wakeup_source_unregister(qmi->ws); + kfree(qmi); + } + } else if (tcm->tcm_ifindex & FLAG_POWERSAVE_MASK) { + qmi_rmnet_work_init(port); + rmnet_set_powersave_format(port); + } + break; + case NLMSG_CLIENT_DELETE: + if (!qmi) + return; + if (tcm->tcm_handle == 0) { /* instance 0 */ + rmnet_clear_powersave_format(port); + if (qmi->wda_client) + wda_data = qmi->wda_client; + else if (qmi->wda_pending) + wda_data = qmi->wda_pending; + wda_qmi_client_release(wda_data); + qmi_rmnet_work_exit(port); + } + qmi_rmnet_delete_client(port, qmi, tcm); + break; + case NLMSG_SCALE_FACTOR: + if (!tcm->tcm_ifindex) + return; + qmi_rmnet_scale_factor = tcm->tcm_ifindex; + break; + case NLMSG_WQ_FREQUENCY: + rmnet_wq_frequency = tcm->tcm_ifindex; + break; + default: + pr_debug("%s(): No handler\n", __func__); + break; + } +} +EXPORT_SYMBOL(qmi_rmnet_change_link); + +void qmi_rmnet_qmi_exit(void *qmi_pt, void *port) +{ + struct qmi_info *qmi = (struct qmi_info *)qmi_pt; + int i; + void *data = NULL; + + if (!qmi) + return; + + ASSERT_RTNL(); + + if (qmi->wda_client) + data = qmi->wda_client; + else if (qmi->wda_pending) + data = qmi->wda_pending; + + wda_qmi_client_release(data); + qmi_rmnet_work_exit(port); + + if (data) { + wda_qmi_client_exit(data); + qmi->wda_client = NULL; + qmi->wda_pending = NULL; + } + + for (i = 0; i < MAX_CLIENT_NUM; i++) { + if (!__qmi_rmnet_delete_client(port, qmi, i)) + return; + } +} +EXPORT_SYMBOL(qmi_rmnet_qmi_exit); + +void qmi_rmnet_enable_all_flows(struct net_device *dev) +{ + struct qos_info *qos; + struct rmnet_bearer_map *bearer; + bool do_wake; + + qos = (struct qos_info *)rmnet_get_qos_pt(dev); + if (!qos) + return; + + spin_lock_bh(&qos->qos_lock); + + list_for_each_entry(bearer, &qos->bearer_head, list) { + bearer->seq = 0; + bearer->ack_req = 0; + bearer->bytes_in_flight = 0; + bearer->tcp_bidir = false; + bearer->rat_switch = false; + + qmi_rmnet_watchdog_remove(bearer); + + if (bearer->tx_off) + continue; + + do_wake = !bearer->grant_size; + bearer->grant_size = DEFAULT_GRANT; + bearer->grant_thresh = qmi_rmnet_grant_per(DEFAULT_GRANT); + + if (do_wake) + dfc_bearer_flow_ctl(dev, bearer, qos); + } + + spin_unlock_bh(&qos->qos_lock); +} +EXPORT_SYMBOL(qmi_rmnet_enable_all_flows); + +bool qmi_rmnet_all_flows_enabled(struct net_device *dev) +{ + struct qos_info *qos; + struct rmnet_bearer_map *bearer; + bool ret = true; + + qos = (struct qos_info *)rmnet_get_qos_pt(dev); + if (!qos) + return true; + + spin_lock_bh(&qos->qos_lock); + + list_for_each_entry(bearer, &qos->bearer_head, list) { + if (!bearer->grant_size) { + ret = false; + break; + } + } + + spin_unlock_bh(&qos->qos_lock); + + return ret; +} +EXPORT_SYMBOL(qmi_rmnet_all_flows_enabled); + +#ifdef CONFIG_QTI_QMI_DFC +void qmi_rmnet_burst_fc_check(struct net_device *dev, + int ip_type, u32 mark, unsigned int len) +{ + struct qos_info *qos = rmnet_get_qos_pt(dev); + + if (!qos) + return; + + dfc_qmi_burst_check(dev, qos, ip_type, mark, len); +} +EXPORT_SYMBOL(qmi_rmnet_burst_fc_check); + +static bool _qmi_rmnet_is_tcp_ack(struct sk_buff *skb) +{ + switch (skb->protocol) { + /* TCPv4 ACKs */ + case htons(ETH_P_IP): + if ((ip_hdr(skb)->protocol == IPPROTO_TCP) && + (ntohs(ip_hdr(skb)->tot_len) - (ip_hdr(skb)->ihl << 2) == + tcp_hdr(skb)->doff << 2) && + ((tcp_flag_word(tcp_hdr(skb)) & + cpu_to_be32(0x00FF0000)) == TCP_FLAG_ACK)) + return true; + break; + + /* TCPv6 ACKs */ + case htons(ETH_P_IPV6): + if ((ipv6_hdr(skb)->nexthdr == IPPROTO_TCP) && + (ntohs(ipv6_hdr(skb)->payload_len) == + (tcp_hdr(skb)->doff) << 2) && + ((tcp_flag_word(tcp_hdr(skb)) & + cpu_to_be32(0x00FF0000)) == TCP_FLAG_ACK)) + return true; + break; + } + + return false; +} + +static inline bool qmi_rmnet_is_tcp_ack(struct sk_buff *skb) +{ + /* Locally generated TCP acks */ + if (skb_is_tcp_pure_ack(skb)) + return true; + + /* Forwarded */ + if (unlikely(_qmi_rmnet_is_tcp_ack(skb))) + return true; + + return false; +} + +static int qmi_rmnet_get_queue_sa(struct qos_info *qos, struct sk_buff *skb) +{ + struct rmnet_flow_map *itm; + int ip_type; + int txq = DEFAULT_MQ_NUM; + + /* Put NDP in default mq */ + if (skb->protocol == htons(ETH_P_IPV6) && + ipv6_hdr(skb)->nexthdr == IPPROTO_ICMPV6 && + icmp6_hdr(skb)->icmp6_type >= 133 && + icmp6_hdr(skb)->icmp6_type <= 137) { + return DEFAULT_MQ_NUM; + } + + ip_type = (skb->protocol == htons(ETH_P_IPV6)) ? AF_INET6 : AF_INET; + + spin_lock_bh(&qos->qos_lock); + + itm = qmi_rmnet_get_flow_map(qos, skb->mark, ip_type); + if (unlikely(!itm)) + goto done; + + /* Put the packet in the assigned mq except TCP ack */ + if (likely(itm->bearer) && qmi_rmnet_is_tcp_ack(skb)) + txq = itm->bearer->ack_mq_idx; + else + txq = itm->mq_idx; + +done: + spin_unlock_bh(&qos->qos_lock); + return txq; +} + +int qmi_rmnet_get_queue(struct net_device *dev, struct sk_buff *skb) +{ + struct qos_info *qos = rmnet_get_qos_pt(dev); + int txq = 0, ip_type = AF_INET; + struct rmnet_flow_map *itm; + u32 mark = skb->mark; + + if (!qos) + return 0; + + if (likely(dfc_mode == DFC_MODE_SA)) + return qmi_rmnet_get_queue_sa(qos, skb); + + ip_type = (skb->protocol == htons(ETH_P_IPV6)) ? AF_INET6 : AF_INET; + + spin_lock_bh(&qos->qos_lock); + + itm = qmi_rmnet_get_flow_map(qos, mark, ip_type); + if (itm) + txq = itm->mq_idx; + + spin_unlock_bh(&qos->qos_lock); + + return txq; +} +EXPORT_SYMBOL(qmi_rmnet_get_queue); + +inline unsigned int qmi_rmnet_grant_per(unsigned int grant) +{ + return grant / qmi_rmnet_scale_factor; +} +EXPORT_SYMBOL(qmi_rmnet_grant_per); + +void *qmi_rmnet_qos_init(struct net_device *real_dev, + struct net_device *vnd_dev, u8 mux_id) +{ + struct qos_info *qos; + + qos = kzalloc(sizeof(*qos), GFP_KERNEL); + if (!qos) + return NULL; + + qos->mux_id = mux_id; + qos->real_dev = real_dev; + qos->vnd_dev = vnd_dev; + qos->tran_num = 0; + INIT_LIST_HEAD(&qos->flow_head); + INIT_LIST_HEAD(&qos->bearer_head); + spin_lock_init(&qos->qos_lock); + + return qos; +} +EXPORT_SYMBOL(qmi_rmnet_qos_init); + +void qmi_rmnet_qos_exit_pre(void *qos) +{ + struct qos_info *qosi = (struct qos_info *)qos; + struct rmnet_bearer_map *bearer; + + if (!qos) + return; + + list_for_each_entry(bearer, &qosi->bearer_head, list) { + bearer->watchdog_quit = true; + del_timer_sync(&bearer->watchdog); + } + + list_add(&qosi->list, &qos_cleanup_list); +} +EXPORT_SYMBOL(qmi_rmnet_qos_exit_pre); + +void qmi_rmnet_qos_exit_post(void) +{ + struct qos_info *qos, *tmp; + + synchronize_rcu(); + list_for_each_entry_safe(qos, tmp, &qos_cleanup_list, list) { + list_del(&qos->list); + qmi_rmnet_clean_flow_list(qos); + kfree(qos); + } +} +EXPORT_SYMBOL(qmi_rmnet_qos_exit_post); +#endif + +#ifdef CONFIG_QTI_QMI_POWER_COLLAPSE +static struct workqueue_struct *rmnet_ps_wq; +static struct rmnet_powersave_work *rmnet_work; +static bool rmnet_work_quit; +static bool rmnet_work_inited; +static LIST_HEAD(ps_list); + +struct rmnet_powersave_work { + struct delayed_work work; + struct alarm atimer; + void *port; + u64 old_rx_pkts; + u64 old_tx_pkts; +}; + +void qmi_rmnet_ps_on_notify(void *port) +{ + struct qmi_rmnet_ps_ind *tmp; + + list_for_each_entry_rcu(tmp, &ps_list, list) + tmp->ps_on_handler(port); +} +EXPORT_SYMBOL(qmi_rmnet_ps_on_notify); + +void qmi_rmnet_ps_off_notify(void *port) +{ + struct qmi_rmnet_ps_ind *tmp; + + list_for_each_entry_rcu(tmp, &ps_list, list) + tmp->ps_off_handler(port); +} +EXPORT_SYMBOL(qmi_rmnet_ps_off_notify); + +int qmi_rmnet_ps_ind_register(void *port, + struct qmi_rmnet_ps_ind *ps_ind) +{ + + if (!port || !ps_ind || !ps_ind->ps_on_handler || + !ps_ind->ps_off_handler) + return -EINVAL; + + list_add_rcu(&ps_ind->list, &ps_list); + + return 0; +} +EXPORT_SYMBOL(qmi_rmnet_ps_ind_register); + +int qmi_rmnet_ps_ind_deregister(void *port, + struct qmi_rmnet_ps_ind *ps_ind) +{ + struct qmi_rmnet_ps_ind *tmp; + + if (!port || !ps_ind) + return -EINVAL; + + list_for_each_entry_rcu(tmp, &ps_list, list) { + if (tmp == ps_ind) { + list_del_rcu(&ps_ind->list); + goto done; + } + } +done: + return 0; +} +EXPORT_SYMBOL(qmi_rmnet_ps_ind_deregister); + +int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable) +{ + int rc = -EINVAL; + struct qmi_info *qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + + if (!qmi || !qmi->wda_client) + return rc; + + rc = wda_set_powersave_mode(qmi->wda_client, enable); + if (rc < 0) { + pr_err("%s() failed set powersave mode[%u], err=%d\n", + __func__, enable, rc); + return rc; + } + + return 0; +} +EXPORT_SYMBOL(qmi_rmnet_set_powersave_mode); + +static void qmi_rmnet_work_restart(void *port) +{ + rcu_read_lock(); + if (!rmnet_work_quit) + queue_delayed_work(rmnet_ps_wq, &rmnet_work->work, NO_DELAY); + rcu_read_unlock(); +} + +static enum alarmtimer_restart qmi_rmnet_work_alarm(struct alarm *atimer, + ktime_t now) +{ + struct rmnet_powersave_work *real_work; + + real_work = container_of(atimer, struct rmnet_powersave_work, atimer); + qmi_rmnet_work_restart(real_work->port); + return ALARMTIMER_NORESTART; +} + +static void dfc_wakelock_acquire(struct qmi_info *qmi) +{ + if (qmi && !qmi->wakelock_active) { + __pm_stay_awake(qmi->ws); + qmi->wakelock_active = true; + } +} + +static void dfc_wakelock_release(struct qmi_info *qmi) +{ + if (qmi && qmi->wakelock_active) { + __pm_relax(qmi->ws); + qmi->wakelock_active = false; + } +} + +static void qmi_rmnet_check_stats(struct work_struct *work) +{ + struct rmnet_powersave_work *real_work; + struct qmi_info *qmi; + u64 rxd, txd; + u64 rx, tx; + bool dl_msg_active; + bool use_alarm_timer = true; + + real_work = container_of(to_delayed_work(work), + struct rmnet_powersave_work, work); + + if (unlikely(!real_work || !real_work->port)) + return; + + qmi = (struct qmi_info *)rmnet_get_qmi_pt(real_work->port); + if (unlikely(!qmi)) + return; + + dfc_wakelock_release(qmi); + + rmnet_get_packets(real_work->port, &rx, &tx); + rxd = rx - real_work->old_rx_pkts; + txd = tx - real_work->old_tx_pkts; + real_work->old_rx_pkts = rx; + real_work->old_tx_pkts = tx; + + dl_msg_active = qmi->dl_msg_active; + qmi->dl_msg_active = false; + + if (qmi->ps_enabled) { + + /* Ready to accept grant */ + qmi->ps_ignore_grant = false; + + /* Register to get QMI DFC and DL marker */ + if (qmi_rmnet_set_powersave_mode(real_work->port, 0) < 0) + goto end; + + qmi->ps_enabled = false; + + /* Do a query when coming out of powersave */ + qmi_rmnet_query_flows(qmi); + + if (rmnet_get_powersave_notif(real_work->port)) + qmi_rmnet_ps_off_notify(real_work->port); + + goto end; + } + + if (!rxd && !txd) { + /* If no DL msg received and there is a flow disabled, + * (likely in RLF), no need to enter powersave + */ + if (!dl_msg_active && + !rmnet_all_flows_enabled(real_work->port)) { + use_alarm_timer = false; + goto end; + } + + /* Deregister to suppress QMI DFC and DL marker */ + if (qmi_rmnet_set_powersave_mode(real_work->port, 1) < 0) + goto end; + + qmi->ps_enabled = true; + + /* Ignore grant after going into powersave */ + qmi->ps_ignore_grant = true; + + /* Clear the bit before enabling flow so pending packets + * can trigger the work again + */ + clear_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active); + rmnet_enable_all_flows(real_work->port); + + if (rmnet_get_powersave_notif(real_work->port)) + qmi_rmnet_ps_on_notify(real_work->port); + + return; + } +end: + rcu_read_lock(); + if (!rmnet_work_quit) { + if (use_alarm_timer) { + /* Suspend will fail and get delayed for 2s if + * alarmtimer expires within 2s. Hold a wakelock + * for the actual timer duration to prevent suspend + */ + if (PS_INTERVAL_MS < 2000) + dfc_wakelock_acquire(qmi); + alarm_start_relative(&real_work->atimer, + PS_INTERVAL_KT); + } else { + queue_delayed_work(rmnet_ps_wq, &real_work->work, + PS_INTERVAL); + } + } + rcu_read_unlock(); +} + +static void qmi_rmnet_work_set_active(void *port, int status) +{ + struct qmi_info *qmi; + + qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + if (unlikely(!qmi)) + return; + + if (status) + set_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active); + else + clear_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active); +} + +void qmi_rmnet_work_init(void *port) +{ + if (rmnet_ps_wq) + return; + + rmnet_ps_wq = alloc_workqueue("rmnet_powersave_work", + WQ_CPU_INTENSIVE, 1); + + if (!rmnet_ps_wq) + return; + + rmnet_work = kzalloc(sizeof(*rmnet_work), GFP_ATOMIC); + if (!rmnet_work) { + destroy_workqueue(rmnet_ps_wq); + rmnet_ps_wq = NULL; + return; + } + INIT_DEFERRABLE_WORK(&rmnet_work->work, qmi_rmnet_check_stats); + alarm_init(&rmnet_work->atimer, ALARM_BOOTTIME, qmi_rmnet_work_alarm); + rmnet_work->port = port; + rmnet_get_packets(rmnet_work->port, &rmnet_work->old_rx_pkts, + &rmnet_work->old_tx_pkts); + + rmnet_work_quit = false; + qmi_rmnet_work_set_active(rmnet_work->port, 1); + queue_delayed_work(rmnet_ps_wq, &rmnet_work->work, PS_INTERVAL); + rmnet_work_inited = true; +} +EXPORT_SYMBOL(qmi_rmnet_work_init); + +void qmi_rmnet_work_maybe_restart(void *port) +{ + struct qmi_info *qmi; + + qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + if (unlikely(!qmi || !rmnet_work_inited)) + return; + + if (!test_and_set_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active)) + qmi_rmnet_work_restart(port); +} +EXPORT_SYMBOL(qmi_rmnet_work_maybe_restart); + +void qmi_rmnet_work_exit(void *port) +{ + if (!rmnet_ps_wq || !rmnet_work) + return; + + rmnet_work_quit = true; + synchronize_rcu(); + + rmnet_work_inited = false; + alarm_cancel(&rmnet_work->atimer); + cancel_delayed_work_sync(&rmnet_work->work); + destroy_workqueue(rmnet_ps_wq); + qmi_rmnet_work_set_active(port, 0); + rmnet_ps_wq = NULL; + kfree(rmnet_work); + rmnet_work = NULL; + dfc_wakelock_release((struct qmi_info *)rmnet_get_qmi_pt(port)); +} +EXPORT_SYMBOL(qmi_rmnet_work_exit); + +void qmi_rmnet_set_dl_msg_active(void *port) +{ + struct qmi_info *qmi; + + qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + if (unlikely(!qmi)) + return; + + qmi->dl_msg_active = true; +} +EXPORT_SYMBOL(qmi_rmnet_set_dl_msg_active); + +void qmi_rmnet_flush_ps_wq(void) +{ + if (rmnet_ps_wq) + flush_workqueue(rmnet_ps_wq); +} + +bool qmi_rmnet_ignore_grant(void *port) +{ + struct qmi_info *qmi; + + qmi = (struct qmi_info *)rmnet_get_qmi_pt(port); + if (unlikely(!qmi)) + return false; + + return qmi->ps_ignore_grant; +} +EXPORT_SYMBOL(qmi_rmnet_ignore_grant); +#endif diff --git a/techpack/datarmnet/core/qmi_rmnet.h b/techpack/datarmnet/core/qmi_rmnet.h new file mode 100644 index 000000000000..80689e807236 --- /dev/null +++ b/techpack/datarmnet/core/qmi_rmnet.h @@ -0,0 +1,153 @@ +/* + * Copyright (c) 2018-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _QMI_RMNET_H +#define _QMI_RMNET_H + +#include +#include +#define CONFIG_QTI_QMI_RMNET 1 +#define CONFIG_QTI_QMI_DFC 1 +#define CONFIG_QTI_QMI_POWER_COLLAPSE 1 + +struct qmi_rmnet_ps_ind { + void (*ps_on_handler)(void *port); + void (*ps_off_handler)(void *port); + struct list_head list; +}; + + +#ifdef CONFIG_QTI_QMI_RMNET +void qmi_rmnet_qmi_exit(void *qmi_pt, void *port); +void qmi_rmnet_change_link(struct net_device *dev, void *port, void *tcm_pt); +void qmi_rmnet_enable_all_flows(struct net_device *dev); +bool qmi_rmnet_all_flows_enabled(struct net_device *dev); +#else +static inline void qmi_rmnet_qmi_exit(void *qmi_pt, void *port) +{ +} + +static inline void +qmi_rmnet_change_link(struct net_device *dev, void *port, void *tcm_pt) +{ +} + +static inline void +qmi_rmnet_enable_all_flows(struct net_device *dev) +{ +} + +static inline bool +qmi_rmnet_all_flows_enabled(struct net_device *dev) +{ + return true; +} +#endif + +#ifdef CONFIG_QTI_QMI_DFC +void *qmi_rmnet_qos_init(struct net_device *real_dev, + struct net_device *vnd_dev, u8 mux_id); +void qmi_rmnet_qos_exit_pre(void *qos); +void qmi_rmnet_qos_exit_post(void); +void qmi_rmnet_burst_fc_check(struct net_device *dev, + int ip_type, u32 mark, unsigned int len); +int qmi_rmnet_get_queue(struct net_device *dev, struct sk_buff *skb); +#else +static inline void * +qmi_rmnet_qos_init(struct net_device *real_dev, + struct net_device *vnd_dev, u8 mux_id) +{ + return NULL; +} + +static inline void qmi_rmnet_qos_exit_pre(void *qos) +{ +} + +static inline void qmi_rmnet_qos_exit_post(void) +{ +} + +static inline void +qmi_rmnet_burst_fc_check(struct net_device *dev, + int ip_type, u32 mark, unsigned int len) +{ +} + +static inline int qmi_rmnet_get_queue(struct net_device *dev, + struct sk_buff *skb) +{ + return 0; +} +#endif + +#ifdef CONFIG_QTI_QMI_POWER_COLLAPSE +int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable); +void qmi_rmnet_work_init(void *port); +void qmi_rmnet_work_exit(void *port); +void qmi_rmnet_work_maybe_restart(void *port); +void qmi_rmnet_set_dl_msg_active(void *port); +bool qmi_rmnet_ignore_grant(void *port); + +int qmi_rmnet_ps_ind_register(void *port, + struct qmi_rmnet_ps_ind *ps_ind); +int qmi_rmnet_ps_ind_deregister(void *port, + struct qmi_rmnet_ps_ind *ps_ind); +void qmi_rmnet_ps_off_notify(void *port); +void qmi_rmnet_ps_on_notify(void *port); + +#else +static inline int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable) +{ + return 0; +} +static inline void qmi_rmnet_work_init(void *port) +{ +} +static inline void qmi_rmnet_work_exit(void *port) +{ +} +static inline void qmi_rmnet_work_maybe_restart(void *port) +{ + +} +static inline void qmi_rmnet_set_dl_msg_active(void *port) +{ +} +static inline bool qmi_rmnet_ignore_grant(void *port) +{ + return false; +} + +static inline int qmi_rmnet_ps_ind_register(struct rmnet_port *port, + struct qmi_rmnet_ps_ind *ps_ind) +{ + return 0; +} +static inline int qmi_rmnet_ps_ind_deregister(struct rmnet_port *port, + struct qmi_rmnet_ps_ind *ps_ind) +{ + return 0; +} + +static inline void qmi_rmnet_ps_off_notify(struct rmnet_port *port) +{ + +} + +static inline void qmi_rmnet_ps_on_notify(struct rmnet_port *port) +{ + +} +#endif +#endif /*_QMI_RMNET_H*/ diff --git a/techpack/datarmnet/core/qmi_rmnet_i.h b/techpack/datarmnet/core/qmi_rmnet_i.h new file mode 100644 index 000000000000..082351065686 --- /dev/null +++ b/techpack/datarmnet/core/qmi_rmnet_i.h @@ -0,0 +1,253 @@ +/* + * Copyright (c) 2018-2021, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _RMNET_QMI_I_H +#define _RMNET_QMI_I_H + +#include +#include +#include +#include + +#define MAX_MQ_NUM 16 +#define MAX_CLIENT_NUM 2 +#define MAX_FLOW_NUM 32 +#define DEFAULT_GRANT 1 +#define DEFAULT_CALL_GRANT 20480 +#define DFC_MAX_BEARERS_V01 16 +#define DEFAULT_MQ_NUM 0 +#define ACK_MQ_OFFSET (MAX_MQ_NUM - 1) +#define INVALID_MQ 0xFF + +#define DFC_MODE_SA 4 + +#define CONFIG_QTI_QMI_RMNET 1 +#define CONFIG_QTI_QMI_DFC 1 +#define CONFIG_QTI_QMI_POWER_COLLAPSE 1 + +extern int dfc_mode; +extern int dfc_qmap; + +struct qos_info; + +struct rmnet_bearer_map { + struct list_head list; + u8 bearer_id; + int flow_ref; + u32 grant_size; + u32 grant_thresh; + u16 seq; + u8 ack_req; + u32 last_grant; + u16 last_seq; + u32 bytes_in_flight; + u32 last_adjusted_grant; + bool tcp_bidir; + bool rat_switch; + bool tx_off; + u32 ack_txid; + u32 mq_idx; + u32 ack_mq_idx; + struct qos_info *qos; + struct timer_list watchdog; + bool watchdog_started; + bool watchdog_quit; + u32 watchdog_expire_cnt; +}; + +struct rmnet_flow_map { + struct list_head list; + u8 bearer_id; + u32 flow_id; + int ip_type; + u32 mq_idx; + struct rmnet_bearer_map *bearer; +}; + +struct svc_info { + u32 instance; + u32 ep_type; + u32 iface_id; +}; + +struct mq_map { + struct rmnet_bearer_map *bearer; +}; + +struct qos_info { + struct list_head list; + u8 mux_id; + struct net_device *real_dev; + struct net_device *vnd_dev; + struct list_head flow_head; + struct list_head bearer_head; + struct mq_map mq[MAX_MQ_NUM]; + u32 tran_num; + spinlock_t qos_lock; + struct rmnet_bearer_map *removed_bearer; +}; + +struct qmi_info { + int flag; + void *wda_client; + void *wda_pending; + void *dfc_clients[MAX_CLIENT_NUM]; + void *dfc_pending[MAX_CLIENT_NUM]; + bool dfc_client_exiting[MAX_CLIENT_NUM]; + unsigned long ps_work_active; + bool ps_enabled; + bool dl_msg_active; + bool ps_ignore_grant; + bool wakelock_active; + struct wakeup_source *ws; +}; + +enum data_ep_type_enum_v01 { + DATA_EP_TYPE_ENUM_MIN_ENUM_VAL_V01 = INT_MIN, + DATA_EP_TYPE_RESERVED_V01 = 0x00, + DATA_EP_TYPE_HSIC_V01 = 0x01, + DATA_EP_TYPE_HSUSB_V01 = 0x02, + DATA_EP_TYPE_PCIE_V01 = 0x03, + DATA_EP_TYPE_EMBEDDED_V01 = 0x04, + DATA_EP_TYPE_ENUM_MAX_ENUM_VAL_V01 = INT_MAX +}; + +struct data_ep_id_type_v01 { + + enum data_ep_type_enum_v01 ep_type; + u32 iface_id; +}; + +extern struct qmi_elem_info data_ep_id_type_v01_ei[]; + +void *qmi_rmnet_has_dfc_client(struct qmi_info *qmi); + +#ifdef CONFIG_QTI_QMI_DFC +struct rmnet_flow_map * +qmi_rmnet_get_flow_map(struct qos_info *qos_info, + u32 flow_id, int ip_type); + +struct rmnet_bearer_map * +qmi_rmnet_get_bearer_map(struct qos_info *qos_info, u8 bearer_id); + +unsigned int qmi_rmnet_grant_per(unsigned int grant); + +int dfc_qmi_client_init(void *port, int index, struct svc_info *psvc, + struct qmi_info *qmi); + +void dfc_qmi_client_exit(void *dfc_data); + +void dfc_qmi_burst_check(struct net_device *dev, struct qos_info *qos, + int ip_type, u32 mark, unsigned int len); + +int qmi_rmnet_flow_control(struct net_device *dev, u32 mq_idx, int enable); + +void dfc_qmi_query_flow(void *dfc_data); + +int dfc_bearer_flow_ctl(struct net_device *dev, + struct rmnet_bearer_map *bearer, + struct qos_info *qos); + +int dfc_qmap_client_init(void *port, int index, struct svc_info *psvc, + struct qmi_info *qmi); + +void dfc_qmap_client_exit(void *dfc_data); + +void dfc_qmap_send_ack(struct qos_info *qos, u8 bearer_id, u16 seq, u8 type); + +struct rmnet_bearer_map *qmi_rmnet_get_bearer_noref(struct qos_info *qos_info, + u8 bearer_id); + +void qmi_rmnet_watchdog_add(struct rmnet_bearer_map *bearer); + +void qmi_rmnet_watchdog_remove(struct rmnet_bearer_map *bearer); + +#else +static inline struct rmnet_flow_map * +qmi_rmnet_get_flow_map(struct qos_info *qos_info, + uint32_t flow_id, int ip_type) +{ + return NULL; +} + +static inline struct rmnet_bearer_map * +qmi_rmnet_get_bearer_map(struct qos_info *qos_info, u8 bearer_id) +{ + return NULL; +} + +static inline int +dfc_qmi_client_init(void *port, int index, struct svc_info *psvc, + struct qmi_info *qmi) +{ + return -EINVAL; +} + +static inline void dfc_qmi_client_exit(void *dfc_data) +{ +} + +static inline int +dfc_bearer_flow_ctl(struct net_device *dev, + struct rmnet_bearer_map *bearer, + struct qos_info *qos) +{ + return 0; +} + +static inline int +dfc_qmap_client_init(void *port, int index, struct svc_info *psvc, + struct qmi_info *qmi) +{ + return -EINVAL; +} + +static inline void dfc_qmap_client_exit(void *dfc_data) +{ +} + +static inline void qmi_rmnet_watchdog_remove(struct rmnet_bearer_map *bearer) +{ +} +#endif + +#ifdef CONFIG_QTI_QMI_POWER_COLLAPSE +int +wda_qmi_client_init(void *port, struct svc_info *psvc, struct qmi_info *qmi); +void wda_qmi_client_exit(void *wda_data); +int wda_set_powersave_mode(void *wda_data, u8 enable); +void qmi_rmnet_flush_ps_wq(void); +void wda_qmi_client_release(void *wda_data); +#else +static inline int +wda_qmi_client_init(void *port, struct svc_info *psvc, struct qmi_info *qmi) +{ + return -EINVAL; +} + +static inline void wda_qmi_client_exit(void *wda_data) +{ +} + +static inline int wda_set_powersave_mode(void *wda_data, u8 enable) +{ + return -EINVAL; +} +static inline void qmi_rmnet_flush_ps_wq(void) +{ +} +static inline void wda_qmi_client_release(void *wda_data) +{ +} +#endif +#endif /*_RMNET_QMI_I_H*/ diff --git a/techpack/datarmnet/core/rmnet_config.c b/techpack/datarmnet/core/rmnet_config.c new file mode 100644 index 000000000000..b341099c15b7 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_config.c @@ -0,0 +1,790 @@ +/* Copyright (c) 2013-2021, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET configuration engine + * + */ + +#include +#include +#include +#include +#include "rmnet_config.h" +#include "rmnet_handlers.h" +#include "rmnet_vnd.h" +#include "rmnet_private.h" +#include "rmnet_map.h" +#include "rmnet_descriptor.h" +#include "rmnet_genl.h" +#include "rmnet_qmi.h" +#include "qmi_rmnet.h" +#define CONFIG_QTI_QMI_RMNET 1 +#define CONFIG_QTI_QMI_DFC 1 +#define CONFIG_QTI_QMI_POWER_COLLAPSE 1 + +#define QMAP_SHS_MASK 0xFF +#define QMAP_SHS_PKT_LIMIT 200 + +/* Locking scheme - + * The shared resource which needs to be protected is realdev->rx_handler_data. + * For the writer path, this is using rtnl_lock(). The writer paths are + * rmnet_newlink(), rmnet_dellink() and rmnet_force_unassociate_device(). These + * paths are already called with rtnl_lock() acquired in. There is also an + * ASSERT_RTNL() to ensure that we are calling with rtnl acquired. For + * dereference here, we will need to use rtnl_dereference(). Dev list writing + * needs to happen with rtnl_lock() acquired for netdev_master_upper_dev_link(). + * For the reader path, the real_dev->rx_handler_data is called in the TX / RX + * path. We only need rcu_read_lock() for these scenarios. In these cases, + * the rcu_read_lock() is held in __dev_queue_xmit() and + * netif_receive_skb_internal(), so readers need to use rcu_dereference_rtnl() + * to get the relevant information. For dev list reading, we again acquire + * rcu_read_lock() in rmnet_dellink() for netdev_master_upper_dev_get_rcu(). + * We also use unregister_netdevice_many() to free all rmnet devices in + * rmnet_force_unassociate_device() so we dont lose the rtnl_lock() and free in + * same context. + */ + +/* Local Definitions and Declarations */ + +enum { + IFLA_RMNET_DFC_QOS = __IFLA_RMNET_MAX, + IFLA_RMNET_UL_AGG_PARAMS, + __IFLA_RMNET_EXT_MAX, +}; + +static const struct nla_policy rmnet_policy[__IFLA_RMNET_EXT_MAX] = { + [IFLA_RMNET_MUX_ID] = { + .type = NLA_U16 + }, + [IFLA_RMNET_FLAGS] = { + .len = sizeof(struct ifla_rmnet_flags) + }, + [IFLA_RMNET_DFC_QOS] = { + .len = sizeof(struct tcmsg) + }, + [IFLA_RMNET_UL_AGG_PARAMS] = { + .len = sizeof(struct rmnet_egress_agg_params) + }, +}; + +int rmnet_is_real_dev_registered(const struct net_device *real_dev) +{ + return rcu_access_pointer(real_dev->rx_handler) == rmnet_rx_handler; +} +EXPORT_SYMBOL(rmnet_is_real_dev_registered); + +/* Needs rtnl lock */ +static struct rmnet_port* +rmnet_get_port_rtnl(const struct net_device *real_dev) +{ + return rtnl_dereference(real_dev->rx_handler_data); +} + +static int rmnet_unregister_real_device(struct net_device *real_dev, + struct rmnet_port *port) +{ + if (port->nr_rmnet_devs) + return -EINVAL; + + netdev_rx_handler_unregister(real_dev); + + rmnet_map_cmd_exit(port); + rmnet_map_tx_aggregate_exit(port); + + rmnet_descriptor_deinit(port); + + kfree(port); + + /* release reference on real_dev */ + dev_put(real_dev); + + netdev_dbg(real_dev, "Removed from rmnet\n"); + return 0; +} + +static int rmnet_register_real_device(struct net_device *real_dev) +{ + struct rmnet_port *port; + int rc, entry; + + ASSERT_RTNL(); + + if (rmnet_is_real_dev_registered(real_dev)) + return 0; + + port = kzalloc(sizeof(*port), GFP_ATOMIC); + if (!port) + return -ENOMEM; + + port->dev = real_dev; + port->phy_shs_cfg.config = RMNET_SHS_NO_DLMKR | RMNET_SHS_NO_PSH | + RMNET_SHS_STMP_ALL; + port->phy_shs_cfg.map_mask = QMAP_SHS_MASK; + port->phy_shs_cfg.max_pkts = QMAP_SHS_PKT_LIMIT; + + rc = netdev_rx_handler_register(real_dev, rmnet_rx_handler, port); + if (rc) { + kfree(port); + return -EBUSY; + } + /* hold on to real dev for MAP data */ + dev_hold(real_dev); + + for (entry = 0; entry < RMNET_MAX_LOGICAL_EP; entry++) + INIT_HLIST_HEAD(&port->muxed_ep[entry]); + + rc = rmnet_descriptor_init(port); + if (rc) { + rmnet_descriptor_deinit(port); + return rc; + } + + rmnet_map_tx_aggregate_init(port); + rmnet_map_cmd_init(port); + + netdev_dbg(real_dev, "registered with rmnet\n"); + return 0; +} + +static void rmnet_unregister_bridge(struct net_device *dev, + struct rmnet_port *port) +{ + struct rmnet_port *bridge_port; + struct net_device *bridge_dev; + + if (port->rmnet_mode != RMNET_EPMODE_BRIDGE) + return; + + /* bridge slave handling */ + if (!port->nr_rmnet_devs) { + bridge_dev = port->bridge_ep; + + bridge_port = rmnet_get_port_rtnl(bridge_dev); + bridge_port->bridge_ep = NULL; + bridge_port->rmnet_mode = RMNET_EPMODE_VND; + } else { + bridge_dev = port->bridge_ep; + + bridge_port = rmnet_get_port_rtnl(bridge_dev); + rmnet_unregister_real_device(bridge_dev, bridge_port); + } +} + +static int rmnet_newlink(struct net *src_net, struct net_device *dev, + struct nlattr *tb[], struct nlattr *data[], + struct netlink_ext_ack *extack) +{ + struct net_device *real_dev; + int mode = RMNET_EPMODE_VND; + struct rmnet_endpoint *ep; + struct rmnet_port *port; + u32 data_format; + int err = 0; + u16 mux_id; + + real_dev = __dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK])); + if (!real_dev || !dev) + return -ENODEV; + + if (!data[IFLA_RMNET_MUX_ID]) + return -EINVAL; + + ep = kzalloc(sizeof(*ep), GFP_ATOMIC); + if (!ep) + return -ENOMEM; + + mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]); + + err = rmnet_register_real_device(real_dev); + if (err) + goto err0; + + port = rmnet_get_port_rtnl(real_dev); + err = rmnet_vnd_newlink(mux_id, dev, port, real_dev, ep); + if (err) + goto err1; + + port->rmnet_mode = mode; + + hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]); + + if (data[IFLA_RMNET_FLAGS]) { + struct ifla_rmnet_flags *flags; + + flags = nla_data(data[IFLA_RMNET_FLAGS]); + data_format = flags->flags & flags->mask; + netdev_dbg(dev, "data format [0x%08X]\n", data_format); + port->data_format = data_format; + } + + if (data[IFLA_RMNET_UL_AGG_PARAMS]) { + void *agg_params; + unsigned long irq_flags; + + agg_params = nla_data(data[IFLA_RMNET_UL_AGG_PARAMS]); + spin_lock_irqsave(&port->agg_lock, irq_flags); + memcpy(&port->egress_agg_params, agg_params, + sizeof(port->egress_agg_params)); + spin_unlock_irqrestore(&port->agg_lock, irq_flags); + } + + return 0; + +err1: + rmnet_unregister_real_device(real_dev, port); +err0: + kfree(ep); + return err; +} + +static void rmnet_dellink(struct net_device *dev, struct list_head *head) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct net_device *real_dev; + struct rmnet_endpoint *ep; + struct rmnet_port *port; + u8 mux_id; + + real_dev = priv->real_dev; + + if (!real_dev || !rmnet_is_real_dev_registered(real_dev)) + return; + + port = rmnet_get_port_rtnl(real_dev); + + mux_id = rmnet_vnd_get_mux(dev); + + ep = rmnet_get_endpoint(port, mux_id); + if (ep) { + hlist_del_init_rcu(&ep->hlnode); + rmnet_unregister_bridge(dev, port); + rmnet_vnd_dellink(mux_id, port, ep); + synchronize_rcu(); + kfree(ep); + } + + if (!port->nr_rmnet_devs) + qmi_rmnet_qmi_exit(port->qmi_info, port); + + unregister_netdevice(dev); + + qmi_rmnet_qos_exit_post(); + + rmnet_unregister_real_device(real_dev, port); +} + +static void rmnet_force_unassociate_device(struct net_device *dev) +{ + struct net_device *real_dev = dev; + struct hlist_node *tmp_ep; + struct rmnet_endpoint *ep; + struct rmnet_port *port; + unsigned long bkt_ep; + LIST_HEAD(list); + HLIST_HEAD(cleanup_list); + + if (!rmnet_is_real_dev_registered(real_dev)) + return; + + ASSERT_RTNL(); + + port = rmnet_get_port_rtnl(dev); + qmi_rmnet_qmi_exit(port->qmi_info, port); + + rmnet_unregister_bridge(dev, port); + + hash_for_each_safe(port->muxed_ep, bkt_ep, tmp_ep, ep, hlnode) { + unregister_netdevice_queue(ep->egress_dev, &list); + rmnet_vnd_dellink(ep->mux_id, port, ep); + + hlist_del_init_rcu(&ep->hlnode); + hlist_add_head(&ep->hlnode, &cleanup_list); + } + + synchronize_rcu(); + + hlist_for_each_entry_safe(ep, tmp_ep, &cleanup_list, hlnode) { + hlist_del(&ep->hlnode); + kfree(ep); + } + + /* Unregistering devices in context before freeing port. + * If this API becomes non-context their order should switch. + */ + unregister_netdevice_many(&list); + + qmi_rmnet_qos_exit_post(); + + rmnet_unregister_real_device(real_dev, port); +} + +static int rmnet_config_notify_cb(struct notifier_block *nb, + unsigned long event, void *data) +{ + struct net_device *dev = netdev_notifier_info_to_dev(data); + + if (!dev) + return NOTIFY_DONE; + + switch (event) { + case NETDEV_UNREGISTER: + netdev_dbg(dev, "Kernel unregister\n"); + rmnet_force_unassociate_device(dev); + break; + case NETDEV_DOWN: + rmnet_vnd_reset_mac_addr(dev); + break; + default: + break; + } + + return NOTIFY_DONE; +} + +static struct notifier_block rmnet_dev_notifier __read_mostly = { + .notifier_call = rmnet_config_notify_cb, +}; + +static int rmnet_rtnl_validate(struct nlattr *tb[], struct nlattr *data[], + struct netlink_ext_ack *extack) +{ + struct rmnet_egress_agg_params *agg_params; + u16 mux_id; + + if (!data) { + return -EINVAL; + } else { + if (data[IFLA_RMNET_MUX_ID]) { + mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]); + if (mux_id > (RMNET_MAX_LOGICAL_EP - 1)) + return -ERANGE; + } + + if (data[IFLA_RMNET_UL_AGG_PARAMS]) { + agg_params = nla_data(data[IFLA_RMNET_UL_AGG_PARAMS]); + if (agg_params->agg_time < 1000000) + return -EINVAL; + } + } + + return 0; +} + +static int rmnet_changelink(struct net_device *dev, struct nlattr *tb[], + struct nlattr *data[], + struct netlink_ext_ack *extack) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct net_device *real_dev; + struct rmnet_endpoint *ep; + struct rmnet_port *port; + u16 mux_id; + + real_dev = __dev_get_by_index(dev_net(dev), + nla_get_u32(tb[IFLA_LINK])); + + if (!real_dev || !dev || !rmnet_is_real_dev_registered(real_dev)) + return -ENODEV; + + port = rmnet_get_port_rtnl(real_dev); + + if (data[IFLA_RMNET_MUX_ID]) { + mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]); + ep = rmnet_get_endpoint(port, priv->mux_id); + if (!ep) + return -ENODEV; + + hlist_del_init_rcu(&ep->hlnode); + hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]); + + ep->mux_id = mux_id; + priv->mux_id = mux_id; + } + + if (data[IFLA_RMNET_FLAGS]) { + struct ifla_rmnet_flags *flags; + + flags = nla_data(data[IFLA_RMNET_FLAGS]); + port->data_format = flags->flags & flags->mask; + } + + if (data[IFLA_RMNET_DFC_QOS]) { + struct tcmsg *tcm; + + tcm = nla_data(data[IFLA_RMNET_DFC_QOS]); + qmi_rmnet_change_link(dev, port, tcm); + } + + if (data[IFLA_RMNET_UL_AGG_PARAMS]) { + struct rmnet_egress_agg_params *agg_params; + + agg_params = nla_data(data[IFLA_RMNET_UL_AGG_PARAMS]); + rmnet_map_update_ul_agg_config(port, agg_params->agg_size, + agg_params->agg_count, + agg_params->agg_features, + agg_params->agg_time); + } + + return 0; +} + +static size_t rmnet_get_size(const struct net_device *dev) +{ + return + /* IFLA_RMNET_MUX_ID */ + nla_total_size(2) + + /* IFLA_RMNET_FLAGS */ + nla_total_size(sizeof(struct ifla_rmnet_flags)) + + /* IFLA_RMNET_DFC_QOS */ + nla_total_size(sizeof(struct tcmsg)) + + /* IFLA_RMNET_UL_AGG_PARAMS */ + nla_total_size(sizeof(struct rmnet_egress_agg_params)); +} + +static int rmnet_fill_info(struct sk_buff *skb, const struct net_device *dev) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct net_device *real_dev; + struct ifla_rmnet_flags f; + struct rmnet_port *port = NULL; + + real_dev = priv->real_dev; + + if (nla_put_u16(skb, IFLA_RMNET_MUX_ID, priv->mux_id)) + goto nla_put_failure; + + if (rmnet_is_real_dev_registered(real_dev)) { + port = rmnet_get_port_rtnl(real_dev); + f.flags = port->data_format; + } else { + f.flags = 0; + } + + f.mask = ~0; + + if (nla_put(skb, IFLA_RMNET_FLAGS, sizeof(f), &f)) + goto nla_put_failure; + + if (port) { + if (nla_put(skb, IFLA_RMNET_UL_AGG_PARAMS, + sizeof(port->egress_agg_params), + &port->egress_agg_params)) + goto nla_put_failure; + } + + return 0; + +nla_put_failure: + return -EMSGSIZE; +} + +struct rtnl_link_ops rmnet_link_ops __read_mostly = { + .kind = "rmnet", + .maxtype = __IFLA_RMNET_EXT_MAX, + .priv_size = sizeof(struct rmnet_priv), + .setup = rmnet_vnd_setup, + .validate = rmnet_rtnl_validate, + .newlink = rmnet_newlink, + .dellink = rmnet_dellink, + .get_size = rmnet_get_size, + .changelink = rmnet_changelink, + .policy = rmnet_policy, + .fill_info = rmnet_fill_info, +}; + +/* Needs either rcu_read_lock() or rtnl lock */ +struct rmnet_port *rmnet_get_port(struct net_device *real_dev) +{ + if (rmnet_is_real_dev_registered(real_dev)) + return rcu_dereference_rtnl(real_dev->rx_handler_data); + else + return NULL; +} +EXPORT_SYMBOL(rmnet_get_port); + +struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id) +{ + struct rmnet_endpoint *ep; + + hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) { + if (ep->mux_id == mux_id) + return ep; + } + + return NULL; +} +EXPORT_SYMBOL(rmnet_get_endpoint); + +int rmnet_add_bridge(struct net_device *rmnet_dev, + struct net_device *slave_dev, + struct netlink_ext_ack *extack) +{ + struct rmnet_priv *priv = netdev_priv(rmnet_dev); + struct net_device *real_dev = priv->real_dev; + struct rmnet_port *port, *slave_port; + int err; + + port = rmnet_get_port(real_dev); + + /* If there is more than one rmnet dev attached, its probably being + * used for muxing. Skip the briding in that case + */ + if (port->nr_rmnet_devs > 1) + return -EINVAL; + + if (rmnet_is_real_dev_registered(slave_dev)) + return -EBUSY; + + err = rmnet_register_real_device(slave_dev); + if (err) + return -EBUSY; + + slave_port = rmnet_get_port(slave_dev); + slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE; + slave_port->bridge_ep = real_dev; + + port->rmnet_mode = RMNET_EPMODE_BRIDGE; + port->bridge_ep = slave_dev; + + netdev_dbg(slave_dev, "registered with rmnet as slave\n"); + return 0; +} + +int rmnet_del_bridge(struct net_device *rmnet_dev, + struct net_device *slave_dev) +{ + struct rmnet_priv *priv = netdev_priv(rmnet_dev); + struct net_device *real_dev = priv->real_dev; + struct rmnet_port *port, *slave_port; + + port = rmnet_get_port(real_dev); + port->rmnet_mode = RMNET_EPMODE_VND; + port->bridge_ep = NULL; + + slave_port = rmnet_get_port(slave_dev); + rmnet_unregister_real_device(slave_dev, slave_port); + + netdev_dbg(slave_dev, "removed from rmnet as slave\n"); + return 0; +} + +void *rmnet_get_qmi_pt(void *port) +{ + if (port) + return ((struct rmnet_port *)port)->qmi_info; + + return NULL; +} +EXPORT_SYMBOL(rmnet_get_qmi_pt); + +void *rmnet_get_qos_pt(struct net_device *dev) +{ + struct rmnet_priv *priv; + + if (dev) { + priv = netdev_priv(dev); + return rcu_dereference(priv->qos_info); + } + + return NULL; +} +EXPORT_SYMBOL(rmnet_get_qos_pt); + +void *rmnet_get_rmnet_port(struct net_device *dev) +{ + struct rmnet_priv *priv; + + if (dev) { + priv = netdev_priv(dev); + return (void *)rmnet_get_port(priv->real_dev); + } + + return NULL; +} +EXPORT_SYMBOL(rmnet_get_rmnet_port); + +struct net_device *rmnet_get_rmnet_dev(void *port, u8 mux_id) +{ + struct rmnet_endpoint *ep; + + if (port) { + ep = rmnet_get_endpoint((struct rmnet_port *)port, mux_id); + if (ep) + return ep->egress_dev; + } + + return NULL; +} +EXPORT_SYMBOL(rmnet_get_rmnet_dev); + +void rmnet_reset_qmi_pt(void *port) +{ + if (port) + ((struct rmnet_port *)port)->qmi_info = NULL; +} +EXPORT_SYMBOL(rmnet_reset_qmi_pt); + +void rmnet_init_qmi_pt(void *port, void *qmi) +{ + if (port) + ((struct rmnet_port *)port)->qmi_info = qmi; +} +EXPORT_SYMBOL(rmnet_init_qmi_pt); + +void rmnet_get_packets(void *port, u64 *rx, u64 *tx) +{ + struct rmnet_priv *priv; + struct rmnet_pcpu_stats *ps; + unsigned int cpu, start; + + struct rmnet_endpoint *ep; + unsigned long bkt; + + if (!port || !tx || !rx) + return; + + *tx = 0; + *rx = 0; + rcu_read_lock(); + hash_for_each(((struct rmnet_port *)port)->muxed_ep, bkt, ep, hlnode) { + priv = netdev_priv(ep->egress_dev); + for_each_possible_cpu(cpu) { + ps = per_cpu_ptr(priv->pcpu_stats, cpu); + do { + start = u64_stats_fetch_begin_irq(&ps->syncp); + *tx += ps->stats.tx_pkts; + *rx += ps->stats.rx_pkts; + } while (u64_stats_fetch_retry_irq(&ps->syncp, start)); + } + } + rcu_read_unlock(); +} +EXPORT_SYMBOL(rmnet_get_packets); + +void rmnet_set_powersave_format(void *port) +{ + if (!port) + return; + ((struct rmnet_port *)port)->data_format |= RMNET_INGRESS_FORMAT_PS; +} +EXPORT_SYMBOL(rmnet_set_powersave_format); + +void rmnet_clear_powersave_format(void *port) +{ + if (!port) + return; + ((struct rmnet_port *)port)->data_format &= ~RMNET_INGRESS_FORMAT_PS; +} +EXPORT_SYMBOL(rmnet_clear_powersave_format); + +void rmnet_enable_all_flows(void *port) +{ + struct rmnet_endpoint *ep; + unsigned long bkt; + + if (unlikely(!port)) + return; + + rcu_read_lock(); + hash_for_each_rcu(((struct rmnet_port *)port)->muxed_ep, + bkt, ep, hlnode) { + qmi_rmnet_enable_all_flows(ep->egress_dev); + } + rcu_read_unlock(); +} +EXPORT_SYMBOL(rmnet_enable_all_flows); + +bool rmnet_all_flows_enabled(void *port) +{ + struct rmnet_endpoint *ep; + unsigned long bkt; + bool ret = true; + + if (unlikely(!port)) + return true; + + rcu_read_lock(); + hash_for_each_rcu(((struct rmnet_port *)port)->muxed_ep, + bkt, ep, hlnode) { + if (!qmi_rmnet_all_flows_enabled(ep->egress_dev)) { + ret = false; + goto out; + } + } +out: + rcu_read_unlock(); + + return ret; +} +EXPORT_SYMBOL(rmnet_all_flows_enabled); + +int rmnet_get_powersave_notif(void *port) +{ + if (!port) + return 0; + return ((struct rmnet_port *)port)->data_format & RMNET_FORMAT_PS_NOTIF; +} +EXPORT_SYMBOL(rmnet_get_powersave_notif); + +struct net_device *rmnet_get_real_dev(void *port) +{ + if (port) + return ((struct rmnet_port *)port)->dev; + + return NULL; +} +EXPORT_SYMBOL(rmnet_get_real_dev); + +int rmnet_get_dlmarker_info(void *port) +{ + if (!port) + return 0; + + return ((struct rmnet_port *)port)->data_format & + (RMNET_INGRESS_FORMAT_DL_MARKER_V1 | + RMNET_INGRESS_FORMAT_DL_MARKER_V2); +} +EXPORT_SYMBOL(rmnet_get_dlmarker_info); + +/* Startup/Shutdown */ + +static int __init rmnet_init(void) +{ + int rc; + + rc = register_netdevice_notifier(&rmnet_dev_notifier); + if (rc != 0) + return rc; + + rc = rtnl_link_register(&rmnet_link_ops); + if (rc != 0) { + unregister_netdevice_notifier(&rmnet_dev_notifier); + return rc; + } + + rmnet_core_genl_init(); + + try_module_get(THIS_MODULE); + return rc; +} + +static void __exit rmnet_exit(void) +{ + unregister_netdevice_notifier(&rmnet_dev_notifier); + rtnl_link_unregister(&rmnet_link_ops); + rmnet_core_genl_deinit(); + + module_put(THIS_MODULE); +} + +module_init(rmnet_init) +module_exit(rmnet_exit) +MODULE_LICENSE("GPL v2"); diff --git a/techpack/datarmnet/core/rmnet_config.h b/techpack/datarmnet/core/rmnet_config.h new file mode 100755 index 000000000000..4bb68eec3349 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_config.h @@ -0,0 +1,217 @@ +/* Copyright (c) 2013-2014, 2016-2021 The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Data configuration engine + * + */ + +#include +#include + +#ifndef _RMNET_CONFIG_H_ +#define _RMNET_CONFIG_H_ + +#define RMNET_MAX_LOGICAL_EP 255 +#define RMNET_MAX_VEID 4 + +#define RMNET_SHS_STMP_ALL BIT(0) +#define RMNET_SHS_NO_PSH BIT(1) +#define RMNET_SHS_NO_DLMKR BIT(2) + +struct rmnet_shs_clnt_s { + u16 config; + u16 map_mask; + u16 max_pkts; + union { + struct rmnet_port *port; + } info; +}; + +struct rmnet_endpoint { + u8 mux_id; + struct net_device *egress_dev; + struct hlist_node hlnode; +}; + +struct rmnet_agg_stats { + u64 ul_agg_reuse; + u64 ul_agg_alloc; +}; + +struct rmnet_port_priv_stats { + u64 dl_hdr_last_qmap_vers; + u64 dl_hdr_last_ep_id; + u64 dl_hdr_last_trans_id; + u64 dl_hdr_last_seq; + u64 dl_hdr_last_bytes; + u64 dl_hdr_last_pkts; + u64 dl_hdr_last_flows; + u64 dl_hdr_count; + u64 dl_hdr_total_bytes; + u64 dl_hdr_total_pkts; + u64 dl_trl_last_seq; + u64 dl_trl_count; + struct rmnet_agg_stats agg; +}; + +struct rmnet_egress_agg_params { + u16 agg_size; + u8 agg_count; + u8 agg_features; + u32 agg_time; +}; + +struct rmnet_agg_page { + struct list_head list; + struct page *page; +}; + + +/* One instance of this structure is instantiated for each real_dev associated + * with rmnet. + */ +struct rmnet_port { + struct net_device *dev; + u32 data_format; + u8 nr_rmnet_devs; + u8 rmnet_mode; + struct hlist_head muxed_ep[RMNET_MAX_LOGICAL_EP]; + struct net_device *bridge_ep; + void *rmnet_perf; + + struct rmnet_egress_agg_params egress_agg_params; + + /* Protect aggregation related elements */ + spinlock_t agg_lock; + + struct sk_buff *agg_skb; + int agg_state; + u8 agg_count; + struct timespec64 agg_time; + struct timespec64 agg_last; + struct hrtimer hrtimer; + struct work_struct agg_wq; + u8 agg_size_order; + struct list_head agg_list; + struct rmnet_agg_page *agg_head; + + void *qmi_info; + + /* dl marker elements */ + struct list_head dl_list; + struct rmnet_port_priv_stats stats; + int dl_marker_flush; + /* Port Config for shs */ + struct rmnet_shs_clnt_s shs_cfg; + struct rmnet_shs_clnt_s phy_shs_cfg; + + /* Descriptor pool */ + spinlock_t desc_pool_lock; + struct rmnet_frag_descriptor_pool *frag_desc_pool; +}; + +extern struct rtnl_link_ops rmnet_link_ops; + +struct rmnet_vnd_stats { + u64 rx_pkts; + u64 rx_bytes; + u64 tx_pkts; + u64 tx_bytes; + u32 tx_drops; +}; + +struct rmnet_pcpu_stats { + struct rmnet_vnd_stats stats; + struct u64_stats_sync syncp; +}; + +struct rmnet_coal_close_stats { + u64 non_coal; + u64 ip_miss; + u64 trans_miss; + u64 hw_nl; + u64 hw_pkt; + u64 hw_byte; + u64 hw_time; + u64 hw_evict; + u64 coal; +}; + +struct rmnet_coal_stats { + u64 coal_rx; + u64 coal_pkts; + u64 coal_hdr_nlo_err; + u64 coal_hdr_pkt_err; + u64 coal_csum_err; + u64 coal_reconstruct; + u64 coal_ip_invalid; + u64 coal_trans_invalid; + struct rmnet_coal_close_stats close; + u64 coal_veid[RMNET_MAX_VEID]; + u64 coal_tcp; + u64 coal_tcp_bytes; + u64 coal_udp; + u64 coal_udp_bytes; +}; + +struct rmnet_priv_stats { + u64 csum_ok; + u64 csum_valid_unset; + u64 csum_validation_failed; + u64 csum_err_bad_buffer; + u64 csum_err_invalid_ip_version; + u64 csum_err_invalid_transport; + u64 csum_fragmented_pkt; + u64 csum_skipped; + u64 csum_sw; + u64 csum_hw; + struct rmnet_coal_stats coal; + u64 ul_prio; +}; + +struct rmnet_priv { + u8 mux_id; + struct net_device *real_dev; + struct rmnet_pcpu_stats __percpu *pcpu_stats; + struct gro_cells gro_cells; + struct rmnet_priv_stats stats; + void __rcu *qos_info; +}; + +enum rmnet_dl_marker_prio { + RMNET_PERF, + RMNET_SHS, +}; + +enum rmnet_trace_func { + RMNET_MODULE, + NW_STACK_MODULE, +}; + +enum rmnet_trace_evt { + RMNET_DLVR_SKB, + RMNET_RCV_FROM_PND, + RMNET_TX_UL_PKT, + NW_STACK_DEV_Q_XMIT, + NW_STACK_NAPI_GRO_FLUSH, + NW_STACK_RX, + NW_STACK_TX, +}; + +int rmnet_is_real_dev_registered(const struct net_device *real_dev); +struct rmnet_port *rmnet_get_port(struct net_device *real_dev); +struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id); +int rmnet_add_bridge(struct net_device *rmnet_dev, + struct net_device *slave_dev, + struct netlink_ext_ack *extack); +int rmnet_del_bridge(struct net_device *rmnet_dev, + struct net_device *slave_dev); +#endif /* _RMNET_CONFIG_H_ */ diff --git a/techpack/datarmnet/core/rmnet_ctl.h b/techpack/datarmnet/core/rmnet_ctl.h new file mode 100644 index 000000000000..50096140d7fa --- /dev/null +++ b/techpack/datarmnet/core/rmnet_ctl.h @@ -0,0 +1,34 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* Copyright (c) 2019-2020, The Linux Foundation. All rights reserved. + * + * RMNET_CTL header + * + */ + +#ifndef _RMNET_CTL_H_ +#define _RMNET_CTL_H_ + +#include + +enum rmnet_ctl_log_lvl { + RMNET_CTL_LOG_CRIT, + RMNET_CTL_LOG_ERR, + RMNET_CTL_LOG_INFO, + RMNET_CTL_LOG_DEBUG, +}; + +struct rmnet_ctl_client_hooks { + void (*ctl_dl_client_hook)(struct sk_buff *skb); +}; + +struct rmnet_ctl_client_if { + void * (*reg)(struct rmnet_ctl_client_hooks *hook); + int (*dereg)(void *handle); + int (*send)(void *handle, struct sk_buff *skb); + void (*log)(enum rmnet_ctl_log_lvl lvl, const char *msg, int rc, + const void *data, unsigned int len); +}; + +struct rmnet_ctl_client_if *rmnet_ctl_if(void); + +#endif /* _RMNET_CTL_H_ */ diff --git a/techpack/datarmnet/core/rmnet_ctl_client.c b/techpack/datarmnet/core/rmnet_ctl_client.c new file mode 100644 index 000000000000..4958deac1c75 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_ctl_client.c @@ -0,0 +1,219 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* Copyright (c) 2019-2020, The Linux Foundation. All rights reserved. + * + * RMNET_CTL client handlers + * + */ + +#include +#include +#include "rmnet_ctl.h" +#include "rmnet_ctl_client.h" + +#define RMNET_CTL_LOG_PAGE 10 +#define RMNET_CTL_LOG_NAME "rmnet_ctl" +#define RMNET_CTL_LOG_LVL "ipc_log_lvl" + +struct rmnet_ctl_client { + struct rmnet_ctl_client_hooks hooks; +}; + +struct rmnet_ctl_endpoint { + struct rmnet_ctl_dev __rcu *dev; + struct rmnet_ctl_client __rcu *client; + struct dentry *dbgfs_dir; + struct dentry *dbgfs_loglvl; + void *ipc_log; +}; + +#if defined(CONFIG_IPA_DEBUG) || defined(CONFIG_MHI_DEBUG) +#define CONFIG_RMNET_CTL_DEBUG 1 +#endif + +#ifdef CONFIG_RMNET_CTL_DEBUG +static u8 ipc_log_lvl = RMNET_CTL_LOG_DEBUG; +#else +static u8 ipc_log_lvl = RMNET_CTL_LOG_ERR; +#endif + +static DEFINE_SPINLOCK(client_lock); +static struct rmnet_ctl_endpoint ctl_ep; + +void rmnet_ctl_set_dbgfs(bool enable) +{ + if (enable) { + if (IS_ERR_OR_NULL(ctl_ep.dbgfs_dir)) + ctl_ep.dbgfs_dir = debugfs_create_dir( + RMNET_CTL_LOG_NAME, NULL); + + if (!IS_ERR_OR_NULL(ctl_ep.dbgfs_dir)) + ctl_ep.dbgfs_loglvl = debugfs_create_u8( + RMNET_CTL_LOG_LVL, 0644, ctl_ep.dbgfs_dir, + &ipc_log_lvl); + + if (!ctl_ep.ipc_log) + ctl_ep.ipc_log = ipc_log_context_create( + RMNET_CTL_LOG_PAGE, RMNET_CTL_LOG_NAME, 0); + } else { + debugfs_remove_recursive(ctl_ep.dbgfs_dir); + ipc_log_context_destroy(ctl_ep.ipc_log); + ctl_ep.dbgfs_dir = NULL; + ctl_ep.dbgfs_loglvl = NULL; + ctl_ep.ipc_log = NULL; + } +} + +void rmnet_ctl_endpoint_setdev(const struct rmnet_ctl_dev *dev) +{ + rcu_assign_pointer(ctl_ep.dev, dev); +} + +void rmnet_ctl_endpoint_post(const void *data, size_t len) +{ + struct rmnet_ctl_client *client; + struct sk_buff *skb; + + if (unlikely(!data || !len)) + return; + + if (len == 0xFFFFFFFF) { + skb = (struct sk_buff *)data; + rmnet_ctl_log_info("RX", skb->data, skb->len); + + rcu_read_lock(); + + client = rcu_dereference(ctl_ep.client); + if (client && client->hooks.ctl_dl_client_hook) { + skb->protocol = htons(ETH_P_MAP); + client->hooks.ctl_dl_client_hook(skb); + } else { + kfree(skb); + } + + rcu_read_unlock(); + } else { + rmnet_ctl_log_info("RX", data, len); + + rcu_read_lock(); + + client = rcu_dereference(ctl_ep.client); + if (client && client->hooks.ctl_dl_client_hook) { + skb = alloc_skb(len, GFP_ATOMIC); + if (skb) { + skb_put_data(skb, data, len); + skb->protocol = htons(ETH_P_MAP); + client->hooks.ctl_dl_client_hook(skb); + } + } + + rcu_read_unlock(); + } +} + +void *rmnet_ctl_register_client(struct rmnet_ctl_client_hooks *hook) +{ + struct rmnet_ctl_client *client; + + if (!hook) + return NULL; + + client = kzalloc(sizeof(*client), GFP_KERNEL); + if (!client) + return NULL; + client->hooks = *hook; + + spin_lock(&client_lock); + + /* Only support one client for now */ + if (rcu_dereference(ctl_ep.client)) { + spin_unlock(&client_lock); + kfree(client); + return NULL; + } + + rcu_assign_pointer(ctl_ep.client, client); + + spin_unlock(&client_lock); + + rmnet_ctl_set_dbgfs(true); + + return client; +} +EXPORT_SYMBOL(rmnet_ctl_register_client); + +int rmnet_ctl_unregister_client(void *handle) +{ + struct rmnet_ctl_client *client = (struct rmnet_ctl_client *)handle; + + spin_lock(&client_lock); + + if (rcu_dereference(ctl_ep.client) != client) { + spin_unlock(&client_lock); + return -EINVAL; + } + + RCU_INIT_POINTER(ctl_ep.client, NULL); + + spin_unlock(&client_lock); + + synchronize_rcu(); + kfree(client); + + rmnet_ctl_set_dbgfs(false); + + return 0; +} +EXPORT_SYMBOL(rmnet_ctl_unregister_client); + +int rmnet_ctl_send_client(void *handle, struct sk_buff *skb) +{ + struct rmnet_ctl_client *client = (struct rmnet_ctl_client *)handle; + struct rmnet_ctl_dev *dev; + int rc = -EINVAL; + + if (client != rcu_dereference(ctl_ep.client)) + return rc; + + rmnet_ctl_log_info("TX", skb->data, skb->len); + + rcu_read_lock(); + + dev = rcu_dereference(ctl_ep.dev); + if (dev && dev->xmit) + rc = dev->xmit(dev, skb); + + rcu_read_unlock(); + + if (rc) + rmnet_ctl_log_err("TXE", rc, skb->data, skb->len); + + return rc; +} +EXPORT_SYMBOL(rmnet_ctl_send_client); + +void rmnet_ctl_log(enum rmnet_ctl_log_lvl lvl, const char *msg, + int rc, const void *data, unsigned int len) +{ + if (lvl <= ipc_log_lvl && ctl_ep.ipc_log) { + if (data == NULL || len == 0) + ipc_log_string(ctl_ep.ipc_log, "%3s(%d): (null)\n", + msg, rc); + else + ipc_log_string(ctl_ep.ipc_log, "%3s(%d): %*ph\n", + msg, rc, len > 32 ? 32 : len, data); + } +} +EXPORT_SYMBOL(rmnet_ctl_log); + +static struct rmnet_ctl_client_if client_if = { + .reg = rmnet_ctl_register_client, + .dereg = rmnet_ctl_unregister_client, + .send = rmnet_ctl_send_client, + .log = rmnet_ctl_log, +}; + +struct rmnet_ctl_client_if *rmnet_ctl_if(void) +{ + return &client_if; +} +EXPORT_SYMBOL(rmnet_ctl_if); diff --git a/techpack/datarmnet/core/rmnet_ctl_client.h b/techpack/datarmnet/core/rmnet_ctl_client.h new file mode 100644 index 000000000000..f044aa29e6d6 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_ctl_client.h @@ -0,0 +1,43 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* Copyright (c) 2019-2020, The Linux Foundation. All rights reserved. + * + * RMNET_CTL client handlers + * + */ + +#ifndef _RMNET_CTL_CLIENT_H_ +#define _RMNET_CTL_CLIENT_H_ + +#include +#include "rmnet_ctl.h" + +void rmnet_ctl_log(enum rmnet_ctl_log_lvl lvl, const char *msg, + int rc, const void *data, unsigned int len); + +#define rmnet_ctl_log_err(msg, rc, data, len) \ + rmnet_ctl_log(RMNET_CTL_LOG_ERR, msg, rc, data, len) + +#define rmnet_ctl_log_info(msg, data, len) \ + rmnet_ctl_log(RMNET_CTL_LOG_INFO, msg, 0, data, len) + +#define rmnet_ctl_log_debug(msg, data, len) \ + rmnet_ctl_log(RMNET_CTL_LOG_DEBUG, msg, 0, data, len) + +struct rmnet_ctl_stats { + u64 rx_pkts; + u64 rx_err; + u64 tx_pkts; + u64 tx_err; + u64 tx_complete; +}; + +struct rmnet_ctl_dev { + int (*xmit)(struct rmnet_ctl_dev *dev, struct sk_buff *skb); + struct rmnet_ctl_stats stats; +}; + +void rmnet_ctl_endpoint_post(const void *data, size_t len); +void rmnet_ctl_endpoint_setdev(const struct rmnet_ctl_dev *dev); +void rmnet_ctl_set_dbgfs(bool enable); + +#endif /* _RMNET_CTL_CLIENT_H_ */ diff --git a/techpack/datarmnet/core/rmnet_ctl_ipa.c b/techpack/datarmnet/core/rmnet_ctl_ipa.c new file mode 100644 index 000000000000..39eb76ad7781 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_ctl_ipa.c @@ -0,0 +1,113 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* Copyright (c) 2020, The Linux Foundation. All rights reserved. + * + * RMNET_CTL mhi handler + * + */ + +#include +#include +#include +#include "rmnet_ctl.h" +#include "rmnet_ctl_client.h" + +struct rmnet_ctl_ipa_dev { + struct rmnet_ctl_dev dev; + spinlock_t rx_lock; /* rx lock */ + spinlock_t tx_lock; /* tx lock */ +}; + +static struct rmnet_ctl_ipa_dev ctl_ipa_dev; +static bool rmnet_ctl_ipa_registered; + +static int rmnet_ctl_send_ipa(struct rmnet_ctl_dev *dev, struct sk_buff *skb) +{ + struct rmnet_ctl_ipa_dev *ctl_dev = container_of( + dev, struct rmnet_ctl_ipa_dev, dev); + int rc; + + spin_lock_bh(&ctl_dev->tx_lock); + + rc = ipa_rmnet_ctl_xmit(skb); + if (rc) + dev->stats.tx_err++; + else + dev->stats.tx_pkts++; + + spin_unlock_bh(&ctl_dev->tx_lock); + + return rc; +} + +static void rmnet_ctl_dl_callback(void *user_data, void *rx_data) +{ + struct rmnet_ctl_ipa_dev *ctl_dev = user_data; + + ctl_dev->dev.stats.rx_pkts++; + rmnet_ctl_endpoint_post(rx_data, 0xFFFFFFFF); +} + +static void rmnet_ctl_probe(void *user_data) +{ + memset(&ctl_ipa_dev, 0, sizeof(ctl_ipa_dev)); + + spin_lock_init(&ctl_ipa_dev.rx_lock); + spin_lock_init(&ctl_ipa_dev.tx_lock); + + ctl_ipa_dev.dev.xmit = rmnet_ctl_send_ipa; + rmnet_ctl_endpoint_setdev(&ctl_ipa_dev.dev); + + pr_info("rmnet_ctl driver probed\n"); +} + +static void rmnet_ctl_remove(void *user_data) +{ + rmnet_ctl_endpoint_setdev(NULL); + + pr_info("rmnet_ctl driver removed\n"); +} + +static void rmnet_ctl_ipa_ready(void *user_data) +{ + int rc; + + rc = ipa_register_rmnet_ctl_cb( + rmnet_ctl_probe, + &ctl_ipa_dev, + rmnet_ctl_remove, + &ctl_ipa_dev, + rmnet_ctl_dl_callback, + &ctl_ipa_dev); + + if (rc) + pr_err("%s: %d\n", __func__, rc); + else + rmnet_ctl_ipa_registered = true; +} + +static int __init rmnet_ctl_init(void) +{ + int rc; + + rc = ipa_register_ipa_ready_cb(rmnet_ctl_ipa_ready, NULL); + if (rc == -EEXIST) + rmnet_ctl_ipa_ready(NULL); + else if (rc) + pr_err("%s: %d\n", __func__, rc); + + return 0; +} + +static void __exit rmnet_ctl_exit(void) +{ + if (rmnet_ctl_ipa_registered) { + ipa_unregister_rmnet_ctl_cb(); + rmnet_ctl_ipa_registered = false; + } +} + +module_init(rmnet_ctl_init) +module_exit(rmnet_ctl_exit) + +MODULE_DESCRIPTION("RmNet control IPA Driver"); +MODULE_LICENSE("GPL v2"); diff --git a/techpack/datarmnet/core/rmnet_ctl_mhi.c b/techpack/datarmnet/core/rmnet_ctl_mhi.c new file mode 100644 index 000000000000..f9cc2ba2943c --- /dev/null +++ b/techpack/datarmnet/core/rmnet_ctl_mhi.c @@ -0,0 +1,222 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* Copyright (c) 2019-2020, The Linux Foundation. All rights reserved. + * + * RMNET_CTL mhi handler + * + */ + +#include +#include +#include +#include +#include +#include "rmnet_ctl.h" +#include "rmnet_ctl_client.h" + +#define RMNET_CTL_DEFAULT_MRU 256 + +struct rmnet_ctl_mhi_dev { + struct mhi_device *mhi_dev; + struct rmnet_ctl_dev dev; + u32 mru; + spinlock_t rx_lock; /* rx lock */ + spinlock_t tx_lock; /* tx lock */ + atomic_t in_reset; +}; + +static int rmnet_ctl_send_mhi(struct rmnet_ctl_dev *dev, struct sk_buff *skb) +{ + struct rmnet_ctl_mhi_dev *ctl_dev = container_of( + dev, struct rmnet_ctl_mhi_dev, dev); + int rc; + + spin_lock_bh(&ctl_dev->tx_lock); + + rc = mhi_queue_transfer(ctl_dev->mhi_dev, + DMA_TO_DEVICE, skb, skb->len, MHI_EOT); + if (rc) + dev->stats.tx_err++; + else + dev->stats.tx_pkts++; + + spin_unlock_bh(&ctl_dev->tx_lock); + + return rc; +} + +static void rmnet_ctl_alloc_buffers(struct rmnet_ctl_mhi_dev *ctl_dev, + gfp_t gfp, void *free_buf) +{ + struct mhi_device *mhi_dev = ctl_dev->mhi_dev; + void *buf; + int no_tre, i, rc; + + no_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE); + + if (!no_tre && free_buf) { + kfree(free_buf); + return; + } + + for (i = 0; i < no_tre; i++) { + if (free_buf) { + buf = free_buf; + free_buf = NULL; + } else { + buf = kmalloc(ctl_dev->mru, gfp); + } + + if (!buf) + return; + + spin_lock_bh(&ctl_dev->rx_lock); + rc = mhi_queue_transfer(mhi_dev, DMA_FROM_DEVICE, + buf, ctl_dev->mru, MHI_EOT); + spin_unlock_bh(&ctl_dev->rx_lock); + + if (rc) { + kfree(buf); + return; + } + } +} + +static void rmnet_ctl_dl_callback(struct mhi_device *mhi_dev, + struct mhi_result *mhi_res) +{ + struct rmnet_ctl_mhi_dev *ctl_dev = dev_get_drvdata(&mhi_dev->dev); + + if (mhi_res->transaction_status == -ENOTCONN) { + kfree(mhi_res->buf_addr); + return; + } else if (mhi_res->transaction_status || + !mhi_res->buf_addr || !mhi_res->bytes_xferd) { + rmnet_ctl_log_err("RXE", mhi_res->transaction_status, NULL, 0); + ctl_dev->dev.stats.rx_err++; + } else { + ctl_dev->dev.stats.rx_pkts++; + rmnet_ctl_endpoint_post(mhi_res->buf_addr, + mhi_res->bytes_xferd); + } + + /* Re-supply receive buffers */ + rmnet_ctl_alloc_buffers(ctl_dev, GFP_ATOMIC, mhi_res->buf_addr); +} + +static void rmnet_ctl_ul_callback(struct mhi_device *mhi_dev, + struct mhi_result *mhi_res) +{ + struct rmnet_ctl_mhi_dev *ctl_dev = dev_get_drvdata(&mhi_dev->dev); + struct sk_buff *skb = (struct sk_buff *)mhi_res->buf_addr; + + if (skb) { + if (mhi_res->transaction_status) { + rmnet_ctl_log_err("TXE", mhi_res->transaction_status, + skb->data, skb->len); + ctl_dev->dev.stats.tx_err++; + } else { + rmnet_ctl_log_debug("TXC", skb->data, skb->len); + ctl_dev->dev.stats.tx_complete++; + } + kfree_skb(skb); + } +} + +static void rmnet_ctl_status_callback(struct mhi_device *mhi_dev, + enum MHI_CB mhi_cb) +{ + struct rmnet_ctl_mhi_dev *ctl_dev = dev_get_drvdata(&mhi_dev->dev); + + if (mhi_cb != MHI_CB_FATAL_ERROR) + return; + + atomic_inc(&ctl_dev->in_reset); +} + +static int rmnet_ctl_probe(struct mhi_device *mhi_dev, + const struct mhi_device_id *id) +{ + struct rmnet_ctl_mhi_dev *ctl_dev; + struct device_node *of_node = mhi_dev->dev.of_node; + int rc; + + ctl_dev = devm_kzalloc(&mhi_dev->dev, sizeof(*ctl_dev), GFP_KERNEL); + if (!ctl_dev) + return -ENOMEM; + + ctl_dev->mhi_dev = mhi_dev; + ctl_dev->dev.xmit = rmnet_ctl_send_mhi; + + spin_lock_init(&ctl_dev->rx_lock); + spin_lock_init(&ctl_dev->tx_lock); + atomic_set(&ctl_dev->in_reset, 0); + dev_set_drvdata(&mhi_dev->dev, ctl_dev); + + rc = of_property_read_u32(of_node, "mhi,mru", &ctl_dev->mru); + if (rc || !ctl_dev->mru) + ctl_dev->mru = RMNET_CTL_DEFAULT_MRU; + + rc = mhi_prepare_for_transfer(mhi_dev); + if (rc) { + pr_err("%s(): Failed to prep for transfer %d\n", __func__, rc); + return -EINVAL; + } + + /* Post receive buffers */ + rmnet_ctl_alloc_buffers(ctl_dev, GFP_KERNEL, NULL); + + rmnet_ctl_endpoint_setdev(&ctl_dev->dev); + + pr_info("rmnet_ctl driver probed\n"); + + return 0; +} + +static void rmnet_ctl_remove(struct mhi_device *mhi_dev) +{ + rmnet_ctl_endpoint_setdev(NULL); + synchronize_rcu(); + dev_set_drvdata(&mhi_dev->dev, NULL); + + pr_info("rmnet_ctl driver removed\n"); +} + +static const struct mhi_device_id rmnet_ctl_mhi_match[] = { + { .chan = "RMNET_CTL" }, + {} +}; + +static struct mhi_driver rmnet_ctl_driver = { + .probe = rmnet_ctl_probe, + .remove = rmnet_ctl_remove, + .dl_xfer_cb = rmnet_ctl_dl_callback, + .ul_xfer_cb = rmnet_ctl_ul_callback, + .status_cb = rmnet_ctl_status_callback, + .id_table = rmnet_ctl_mhi_match, + .driver = { + .name = "rmnet_ctl", + .owner = THIS_MODULE, + }, +}; + +static int __init rmnet_ctl_init(void) +{ + int rc; + + rc = mhi_driver_register(&rmnet_ctl_driver); + rmnet_ctl_set_dbgfs(true); + + return rc; +} + +static void __exit rmnet_ctl_exit(void) +{ + mhi_driver_unregister(&rmnet_ctl_driver); + rmnet_ctl_set_dbgfs(false); +} + +module_init(rmnet_ctl_init) +module_exit(rmnet_ctl_exit) + +MODULE_DESCRIPTION("RmNet Control MHI Driver"); +MODULE_LICENSE("GPL v2"); diff --git a/techpack/datarmnet/core/rmnet_descriptor.c b/techpack/datarmnet/core/rmnet_descriptor.c new file mode 100644 index 000000000000..f7818d90a20c --- /dev/null +++ b/techpack/datarmnet/core/rmnet_descriptor.c @@ -0,0 +1,1642 @@ +/* Copyright (c) 2013-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Packet Descriptor Framework + * + */ + +#include +#include +#include +#include +#include "rmnet_config.h" +#include "rmnet_descriptor.h" +#include "rmnet_handlers.h" +#include "rmnet_private.h" +#include "rmnet_vnd.h" +#include "rmnet_qmi.h" +#include "qmi_rmnet.h" + +#define RMNET_FRAG_DESCRIPTOR_POOL_SIZE 64 +#define RMNET_DL_IND_HDR_SIZE (sizeof(struct rmnet_map_dl_ind_hdr) + \ + sizeof(struct rmnet_map_header) + \ + sizeof(struct rmnet_map_control_command_header)) +#define RMNET_DL_IND_TRL_SIZE (sizeof(struct rmnet_map_dl_ind_trl) + \ + sizeof(struct rmnet_map_header) + \ + sizeof(struct rmnet_map_control_command_header)) + +typedef void (*rmnet_perf_desc_hook_t)(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port); +typedef void (*rmnet_perf_chain_hook_t)(void); + +struct rmnet_frag_descriptor * +rmnet_get_frag_descriptor(struct rmnet_port *port) +{ + struct rmnet_frag_descriptor_pool *pool = port->frag_desc_pool; + struct rmnet_frag_descriptor *frag_desc; + unsigned long flags; + + spin_lock_irqsave(&port->desc_pool_lock, flags); + if (!list_empty(&pool->free_list)) { + frag_desc = list_first_entry(&pool->free_list, + struct rmnet_frag_descriptor, + list); + list_del_init(&frag_desc->list); + } else { + frag_desc = kzalloc(sizeof(*frag_desc), GFP_ATOMIC); + if (!frag_desc) + goto out; + + INIT_LIST_HEAD(&frag_desc->list); + INIT_LIST_HEAD(&frag_desc->frags); + pool->pool_size++; + } + +out: + spin_unlock_irqrestore(&port->desc_pool_lock, flags); + return frag_desc; +} +EXPORT_SYMBOL(rmnet_get_frag_descriptor); + +void rmnet_recycle_frag_descriptor(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port) +{ + struct rmnet_frag_descriptor_pool *pool = port->frag_desc_pool; + struct rmnet_fragment *frag, *tmp; + unsigned long flags; + + list_del(&frag_desc->list); + + list_for_each_entry_safe(frag, tmp, &frag_desc->frags, list) { + struct page *page = skb_frag_page(&frag->frag); + + if (page) + put_page(page); + + list_del(&frag->list); + kfree(frag); + } + + memset(frag_desc, 0, sizeof(*frag_desc)); + INIT_LIST_HEAD(&frag_desc->list); + INIT_LIST_HEAD(&frag_desc->frags); + spin_lock_irqsave(&port->desc_pool_lock, flags); + list_add_tail(&frag_desc->list, &pool->free_list); + spin_unlock_irqrestore(&port->desc_pool_lock, flags); +} +EXPORT_SYMBOL(rmnet_recycle_frag_descriptor); + +void *rmnet_frag_pull(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, unsigned int size) +{ + struct rmnet_fragment *frag, *tmp; + + if (size >= frag_desc->len) { + pr_info("%s(): Pulling %u bytes from %u byte pkt. Dropping\n", + __func__, size, frag_desc->len); + rmnet_recycle_frag_descriptor(frag_desc, port); + return NULL; + } + + list_for_each_entry_safe(frag, tmp, &frag_desc->frags, list) { + u32 frag_size = skb_frag_size(&frag->frag); + + if (!size) + break; + + if (size >= frag_size) { + /* Remove the whole frag */ + struct page *page = skb_frag_page(&frag->frag); + + if (page) + put_page(page); + + list_del(&frag->list); + size -= frag_size; + frag_desc->len -= frag_size; + kfree(frag); + continue; + } + + /* Pull off 'size' bytes */ + skb_frag_off_add(&frag->frag, size); + skb_frag_size_sub(&frag->frag, size); + frag_desc->len -= size; + break; + } + + return rmnet_frag_data_ptr(frag_desc); +} +EXPORT_SYMBOL(rmnet_frag_pull); + +void *rmnet_frag_trim(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, unsigned int size) +{ + struct rmnet_fragment *frag, *tmp; + unsigned int eat; + + if (!size) { + pr_info("%s(): Trimming %u byte pkt to 0. Dropping\n", + __func__, frag_desc->len); + rmnet_recycle_frag_descriptor(frag_desc, port); + return NULL; + } + + /* Growing bigger doesn't make sense */ + if (size >= frag_desc->len) + goto out; + + /* Compute number of bytes to remove from the end */ + eat = frag_desc->len - size; + list_for_each_entry_safe_reverse(frag, tmp, &frag_desc->frags, list) { + u32 frag_size = skb_frag_size(&frag->frag); + + if (!eat) + goto out; + + if (eat >= frag_size) { + /* Remove the whole frag */ + struct page *page = skb_frag_page(&frag->frag); + + if (page) + put_page(page); + + list_del(&frag->list); + eat -= frag_size; + frag_desc->len -= frag_size; + kfree(frag); + continue; + } + + /* Chop off 'eat' bytes from the end */ + skb_frag_size_sub(&frag->frag, eat); + frag_desc->len -= eat; + goto out; + } + +out: + return rmnet_frag_data_ptr(frag_desc); +} +EXPORT_SYMBOL(rmnet_frag_trim); + +static int rmnet_frag_copy_data(struct rmnet_frag_descriptor *frag_desc, + u32 off, u32 len, void *buf) +{ + struct rmnet_fragment *frag; + u32 frag_size, copy_len; + u32 buf_offset = 0; + + /* Don't make me do something we'd both regret */ + if (off > frag_desc->len || len > frag_desc->len || + off + len > frag_desc->len) + return -EINVAL; + + /* Copy 'len' bytes into the bufer starting from 'off' */ + list_for_each_entry(frag, &frag_desc->frags, list) { + if (!len) + break; + + frag_size = skb_frag_size(&frag->frag); + if (off < frag_size) { + copy_len = min_t(u32, len, frag_size - off); + memcpy(buf + buf_offset, + skb_frag_address(&frag->frag) + off, + copy_len); + buf_offset += copy_len; + len -= copy_len; + off = 0; + } else { + off -= frag_size; + } + } + + return 0; +} + +void *rmnet_frag_header_ptr(struct rmnet_frag_descriptor *frag_desc, u32 off, + u32 len, void *buf) +{ + struct rmnet_fragment *frag; + u8 *start; + u32 frag_size, offset; + + /* Don't take a long pointer off a short frag */ + if (off > frag_desc->len || len > frag_desc->len || + off + len > frag_desc->len) + return NULL; + + /* Find the starting fragment */ + offset = off; + list_for_each_entry(frag, &frag_desc->frags, list) { + frag_size = skb_frag_size(&frag->frag); + if (off < frag_size) { + start = skb_frag_address(&frag->frag) + off; + /* If the header is entirely on this frag, just return + * a pointer to it. + */ + if (off + len <= frag_size) + return start; + + /* Otherwise, we need to copy the data into a linear + * buffer. + */ + break; + } + + off -= frag_size; + } + + if (rmnet_frag_copy_data(frag_desc, offset, len, buf) < 0) + return NULL; + + return buf; +} +EXPORT_SYMBOL(rmnet_frag_header_ptr); + +int rmnet_frag_descriptor_add_frag(struct rmnet_frag_descriptor *frag_desc, + struct page *p, u32 page_offset, u32 len) +{ + struct rmnet_fragment *frag; + + frag = kzalloc(sizeof(*frag), GFP_ATOMIC); + if (!frag) + return -ENOMEM; + + INIT_LIST_HEAD(&frag->list); + get_page(p); + __skb_frag_set_page(&frag->frag, p); + skb_frag_size_set(&frag->frag, len); + skb_frag_off_set(&frag->frag, page_offset); + list_add_tail(&frag->list, &frag_desc->frags); + frag_desc->len += len; + return 0; +} +EXPORT_SYMBOL(rmnet_frag_descriptor_add_frag); + +int rmnet_frag_descriptor_add_frags_from(struct rmnet_frag_descriptor *to, + struct rmnet_frag_descriptor *from, + u32 off, u32 len) +{ + struct rmnet_fragment *frag; + int rc; + + /* Sanity check the lengths */ + if (off > from->len || len > from->len || off + len > from->len) + return -EINVAL; + + list_for_each_entry(frag, &from->frags, list) { + u32 frag_size; + + if (!len) + break; + + frag_size = skb_frag_size(&frag->frag); + if (off < frag_size) { + struct page *p = skb_frag_page(&frag->frag); + u32 page_off = skb_frag_off(&frag->frag); + u32 copy_len = min_t(u32, len, frag_size - off); + + rc = rmnet_frag_descriptor_add_frag(to, p, + page_off + off, + copy_len); + if (rc < 0) + return rc; + + len -= copy_len; + off = 0; + } else { + off -= frag_size; + } + } + + return 0; +} +EXPORT_SYMBOL(rmnet_frag_descriptor_add_frags_from); + +int rmnet_frag_ipv6_skip_exthdr(struct rmnet_frag_descriptor *frag_desc, + int start, u8 *nexthdrp, __be16 *fragp) +{ + u8 nexthdr = *nexthdrp; + + *fragp = 0; + + while (ipv6_ext_hdr(nexthdr)) { + struct ipv6_opt_hdr *hp, __hp; + int hdrlen; + + if (nexthdr == NEXTHDR_NONE) + return -EINVAL; + + hp = rmnet_frag_header_ptr(frag_desc, (u32)start, sizeof(*hp), + &__hp); + if (!hp) + return -EINVAL; + + if (nexthdr == NEXTHDR_FRAGMENT) { + u32 off = offsetof(struct frag_hdr, frag_off); + __be16 *fp, __fp; + + fp = rmnet_frag_header_ptr(frag_desc, (u32)start + off, + sizeof(*fp), &__fp); + if (!fp) + return -EINVAL; + + *fragp = *fp; + if (ntohs(*fragp) & ~0x7) + break; + hdrlen = 8; + } else if (nexthdr == NEXTHDR_AUTH) { + hdrlen = (hp->hdrlen + 2) << 2; + } else { + hdrlen = ipv6_optlen(hp); + } + + nexthdr = hp->nexthdr; + start += hdrlen; + } + + *nexthdrp = nexthdr; + return start; +} +EXPORT_SYMBOL(rmnet_frag_ipv6_skip_exthdr); + +static u8 rmnet_frag_do_flow_control(struct rmnet_map_header *qmap, + struct rmnet_map_control_command *cmd, + struct rmnet_port *port, + int enable) +{ + struct rmnet_endpoint *ep; + struct net_device *vnd; + u16 ip_family; + u16 fc_seq; + u32 qos_id; + u8 mux_id; + int r; + + mux_id = qmap->mux_id; + if (mux_id >= RMNET_MAX_LOGICAL_EP) + return RX_HANDLER_CONSUMED; + + ep = rmnet_get_endpoint(port, mux_id); + if (!ep) + return RX_HANDLER_CONSUMED; + + vnd = ep->egress_dev; + + ip_family = cmd->flow_control.ip_family; + fc_seq = ntohs(cmd->flow_control.flow_control_seq_num); + qos_id = ntohl(cmd->flow_control.qos_id); + + /* Ignore the ip family and pass the sequence number for both v4 and v6 + * sequence. User space does not support creating dedicated flows for + * the 2 protocols + */ + r = rmnet_vnd_do_flow_control(vnd, enable); + if (r) + return RMNET_MAP_COMMAND_UNSUPPORTED; + else + return RMNET_MAP_COMMAND_ACK; +} + +static void rmnet_frag_send_ack(struct rmnet_map_header *qmap, + unsigned char type, + struct rmnet_port *port) +{ + struct rmnet_map_control_command *cmd; + struct net_device *dev = port->dev; + struct sk_buff *skb; + u16 alloc_len = ntohs(qmap->pkt_len) + sizeof(*qmap); + + skb = alloc_skb(alloc_len, GFP_ATOMIC); + if (!skb) + return; + + skb->protocol = htons(ETH_P_MAP); + skb->dev = dev; + + cmd = rmnet_map_get_cmd_start(skb); + cmd->cmd_type = type & 0x03; + + netif_tx_lock(dev); + dev->netdev_ops->ndo_start_xmit(skb, dev); + netif_tx_unlock(dev); +} + +static void +rmnet_frag_process_flow_start(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_map_control_command_header *cmd, + struct rmnet_port *port, + u16 cmd_len) +{ + struct rmnet_map_dl_ind_hdr *dlhdr, __dlhdr; + u32 offset = sizeof(struct rmnet_map_header); + u32 data_format; + bool is_dl_mark_v2; + + if (cmd_len + offset < RMNET_DL_IND_HDR_SIZE) + return; + + data_format = port->data_format; + is_dl_mark_v2 = data_format & RMNET_INGRESS_FORMAT_DL_MARKER_V2; + dlhdr = rmnet_frag_header_ptr(frag_desc, offset + sizeof(*cmd), + sizeof(*dlhdr), &__dlhdr); + if (!dlhdr) + return; + + port->stats.dl_hdr_last_ep_id = cmd->source_id; + port->stats.dl_hdr_last_qmap_vers = cmd->reserved; + port->stats.dl_hdr_last_trans_id = cmd->transaction_id; + port->stats.dl_hdr_last_seq = dlhdr->le.seq; + port->stats.dl_hdr_last_bytes = dlhdr->le.bytes; + port->stats.dl_hdr_last_pkts = dlhdr->le.pkts; + port->stats.dl_hdr_last_flows = dlhdr->le.flows; + port->stats.dl_hdr_total_bytes += port->stats.dl_hdr_last_bytes; + port->stats.dl_hdr_total_pkts += port->stats.dl_hdr_last_pkts; + port->stats.dl_hdr_count++; + + /* If a target is taking frag path, we can assume DL marker v2 is in + * play + */ + if (is_dl_mark_v2) + rmnet_map_dl_hdr_notify_v2(port, dlhdr, cmd); +} + +static void +rmnet_frag_process_flow_end(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_map_control_command_header *cmd, + struct rmnet_port *port, u16 cmd_len) +{ + struct rmnet_map_dl_ind_trl *dltrl, __dltrl; + u32 offset = sizeof(struct rmnet_map_header); + u32 data_format; + bool is_dl_mark_v2; + + + if (cmd_len + offset < RMNET_DL_IND_TRL_SIZE) + return; + + data_format = port->data_format; + is_dl_mark_v2 = data_format & RMNET_INGRESS_FORMAT_DL_MARKER_V2; + dltrl = rmnet_frag_header_ptr(frag_desc, offset + sizeof(*cmd), + sizeof(*dltrl), &__dltrl); + if (!dltrl) + return; + + port->stats.dl_trl_last_seq = dltrl->seq_le; + port->stats.dl_trl_count++; + + /* If a target is taking frag path, we can assume DL marker v2 is in + * play + */ + if (is_dl_mark_v2) + rmnet_map_dl_trl_notify_v2(port, dltrl, cmd); +} + +/* Process MAP command frame and send N/ACK message as appropriate. Message cmd + * name is decoded here and appropriate handler is called. + */ +void rmnet_frag_command(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_map_header *qmap, struct rmnet_port *port) +{ + struct rmnet_map_control_command *cmd, __cmd; + unsigned char rc = 0; + + cmd = rmnet_frag_header_ptr(frag_desc, sizeof(*qmap), sizeof(*cmd), + &__cmd); + if (!cmd) + return; + + switch (cmd->command_name) { + case RMNET_MAP_COMMAND_FLOW_ENABLE: + rc = rmnet_frag_do_flow_control(qmap, cmd, port, 1); + break; + + case RMNET_MAP_COMMAND_FLOW_DISABLE: + rc = rmnet_frag_do_flow_control(qmap, cmd, port, 0); + break; + + default: + rc = RMNET_MAP_COMMAND_UNSUPPORTED; + break; + } + if (rc == RMNET_MAP_COMMAND_ACK) + rmnet_frag_send_ack(qmap, rc, port); +} + +int rmnet_frag_flow_command(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, u16 pkt_len) +{ + struct rmnet_map_control_command_header *cmd, __cmd; + + cmd = rmnet_frag_header_ptr(frag_desc, sizeof(struct rmnet_map_header), + sizeof(*cmd), &__cmd); + if (!cmd) + return -1; + + switch (cmd->command_name) { + case RMNET_MAP_COMMAND_FLOW_START: + rmnet_frag_process_flow_start(frag_desc, cmd, port, pkt_len); + break; + + case RMNET_MAP_COMMAND_FLOW_END: + rmnet_frag_process_flow_end(frag_desc, cmd, port, pkt_len); + break; + + default: + return 1; + } + + return 0; +} +EXPORT_SYMBOL(rmnet_frag_flow_command); + +static int rmnet_frag_deaggregate_one(struct sk_buff *skb, + struct rmnet_port *port, + struct list_head *list, + u32 start_frag) +{ + struct skb_shared_info *shinfo = skb_shinfo(skb); + struct rmnet_frag_descriptor *frag_desc; + struct rmnet_map_header *maph, __maph; + skb_frag_t *frag; + u32 i; + u32 pkt_len; + int rc; + + frag = &shinfo->frags[start_frag]; + /* Grab the QMAP header. Careful, as there's no guarantee that it's + * continugous! + */ + if (likely(skb_frag_size(frag) >= sizeof(*maph))) { + maph = skb_frag_address(frag); + } else { + /* The header's split across pages. We can rebuild it. + * Probably not faster or stronger than before. But certainly + * more linear. + */ + if (skb_copy_bits(skb, 0, &__maph, sizeof(__maph)) < 0) + return -1; + + maph = &__maph; + } + + pkt_len = ntohs(maph->pkt_len); + /* Catch empty frames */ + if (!pkt_len) + return -1; + + frag_desc = rmnet_get_frag_descriptor(port); + if (!frag_desc) + return -1; + + pkt_len += sizeof(*maph); + if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) { + pkt_len += sizeof(struct rmnet_map_dl_csum_trailer); + } else if ((port->data_format & (RMNET_FLAGS_INGRESS_MAP_CKSUMV5 | + RMNET_FLAGS_INGRESS_COALESCE)) && + !maph->cd_bit) { + u32 hsize = 0; + u8 type; + + /* Check the type. This seems like should be overkill for less + * than a single byte, doesn't it? + */ + if (likely(skb_frag_size(frag) >= sizeof(*maph) + 1)) { + type = *((u8 *)maph + sizeof(*maph)); + } else { + if (skb_copy_bits(skb, sizeof(*maph), &type, + sizeof(type)) < 0) + return -1; + } + + /* Type only uses the first 7 bits */ + switch ((type & 0xFE) >> 1) { + case RMNET_MAP_HEADER_TYPE_COALESCING: + hsize = sizeof(struct rmnet_map_v5_coal_header); + break; + case RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD: + hsize = sizeof(struct rmnet_map_v5_csum_header); + break; + } + + pkt_len += hsize; + } + + /* Add all frags containing the packet data to the descriptor */ + for (i = start_frag; pkt_len > 0 && i < shinfo->nr_frags; ) { + u32 frag_size; + u32 copy_len; + + frag = &shinfo->frags[i]; + frag_size = skb_frag_size(frag); + copy_len = min_t(u32, frag_size, pkt_len); + rc = rmnet_frag_descriptor_add_frag(frag_desc, + skb_frag_page(frag), + skb_frag_off(frag), + copy_len); + if (rc < 0) { + rmnet_recycle_frag_descriptor(frag_desc, port); + return -1; + } + + pkt_len -= copy_len; + skb_frag_off_add(frag, copy_len); + skb_frag_size_sub(frag, copy_len); + /* If the fragment is exhausted, we can move to the next one */ + if (!skb_frag_size(frag)) + i++; + } + + if (pkt_len) { + /* Packet length is larger than the amount of data we have */ + rmnet_recycle_frag_descriptor(frag_desc, port); + return -1; + } + + list_add_tail(&frag_desc->list, list); + return (int)(i - start_frag); +} + +void rmnet_frag_deaggregate(struct sk_buff *skb, struct rmnet_port *port, + struct list_head *list) +{ + struct skb_shared_info *shinfo = skb_shinfo(skb); + u32 i = 0; + int rc; + + while (i < shinfo->nr_frags) { + rc = rmnet_frag_deaggregate_one(skb, port, list, i); + if (rc < 0) + return; + + i += (u32)rc; + } +} + +/* Fill in GSO metadata to allow the SKB to be segmented by the NW stack + * if needed (i.e. forwarding, UDP GRO) + */ +static void rmnet_frag_gso_stamp(struct sk_buff *skb, + struct rmnet_frag_descriptor *frag_desc) +{ + struct skb_shared_info *shinfo = skb_shinfo(skb); + + if (frag_desc->trans_proto == IPPROTO_TCP) + shinfo->gso_type = (frag_desc->ip_proto == 4) ? + SKB_GSO_TCPV4 : SKB_GSO_TCPV6; + else + shinfo->gso_type = SKB_GSO_UDP_L4; + + shinfo->gso_size = frag_desc->gso_size; + shinfo->gso_segs = frag_desc->gso_segs; +} + +/* Set the partial checksum information. Sets the transport checksum to the + * pseudoheader checksum and sets the offload metadata. + */ +static void rmnet_frag_partial_csum(struct sk_buff *skb, + struct rmnet_frag_descriptor *frag_desc) +{ + struct iphdr *iph = (struct iphdr *)skb->data; + __sum16 pseudo; + u16 pkt_len = skb->len - frag_desc->ip_len; + + if (frag_desc->ip_proto == 4) { + iph->tot_len = htons(skb->len); + iph->check = 0; + iph->check = ip_fast_csum(iph, iph->ihl); + pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr, + pkt_len, frag_desc->trans_proto, + 0); + } else { + struct ipv6hdr *ip6h = (struct ipv6hdr *)iph; + + /* Payload length includes any extension headers */ + ip6h->payload_len = htons(skb->len - sizeof(*ip6h)); + pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + pkt_len, frag_desc->trans_proto, 0); + } + + if (frag_desc->trans_proto == IPPROTO_TCP) { + struct tcphdr *tp = (struct tcphdr *) + ((u8 *)iph + frag_desc->ip_len); + + tp->check = pseudo; + skb->csum_offset = offsetof(struct tcphdr, check); + } else { + struct udphdr *up = (struct udphdr *) + ((u8 *)iph + frag_desc->ip_len); + + up->len = htons(pkt_len); + up->check = pseudo; + skb->csum_offset = offsetof(struct udphdr, check); + } + + skb->ip_summed = CHECKSUM_PARTIAL; + skb->csum_start = (u8 *)iph + frag_desc->ip_len - skb->head; +} + +/* Allocate and populate an skb to contain the packet represented by the + * frag descriptor. + */ +static struct sk_buff *rmnet_alloc_skb(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port) +{ + struct sk_buff *head_skb, *current_skb, *skb; + struct skb_shared_info *shinfo; + struct rmnet_fragment *frag, *tmp; + + /* Use the exact sizes if we know them (i.e. RSB/RSC, rmnet_perf) */ + if (frag_desc->hdrs_valid) { + u16 hdr_len = frag_desc->ip_len + frag_desc->trans_len; + + head_skb = alloc_skb(hdr_len + RMNET_MAP_DEAGGR_HEADROOM, + GFP_ATOMIC); + if (!head_skb) + return NULL; + + skb_reserve(head_skb, RMNET_MAP_DEAGGR_HEADROOM); + rmnet_frag_copy_data(frag_desc, 0, hdr_len, + skb_put(head_skb, hdr_len)); + skb_reset_network_header(head_skb); + if (frag_desc->trans_len) + skb_set_transport_header(head_skb, frag_desc->ip_len); + + /* Pull the headers off carefully */ + if (hdr_len == frag_desc->len) + /* Fast forward "header only" packets */ + goto skip_frags; + + if (!rmnet_frag_pull(frag_desc, port, hdr_len)) { + kfree(head_skb); + return NULL; + } + } else { + /* Allocate enough space to avoid penalties in the stack + * from __pskb_pull_tail() + */ + head_skb = alloc_skb(256 + RMNET_MAP_DEAGGR_HEADROOM, + GFP_ATOMIC); + if (!head_skb) + return NULL; + + skb_reserve(head_skb, RMNET_MAP_DEAGGR_HEADROOM); + } + + shinfo = skb_shinfo(head_skb); + current_skb = head_skb; + + /* Add in the page fragments */ + list_for_each_entry_safe(frag, tmp, &frag_desc->frags, list) { + struct page *p = skb_frag_page(&frag->frag); + u32 frag_size = skb_frag_size(&frag->frag); + +add_frag: + if (shinfo->nr_frags < MAX_SKB_FRAGS) { + get_page(p); + skb_add_rx_frag(current_skb, shinfo->nr_frags, p, + skb_frag_off(&frag->frag), frag_size, + frag_size); + if (current_skb != head_skb) { + head_skb->len += frag_size; + head_skb->data_len += frag_size; + } + } else { + /* Alloc a new skb and try again */ + skb = alloc_skb(0, GFP_ATOMIC); + if (!skb) + break; + + if (current_skb == head_skb) + shinfo->frag_list = skb; + else + current_skb->next = skb; + + current_skb = skb; + shinfo = skb_shinfo(current_skb); + goto add_frag; + } + } + +skip_frags: + head_skb->dev = frag_desc->dev; + rmnet_set_skb_proto(head_skb); + + /* Handle any header metadata that needs to be updated after RSB/RSC + * segmentation + */ + if (frag_desc->ip_id_set) { + struct iphdr *iph; + + iph = (struct iphdr *)rmnet_map_data_ptr(head_skb); + csum_replace2(&iph->check, iph->id, frag_desc->ip_id); + iph->id = frag_desc->ip_id; + } + + if (frag_desc->tcp_seq_set) { + struct tcphdr *th; + + th = (struct tcphdr *) + (rmnet_map_data_ptr(head_skb) + frag_desc->ip_len); + th->seq = frag_desc->tcp_seq; + } + + if (frag_desc->tcp_flags_set) { + struct tcphdr *th; + __be16 *flags; + + th = (struct tcphdr *) + (rmnet_map_data_ptr(head_skb) + frag_desc->ip_len); + flags = (__be16 *)&tcp_flag_word(th); + *flags = frag_desc->tcp_flags; + } + + /* Handle csum offloading */ + if (frag_desc->csum_valid && frag_desc->hdrs_valid) { + /* Set the partial checksum information */ + rmnet_frag_partial_csum(head_skb, frag_desc); + } else if (frag_desc->csum_valid) { + /* Non-RSB/RSC/perf packet. The current checksum is fine */ + head_skb->ip_summed = CHECKSUM_UNNECESSARY; + } else if (frag_desc->hdrs_valid && + (frag_desc->trans_proto == IPPROTO_TCP || + frag_desc->trans_proto == IPPROTO_UDP)) { + /* Unfortunately, we have to fake a bad checksum here, since + * the original bad value is lost by the hardware. The only + * reliable way to do it is to calculate the actual checksum + * and corrupt it. + */ + __sum16 *check; + __wsum csum; + unsigned int offset = skb_transport_offset(head_skb); + __sum16 pseudo; + + /* Calculate pseudo header and update header fields */ + if (frag_desc->ip_proto == 4) { + struct iphdr *iph = ip_hdr(head_skb); + __be16 tot_len = htons(head_skb->len); + + csum_replace2(&iph->check, iph->tot_len, tot_len); + iph->tot_len = tot_len; + pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr, + head_skb->len - + frag_desc->ip_len, + frag_desc->trans_proto, 0); + } else { + struct ipv6hdr *ip6h = ipv6_hdr(head_skb); + + ip6h->payload_len = htons(head_skb->len - + sizeof(*ip6h)); + pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + head_skb->len - + frag_desc->ip_len, + frag_desc->trans_proto, 0); + } + + if (frag_desc->trans_proto == IPPROTO_TCP) { + check = &tcp_hdr(head_skb)->check; + } else { + udp_hdr(head_skb)->len = htons(head_skb->len - + frag_desc->ip_len); + check = &udp_hdr(head_skb)->check; + } + + *check = pseudo; + csum = skb_checksum(head_skb, offset, head_skb->len - offset, + 0); + /* Add 1 to corrupt. This cannot produce a final value of 0 + * since csum_fold() can't return a value of 0xFFFF + */ + *check = csum16_add(csum_fold(csum), htons(1)); + head_skb->ip_summed = CHECKSUM_NONE; + } + + /* Handle any rmnet_perf metadata */ + if (frag_desc->hash) { + head_skb->hash = frag_desc->hash; + head_skb->sw_hash = 1; + } + + if (frag_desc->flush_shs) + head_skb->cb[0] = 1; + + /* Handle coalesced packets */ + if (frag_desc->gso_segs > 1) + rmnet_frag_gso_stamp(head_skb, frag_desc); + + return head_skb; +} + +/* Deliver the packets contained within a frag descriptor */ +void rmnet_frag_deliver(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port) +{ + struct sk_buff *skb; + + skb = rmnet_alloc_skb(frag_desc, port); + if (skb) + rmnet_deliver_skb(skb, port); + rmnet_recycle_frag_descriptor(frag_desc, port); +} +EXPORT_SYMBOL(rmnet_frag_deliver); + +static void __rmnet_frag_segment_data(struct rmnet_frag_descriptor *coal_desc, + struct rmnet_port *port, + struct list_head *list, u8 pkt_id, + bool csum_valid) +{ + struct rmnet_priv *priv = netdev_priv(coal_desc->dev); + struct rmnet_frag_descriptor *new_desc; + u32 dlen = coal_desc->gso_size * coal_desc->gso_segs; + u32 hlen = coal_desc->ip_len + coal_desc->trans_len; + u32 offset = hlen + coal_desc->data_offset; + int rc; + + new_desc = rmnet_get_frag_descriptor(port); + if (!new_desc) + return; + + /* Header information and most metadata is the same as the original */ + memcpy(new_desc, coal_desc, sizeof(*coal_desc)); + INIT_LIST_HEAD(&new_desc->list); + INIT_LIST_HEAD(&new_desc->frags); + new_desc->len = 0; + + /* Add the header fragments */ + rc = rmnet_frag_descriptor_add_frags_from(new_desc, coal_desc, 0, + hlen); + if (rc < 0) + goto recycle; + + /* Add in the data fragments */ + rc = rmnet_frag_descriptor_add_frags_from(new_desc, coal_desc, offset, + dlen); + if (rc < 0) + goto recycle; + + /* Update protocol-specific metadata */ + if (coal_desc->trans_proto == IPPROTO_TCP) { + struct tcphdr *th, __th; + + th = rmnet_frag_header_ptr(coal_desc, coal_desc->ip_len, + sizeof(*th), &__th); + if (!th) + goto recycle; + + new_desc->tcp_seq_set = 1; + new_desc->tcp_seq = htonl(ntohl(th->seq) + + coal_desc->data_offset); + + /* Don't allow any dangerous flags to appear in any segments + * other than the last. + */ + if (th->fin || th->psh) { + if (offset + dlen < coal_desc->len) { + __be32 flag_word = tcp_flag_word(th); + + /* Clear the FIN and PSH flags from this + * segment. + */ + flag_word &= ~TCP_FLAG_FIN; + flag_word &= ~TCP_FLAG_PSH; + + new_desc->tcp_flags_set = 1; + new_desc->tcp_flags = *((__be16 *)&flag_word); + } + } + } else if (coal_desc->trans_proto == IPPROTO_UDP) { + struct udphdr *uh, __uh; + + uh = rmnet_frag_header_ptr(coal_desc, coal_desc->ip_len, + sizeof(*uh), &__uh); + if (!uh) + goto recycle; + + if (coal_desc->ip_proto == 4 && !uh->check) + csum_valid = true; + } + + if (coal_desc->ip_proto == 4) { + struct iphdr *iph, __iph; + + iph = rmnet_frag_header_ptr(coal_desc, 0, sizeof(*iph), + &__iph); + if (!iph) + goto recycle; + + new_desc->ip_id_set = 1; + new_desc->ip_id = htons(ntohs(iph->id) + coal_desc->pkt_id); + } + + new_desc->csum_valid = csum_valid; + priv->stats.coal.coal_reconstruct++; + + /* Update meta information to move past the data we just segmented */ + coal_desc->data_offset += dlen; + coal_desc->pkt_id = pkt_id + 1; + coal_desc->gso_segs = 0; + + list_add_tail(&new_desc->list, list); + return; + +recycle: + rmnet_recycle_frag_descriptor(new_desc, port); +} + +static bool rmnet_frag_validate_csum(struct rmnet_frag_descriptor *frag_desc) +{ + u8 *data = rmnet_frag_data_ptr(frag_desc); + unsigned int datagram_len; + __wsum csum; + __sum16 pseudo; + + /* Keep analysis tools happy, since they will see that + * rmnet_frag_data_ptr() could return NULL. It can't in this case, + * since we can't get this far otherwise... + */ + if (unlikely(!data)) + return false; + + datagram_len = frag_desc->len - frag_desc->ip_len; + if (frag_desc->ip_proto == 4) { + struct iphdr *iph = (struct iphdr *)data; + + pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr, + datagram_len, + frag_desc->trans_proto, 0); + } else { + struct ipv6hdr *ip6h = (struct ipv6hdr *)data; + + pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + datagram_len, frag_desc->trans_proto, + 0); + } + + csum = csum_partial(data + frag_desc->ip_len, datagram_len, + csum_unfold(pseudo)); + return !csum_fold(csum); +} + +/* Converts the coalesced frame into a list of descriptors. + * NLOs containing csum erros will not be included. + */ +static void +rmnet_frag_segment_coal_data(struct rmnet_frag_descriptor *coal_desc, + u64 nlo_err_mask, struct rmnet_port *port, + struct list_head *list) +{ + struct rmnet_priv *priv = netdev_priv(coal_desc->dev); + struct rmnet_map_v5_coal_header coal_hdr; + u8 *version; + u16 pkt_len; + u8 pkt, total_pkt = 0; + u8 nlo; + bool gro = coal_desc->dev->features & NETIF_F_GRO_HW; + bool zero_csum = false; + + /* Copy the coal header into our local storage before pulling it. It's + * possible that this header (or part of it) is the last port of a page + * a pulling it off would cause it to be freed. Referring back to the + * header would be invalid in that case. + */ + if (rmnet_frag_copy_data(coal_desc, sizeof(struct rmnet_map_header), + sizeof(coal_hdr), &coal_hdr) < 0) + return; + + /* Pull off the headers we no longer need */ + if (!rmnet_frag_pull(coal_desc, port, sizeof(struct rmnet_map_header) + + sizeof(coal_hdr))) + return; + + /* By definition, this byte is linear, and the first byte on the + * first fragment. ;) Hence why no header_ptr() call is needed + * for it. + */ + version = rmnet_frag_data_ptr(coal_desc); + if (unlikely(!version)) + return; + + if ((*version & 0xF0) == 0x40) { + struct iphdr *iph, __iph; + + iph = rmnet_frag_header_ptr(coal_desc, 0, sizeof(*iph), + &__iph); + if (!iph) + return; + + coal_desc->ip_proto = 4; + coal_desc->ip_len = iph->ihl * 4; + coal_desc->trans_proto = iph->protocol; + + /* Don't allow coalescing of any packets with IP options */ + if (iph->ihl != 5) + gro = false; + } else if ((*version & 0xF0) == 0x60) { + struct ipv6hdr *ip6h, __ip6h; + int ip_len; + __be16 frag_off; + u8 protocol; + + ip6h = rmnet_frag_header_ptr(coal_desc, 0, sizeof(*ip6h), + &__ip6h); + if (!ip6h) + return; + + coal_desc->ip_proto = 6; + protocol = ip6h->nexthdr; + ip_len = rmnet_frag_ipv6_skip_exthdr(coal_desc, + sizeof(*ip6h), + &protocol, + &frag_off); + coal_desc->trans_proto = protocol; + + /* If we run into a problem, or this has a fragment header + * (which should technically not be possible, if the HW + * works as intended...), bail. + */ + if (ip_len < 0 || frag_off) { + priv->stats.coal.coal_ip_invalid++; + return; + } + + coal_desc->ip_len = (u16)ip_len; + if (coal_desc->ip_len > sizeof(*ip6h)) { + /* Don't allow coalescing of any packets with IPv6 + * extension headers. + */ + gro = false; + } + } else { + priv->stats.coal.coal_ip_invalid++; + return; + } + + if (coal_desc->trans_proto == IPPROTO_TCP) { + struct tcphdr *th, __th; + + th = rmnet_frag_header_ptr(coal_desc, + coal_desc->ip_len, sizeof(*th), + &__th); + if (!th) + return; + + coal_desc->trans_len = th->doff * 4; + priv->stats.coal.coal_tcp++; + priv->stats.coal.coal_tcp_bytes += coal_desc->len; + } else if (coal_desc->trans_proto == IPPROTO_UDP) { + struct udphdr *uh, __uh; + + uh = rmnet_frag_header_ptr(coal_desc, + coal_desc->ip_len, sizeof(*uh), + &__uh); + if (!uh) + return; + + coal_desc->trans_len = sizeof(*uh); + priv->stats.coal.coal_udp++; + priv->stats.coal.coal_udp_bytes += coal_desc->len; + if (coal_desc->ip_proto == 4 && !uh->check) + zero_csum = true; + } else { + priv->stats.coal.coal_trans_invalid++; + return; + } + + coal_desc->hdrs_valid = 1; + + if (rmnet_map_v5_csum_buggy(&coal_hdr) && !zero_csum) { + /* Mark the checksum as valid if it checks out */ + if (rmnet_frag_validate_csum(coal_desc)) + coal_desc->csum_valid = true; + + coal_desc->gso_size = ntohs(coal_hdr.nl_pairs[0].pkt_len); + coal_desc->gso_size -= coal_desc->ip_len + coal_desc->trans_len; + coal_desc->gso_segs = coal_hdr.nl_pairs[0].num_packets; + list_add_tail(&coal_desc->list, list); + return; + } + + /* Fast-forward the case where we have 1 NLO (i.e. 1 packet length), + * no checksum errors, and are allowing GRO. We can just reuse this + * descriptor unchanged. + */ + if (gro && coal_hdr.num_nlos == 1 && coal_hdr.csum_valid) { + coal_desc->csum_valid = true; + coal_desc->gso_size = ntohs(coal_hdr.nl_pairs[0].pkt_len); + coal_desc->gso_size -= coal_desc->ip_len + coal_desc->trans_len; + coal_desc->gso_segs = coal_hdr.nl_pairs[0].num_packets; + list_add_tail(&coal_desc->list, list); + return; + } + + /* Segment the coalesced descriptor into new packets */ + for (nlo = 0; nlo < coal_hdr.num_nlos; nlo++) { + pkt_len = ntohs(coal_hdr.nl_pairs[nlo].pkt_len); + pkt_len -= coal_desc->ip_len + coal_desc->trans_len; + coal_desc->gso_size = pkt_len; + for (pkt = 0; pkt < coal_hdr.nl_pairs[nlo].num_packets; + pkt++, total_pkt++, nlo_err_mask >>= 1) { + bool csum_err = nlo_err_mask & 1; + + /* Segment the packet if we're not sending the larger + * packet up the stack. + */ + if (!gro) { + coal_desc->gso_segs = 1; + if (csum_err) + priv->stats.coal.coal_csum_err++; + + __rmnet_frag_segment_data(coal_desc, port, + list, total_pkt, + !csum_err); + continue; + } + + if (csum_err) { + priv->stats.coal.coal_csum_err++; + + /* Segment out the good data */ + if (coal_desc->gso_segs) + __rmnet_frag_segment_data(coal_desc, + port, + list, + total_pkt, + true); + + /* Segment out the bad checksum */ + coal_desc->gso_segs = 1; + __rmnet_frag_segment_data(coal_desc, port, + list, total_pkt, + false); + } else { + coal_desc->gso_segs++; + } + } + + /* If we're switching NLOs, we need to send out everything from + * the previous one, if we haven't done so. NLOs only switch + * when the packet length changes. + */ + if (coal_desc->gso_segs) + __rmnet_frag_segment_data(coal_desc, port, list, + total_pkt, true); + } +} + +/* Record reason for coalescing pipe closure */ +static void rmnet_frag_data_log_close_stats(struct rmnet_priv *priv, u8 type, + u8 code) +{ + struct rmnet_coal_close_stats *stats = &priv->stats.coal.close; + + switch (type) { + case RMNET_MAP_COAL_CLOSE_NON_COAL: + stats->non_coal++; + break; + case RMNET_MAP_COAL_CLOSE_IP_MISS: + stats->ip_miss++; + break; + case RMNET_MAP_COAL_CLOSE_TRANS_MISS: + stats->trans_miss++; + break; + case RMNET_MAP_COAL_CLOSE_HW: + switch (code) { + case RMNET_MAP_COAL_CLOSE_HW_NL: + stats->hw_nl++; + break; + case RMNET_MAP_COAL_CLOSE_HW_PKT: + stats->hw_pkt++; + break; + case RMNET_MAP_COAL_CLOSE_HW_BYTE: + stats->hw_byte++; + break; + case RMNET_MAP_COAL_CLOSE_HW_TIME: + stats->hw_time++; + break; + case RMNET_MAP_COAL_CLOSE_HW_EVICT: + stats->hw_evict++; + break; + default: + break; + } + break; + case RMNET_MAP_COAL_CLOSE_COAL: + stats->coal++; + break; + default: + break; + } +} + +/* Check if the coalesced header has any incorrect values, in which case, the + * entire coalesced frame must be dropped. Then check if there are any + * checksum issues + */ +static int +rmnet_frag_data_check_coal_header(struct rmnet_frag_descriptor *frag_desc, + u64 *nlo_err_mask) +{ + struct rmnet_map_v5_coal_header *coal_hdr, __coal_hdr; + struct rmnet_priv *priv = netdev_priv(frag_desc->dev); + u64 mask = 0; + int i; + u8 veid, pkts = 0; + + coal_hdr = rmnet_frag_header_ptr(frag_desc, + sizeof(struct rmnet_map_header), + sizeof(*coal_hdr), &__coal_hdr); + if (!coal_hdr) + return -EINVAL; + + veid = coal_hdr->virtual_channel_id; + + if (coal_hdr->num_nlos == 0 || + coal_hdr->num_nlos > RMNET_MAP_V5_MAX_NLOS) { + priv->stats.coal.coal_hdr_nlo_err++; + return -EINVAL; + } + + for (i = 0; i < RMNET_MAP_V5_MAX_NLOS; i++) { + /* If there is a checksum issue, we need to split + * up the skb. Rebuild the full csum error field + */ + u8 err = coal_hdr->nl_pairs[i].csum_error_bitmap; + u8 pkt = coal_hdr->nl_pairs[i].num_packets; + + mask |= ((u64)err) << (8 * i); + + /* Track total packets in frame */ + pkts += pkt; + if (pkts > RMNET_MAP_V5_MAX_PACKETS) { + priv->stats.coal.coal_hdr_pkt_err++; + return -EINVAL; + } + } + + /* Track number of packets we get inside of coalesced frames */ + priv->stats.coal.coal_pkts += pkts; + + /* Update ethtool stats */ + rmnet_frag_data_log_close_stats(priv, + coal_hdr->close_type, + coal_hdr->close_value); + if (veid < RMNET_MAX_VEID) + priv->stats.coal.coal_veid[veid]++; + + *nlo_err_mask = mask; + + return 0; +} + +/* Process a QMAPv5 packet header */ +int rmnet_frag_process_next_hdr_packet(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, + struct list_head *list, + u16 len) +{ + struct rmnet_map_v5_csum_header *csum_hdr, __csum_hdr; + struct rmnet_priv *priv = netdev_priv(frag_desc->dev); + u64 nlo_err_mask; + u32 offset = sizeof(struct rmnet_map_header); + int rc = 0; + + /* Grab the header type. It's easier to grab enough for a full csum + * offload header here since it's only 8 bytes and then check the + * header type using that. This also doubles as a check to make sure + * there's enough data after the QMAP header to ensure that another + * header is present. + */ + csum_hdr = rmnet_frag_header_ptr(frag_desc, offset, sizeof(*csum_hdr), + &__csum_hdr); + if (!csum_hdr) + return -EINVAL; + + switch (csum_hdr->header_type) { + case RMNET_MAP_HEADER_TYPE_COALESCING: + priv->stats.coal.coal_rx++; + rc = rmnet_frag_data_check_coal_header(frag_desc, + &nlo_err_mask); + if (rc) + return rc; + + rmnet_frag_segment_coal_data(frag_desc, nlo_err_mask, port, + list); + if (list_first_entry(list, struct rmnet_frag_descriptor, + list) != frag_desc) + rmnet_recycle_frag_descriptor(frag_desc, port); + break; + case RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD: + if (unlikely(!(frag_desc->dev->features & NETIF_F_RXCSUM))) { + priv->stats.csum_sw++; + } else if (csum_hdr->csum_valid_required) { + priv->stats.csum_ok++; + frag_desc->csum_valid = true; + } else { + priv->stats.csum_valid_unset++; + } + + if (!rmnet_frag_pull(frag_desc, port, + offset + sizeof(*csum_hdr))) { + rc = -EINVAL; + break; + } + + /* Remove padding only for csum offload packets. + * Coalesced packets should never have padding. + */ + if (!rmnet_frag_trim(frag_desc, port, len)) { + rc = -EINVAL; + break; + } + + list_del_init(&frag_desc->list); + list_add_tail(&frag_desc->list, list); + break; + default: + rc = -EINVAL; + break; + } + + return rc; +} + +/* Perf hook handler */ +rmnet_perf_desc_hook_t rmnet_perf_desc_entry __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_perf_desc_entry); + +static void +__rmnet_frag_ingress_handler(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port) +{ + rmnet_perf_desc_hook_t rmnet_perf_ingress; + struct rmnet_map_header *qmap, __qmap; + struct rmnet_endpoint *ep; + struct rmnet_frag_descriptor *frag, *tmp; + LIST_HEAD(segs); + u16 len, pad; + u8 mux_id; + + qmap = rmnet_frag_header_ptr(frag_desc, 0, sizeof(*qmap), &__qmap); + if (!qmap) + goto recycle; + + mux_id = qmap->mux_id; + pad = qmap->pad_len; + len = ntohs(qmap->pkt_len) - pad; + + if (qmap->cd_bit) { + qmi_rmnet_set_dl_msg_active(port); + if (port->data_format & RMNET_INGRESS_FORMAT_DL_MARKER) { + rmnet_frag_flow_command(frag_desc, port, len); + goto recycle; + } + + if (port->data_format & RMNET_FLAGS_INGRESS_MAP_COMMANDS) + rmnet_frag_command(frag_desc, qmap, port); + + goto recycle; + } + + if (mux_id >= RMNET_MAX_LOGICAL_EP) + goto recycle; + + ep = rmnet_get_endpoint(port, mux_id); + if (!ep) + goto recycle; + + frag_desc->dev = ep->egress_dev; + + /* Handle QMAPv5 packet */ + if (qmap->next_hdr && + (port->data_format & (RMNET_FLAGS_INGRESS_COALESCE | + RMNET_FLAGS_INGRESS_MAP_CKSUMV5))) { + if (rmnet_frag_process_next_hdr_packet(frag_desc, port, &segs, + len)) + goto recycle; + } else { + /* We only have the main QMAP header to worry about */ + if (!rmnet_frag_pull(frag_desc, port, sizeof(*qmap))) + return; + + if (!rmnet_frag_trim(frag_desc, port, len)) + return; + + list_add_tail(&frag_desc->list, &segs); + } + + if (port->data_format & RMNET_INGRESS_FORMAT_PS) + qmi_rmnet_work_maybe_restart(port); + + rcu_read_lock(); + rmnet_perf_ingress = rcu_dereference(rmnet_perf_desc_entry); + if (rmnet_perf_ingress) { + list_for_each_entry_safe(frag, tmp, &segs, list) { + list_del_init(&frag->list); + rmnet_perf_ingress(frag, port); + } + rcu_read_unlock(); + return; + } + rcu_read_unlock(); + + list_for_each_entry_safe(frag, tmp, &segs, list) { + list_del_init(&frag->list); + rmnet_frag_deliver(frag, port); + } + return; + +recycle: + rmnet_recycle_frag_descriptor(frag_desc, port); +} + +/* Notify perf at the end of SKB chain */ +rmnet_perf_chain_hook_t rmnet_perf_chain_end __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_perf_chain_end); + +void rmnet_frag_ingress_handler(struct sk_buff *skb, + struct rmnet_port *port) +{ + rmnet_perf_chain_hook_t rmnet_perf_opt_chain_end; + LIST_HEAD(desc_list); + + /* Deaggregation and freeing of HW originating + * buffers is done within here + */ + while (skb) { + struct sk_buff *skb_frag; + + rmnet_frag_deaggregate(skb, port, &desc_list); + if (!list_empty(&desc_list)) { + struct rmnet_frag_descriptor *frag_desc, *tmp; + + list_for_each_entry_safe(frag_desc, tmp, &desc_list, + list) { + list_del_init(&frag_desc->list); + __rmnet_frag_ingress_handler(frag_desc, port); + } + } + + skb_frag = skb_shinfo(skb)->frag_list; + skb_shinfo(skb)->frag_list = NULL; + consume_skb(skb); + skb = skb_frag; + } + + rcu_read_lock(); + rmnet_perf_opt_chain_end = rcu_dereference(rmnet_perf_chain_end); + if (rmnet_perf_opt_chain_end) + rmnet_perf_opt_chain_end(); + rcu_read_unlock(); +} + +void rmnet_descriptor_deinit(struct rmnet_port *port) +{ + struct rmnet_frag_descriptor_pool *pool; + struct rmnet_frag_descriptor *frag_desc, *tmp; + + pool = port->frag_desc_pool; + + list_for_each_entry_safe(frag_desc, tmp, &pool->free_list, list) { + kfree(frag_desc); + pool->pool_size--; + } + + kfree(pool); +} + +int rmnet_descriptor_init(struct rmnet_port *port) +{ + struct rmnet_frag_descriptor_pool *pool; + int i; + + spin_lock_init(&port->desc_pool_lock); + pool = kzalloc(sizeof(*pool), GFP_ATOMIC); + if (!pool) + return -ENOMEM; + + INIT_LIST_HEAD(&pool->free_list); + port->frag_desc_pool = pool; + + for (i = 0; i < RMNET_FRAG_DESCRIPTOR_POOL_SIZE; i++) { + struct rmnet_frag_descriptor *frag_desc; + + frag_desc = kzalloc(sizeof(*frag_desc), GFP_ATOMIC); + if (!frag_desc) + return -ENOMEM; + + INIT_LIST_HEAD(&frag_desc->list); + INIT_LIST_HEAD(&frag_desc->frags); + list_add_tail(&frag_desc->list, &pool->free_list); + pool->pool_size++; + } + + return 0; +} diff --git a/techpack/datarmnet/core/rmnet_descriptor.h b/techpack/datarmnet/core/rmnet_descriptor.h new file mode 100644 index 000000000000..f7bfa55bffe7 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_descriptor.h @@ -0,0 +1,114 @@ +/* Copyright (c) 2013-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Packet Descriptor Framework + * + */ + +#ifndef _RMNET_DESCRIPTOR_H_ +#define _RMNET_DESCRIPTOR_H_ + +#include +#include +#include +#include "rmnet_config.h" +#include "rmnet_map.h" + +struct rmnet_frag_descriptor_pool { + struct list_head free_list; + u32 pool_size; +}; + +struct rmnet_fragment { + struct list_head list; + skb_frag_t frag; +}; + +struct rmnet_frag_descriptor { + struct list_head list; + struct list_head frags; + struct net_device *dev; + u32 len; + u32 hash; + __be32 tcp_seq; + __be16 ip_id; + __be16 tcp_flags; + u16 data_offset; + u16 gso_size; + u16 gso_segs; + u16 ip_len; + u16 trans_len; + u8 ip_proto; + u8 trans_proto; + u8 pkt_id; + u8 csum_valid:1, + hdrs_valid:1, + ip_id_set:1, + tcp_seq_set:1, + flush_shs:1, + tcp_flags_set:1, + reserved:2; +}; + +/* Descriptor management */ +struct rmnet_frag_descriptor * +rmnet_get_frag_descriptor(struct rmnet_port *port); +void rmnet_recycle_frag_descriptor(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port); +void *rmnet_frag_pull(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, unsigned int size); +void *rmnet_frag_trim(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, unsigned int size); +void *rmnet_frag_header_ptr(struct rmnet_frag_descriptor *frag_desc, u32 off, + u32 len, void *buf); +int rmnet_frag_descriptor_add_frag(struct rmnet_frag_descriptor *frag_desc, + struct page *p, u32 page_offset, u32 len); +int rmnet_frag_descriptor_add_frags_from(struct rmnet_frag_descriptor *to, + struct rmnet_frag_descriptor *from, + u32 off, u32 len); +int rmnet_frag_ipv6_skip_exthdr(struct rmnet_frag_descriptor *frag_desc, + int start, u8 *nexthdrp, __be16 *fragp); + +/* QMAP command packets */ +void rmnet_frag_command(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_map_header *qmap, struct rmnet_port *port); +int rmnet_frag_flow_command(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, u16 pkt_len); + +/* Ingress data handlers */ +void rmnet_frag_deaggregate(struct sk_buff *skb, struct rmnet_port *port, + struct list_head *list); +void rmnet_frag_deliver(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port); +int rmnet_frag_process_next_hdr_packet(struct rmnet_frag_descriptor *frag_desc, + struct rmnet_port *port, + struct list_head *list, + u16 len); +void rmnet_frag_ingress_handler(struct sk_buff *skb, + struct rmnet_port *port); + +int rmnet_descriptor_init(struct rmnet_port *port); +void rmnet_descriptor_deinit(struct rmnet_port *port); + +static inline void *rmnet_frag_data_ptr(struct rmnet_frag_descriptor *frag_desc) +{ + struct rmnet_fragment *frag; + + frag = list_first_entry_or_null(&frag_desc->frags, + struct rmnet_fragment, list); + + if (!frag) + return NULL; + + return skb_frag_address(&frag->frag); +} + +#endif /* _RMNET_DESCRIPTOR_H_ */ diff --git a/techpack/datarmnet/core/rmnet_genl.c b/techpack/datarmnet/core/rmnet_genl.c new file mode 100644 index 000000000000..409c32e437c5 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_genl.c @@ -0,0 +1,412 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* Copyright (c) 2019-2021, The Linux Foundation. All rights reserved. + * + * RMNET Data Generic Netlink + * + */ + +#include "rmnet_genl.h" +#include +#include + +#define RMNET_CORE_GENL_MAX_STR_LEN 255 + +/* Static Functions and Definitions */ +static struct nla_policy rmnet_genl_attr_policy[RMNET_CORE_GENL_ATTR_MAX + + 1] = { + [RMNET_CORE_GENL_ATTR_INT] = { .type = NLA_S32 }, + [RMNET_CORE_GENL_ATTR_PID_BPS] = { .type = NLA_EXACT_LEN, .len = + sizeof(struct rmnet_core_pid_bps_resp) }, + [RMNET_CORE_GENL_ATTR_PID_BOOST] = { .type = NLA_EXACT_LEN, .len = + sizeof(struct rmnet_core_pid_boost_req) }, + [RMNET_CORE_GENL_ATTR_STR] = { .type = NLA_NUL_STRING, .len = + RMNET_CORE_GENL_MAX_STR_LEN }, +}; + +#define RMNET_CORE_GENL_OP(_cmd, _func) \ + { \ + .cmd = _cmd, \ + .doit = _func, \ + .dumpit = NULL, \ + .flags = 0, \ + } + +static const struct genl_ops rmnet_core_genl_ops[] = { + RMNET_CORE_GENL_OP(RMNET_CORE_GENL_CMD_PID_BPS_REQ, + rmnet_core_genl_pid_bps_req_hdlr), + RMNET_CORE_GENL_OP(RMNET_CORE_GENL_CMD_PID_BOOST_REQ, + rmnet_core_genl_pid_boost_req_hdlr), +}; + +struct genl_family rmnet_core_genl_family = { + .hdrsize = 0, + .name = RMNET_CORE_GENL_FAMILY_NAME, + .version = RMNET_CORE_GENL_VERSION, + .maxattr = RMNET_CORE_GENL_ATTR_MAX, + .policy = rmnet_genl_attr_policy, + .ops = rmnet_core_genl_ops, + .n_ops = ARRAY_SIZE(rmnet_core_genl_ops), +}; + +#define RMNET_PID_STATS_HT_SIZE (8) +#define RMNET_PID_STATS_HT rmnet_pid_ht +DEFINE_HASHTABLE(rmnet_pid_ht, RMNET_PID_STATS_HT_SIZE); + +/* Spinlock definition for pid hash table */ +static DEFINE_SPINLOCK(rmnet_pid_ht_splock); + +#define RMNET_GENL_SEC_TO_MSEC(x) ((x) * 1000) +#define RMNET_GENL_SEC_TO_NSEC(x) ((x) * 1000000000) +#define RMNET_GENL_BYTES_TO_BITS(x) ((x) * 8) +#define RMNET_GENL_NSEC_TO_SEC(x) ({\ + u64 __quotient = (x); \ + do_div(__quotient, 1000000000); \ + __quotient; \ +}) + +int rmnet_core_userspace_connected; +#define RMNET_QUERY_PERIOD_SEC (1) /* Period of pid/bps queries */ + +struct rmnet_pid_node_s { + struct hlist_node list; + time_t timstamp_last_query; + u64 tx_bytes; + u64 tx_bytes_last_query; + u64 tx_bps; + u64 sched_boost_period_ms; + int sched_boost_remaining_ms; + int sched_boost_enable; + pid_t pid; +}; + +void rmnet_update_pid_and_check_boost(pid_t pid, unsigned int len, + int *boost_enable, u64 *boost_period) +{ + struct hlist_node *tmp; + struct rmnet_pid_node_s *node_p; + unsigned long ht_flags; + u8 is_match_found = 0; + u64 tx_bytes = 0; + + *boost_enable = 0; + *boost_period = 0; + + /* Using do while to spin lock and unlock only once */ + spin_lock_irqsave(&rmnet_pid_ht_splock, ht_flags); + do { + hash_for_each_possible_safe(RMNET_PID_STATS_HT, node_p, tmp, + list, pid) { + if (pid != node_p->pid) + continue; + + /* PID Match found */ + is_match_found = 1; + node_p->tx_bytes += len; + tx_bytes = node_p->tx_bytes; + + if (node_p->sched_boost_enable) { + rm_err("boost triggered for pid %d", + pid); + /* Just triggered boost, dont re-trigger */ + node_p->sched_boost_enable = 0; + *boost_enable = 1; + *boost_period = node_p->sched_boost_period_ms; + node_p->sched_boost_remaining_ms = + (int)*boost_period; + } + + break; + } + + if (is_match_found) + break; + + /* No PID match */ + node_p = kzalloc(sizeof(*node_p), GFP_ATOMIC); + if (!node_p) + break; + + node_p->pid = pid; + node_p->tx_bytes = len; + node_p->sched_boost_enable = 0; + node_p->sched_boost_period_ms = 0; + node_p->sched_boost_remaining_ms = 0; + hash_add_rcu(RMNET_PID_STATS_HT, &node_p->list, pid); + break; + } while (0); + spin_unlock_irqrestore(&rmnet_pid_ht_splock, ht_flags); +} + +void rmnet_boost_for_pid(pid_t pid, int boost_enable, + u64 boost_period) +{ + struct hlist_node *tmp; + struct rmnet_pid_node_s *node_p; + unsigned long ht_flags; + + /* Using do while to spin lock and unlock only once */ + spin_lock_irqsave(&rmnet_pid_ht_splock, ht_flags); + do { + hash_for_each_possible_safe(RMNET_PID_STATS_HT, node_p, tmp, + list, pid) { + if (pid != node_p->pid) + continue; + + /* PID Match found */ + rm_err("CORE_BOOST: enable boost for pid %d for %d ms", + pid, boost_period); + node_p->sched_boost_enable = boost_enable; + node_p->sched_boost_period_ms = boost_period; + break; + } + + break; + } while (0); + spin_unlock_irqrestore(&rmnet_pid_ht_splock, ht_flags); +} + +static void rmnet_create_pid_bps_resp(struct rmnet_core_pid_bps_resp + *pid_bps_resp_ptr) +{ + struct timespec64 time; + struct hlist_node *tmp; + struct rmnet_pid_node_s *node_p; + unsigned long ht_flags; + u64 tx_bytes_cur, byte_diff, time_diff_ns, tmp_bits; + int i; + u16 bkt; + + ktime_get_real_ts64(&time); + pid_bps_resp_ptr->timestamp = RMNET_GENL_SEC_TO_NSEC(time.tv_sec) + + time.tv_nsec; + + /* Using do while to spin lock and unlock only once */ + spin_lock_irqsave(&rmnet_pid_ht_splock, ht_flags); + do { + i = 0; + + hash_for_each_safe(RMNET_PID_STATS_HT, bkt, tmp, + node_p, list) { + tx_bytes_cur = node_p->tx_bytes; + if (tx_bytes_cur <= node_p->tx_bytes_last_query) { + /* Dont send inactive pids to userspace */ + hash_del(&node_p->list); + kfree(node_p); + continue; + } + + /* Compute bits per second */ + byte_diff = (node_p->tx_bytes - + node_p->tx_bytes_last_query); + time_diff_ns = (pid_bps_resp_ptr->timestamp - + node_p->timstamp_last_query); + + tmp_bits = RMNET_GENL_BYTES_TO_BITS(byte_diff); + /* Note that do_div returns remainder and the */ + /* numerator gets assigned the quotient */ + /* Since do_div takes the numerator as a reference, */ + /* a tmp_bits is used*/ + do_div(tmp_bits, RMNET_GENL_NSEC_TO_SEC(time_diff_ns)); + node_p->tx_bps = tmp_bits; + + if (node_p->sched_boost_remaining_ms >= + RMNET_GENL_SEC_TO_MSEC(RMNET_QUERY_PERIOD_SEC)) { + node_p->sched_boost_remaining_ms -= + RMNET_GENL_SEC_TO_MSEC(RMNET_QUERY_PERIOD_SEC); + + rm_err("CORE_BOOST: enabling boost for pid %d\n" + "sched boost remaining = %d ms", + node_p->pid, + node_p->sched_boost_remaining_ms); + } else { + node_p->sched_boost_remaining_ms = 0; + } + + pid_bps_resp_ptr->list[i].pid = node_p->pid; + pid_bps_resp_ptr->list[i].tx_bps = node_p->tx_bps; + pid_bps_resp_ptr->list[i].boost_remaining_ms = + node_p->sched_boost_remaining_ms; + + node_p->timstamp_last_query = + pid_bps_resp_ptr->timestamp; + node_p->tx_bytes_last_query = tx_bytes_cur; + i++; + + /* Support copying up to 32 active pids */ + if (i >= RMNET_CORE_GENL_MAX_PIDS) + break; + } + break; + } while (0); + spin_unlock_irqrestore(&rmnet_pid_ht_splock, ht_flags); + + pid_bps_resp_ptr->list_len = i; +} + +int rmnet_core_genl_send_resp(struct genl_info *info, + struct rmnet_core_pid_bps_resp *pid_bps_resp) +{ + struct sk_buff *skb; + void *msg_head; + int rc; + + if (!info || !pid_bps_resp) { + rm_err("%s", "SHS_GNL: Invalid params\n"); + goto out; + } + + skb = genlmsg_new(sizeof(struct rmnet_core_pid_bps_resp), GFP_KERNEL); + if (!skb) + goto out; + + msg_head = genlmsg_put(skb, 0, info->snd_seq + 1, + &rmnet_core_genl_family, + 0, RMNET_CORE_GENL_CMD_PID_BPS_REQ); + if (!msg_head) { + rc = -ENOMEM; + goto out; + } + rc = nla_put(skb, RMNET_CORE_GENL_ATTR_PID_BPS, + sizeof(struct rmnet_core_pid_bps_resp), + pid_bps_resp); + if (rc != 0) + goto out; + + genlmsg_end(skb, msg_head); + + rc = genlmsg_unicast(genl_info_net(info), skb, info->snd_portid); + if (rc != 0) + goto out; + + rm_err("%s", "SHS_GNL: Successfully sent pid/bytes info\n"); + return RMNET_GENL_SUCCESS; + +out: + /* TODO: Need to free skb?? */ + rm_err("%s", "SHS_GNL: FAILED to send pid/bytes info\n"); + rmnet_core_userspace_connected = 0; + return RMNET_GENL_FAILURE; +} + +int rmnet_core_genl_pid_bps_req_hdlr(struct sk_buff *skb_2, + struct genl_info *info) +{ + struct nlattr *na; + struct rmnet_core_pid_bps_req pid_bps_req; + struct rmnet_core_pid_bps_resp pid_bps_resp; + int is_req_valid = 0; + + rm_err("CORE_GNL: %s connected = %d", __func__, + rmnet_core_userspace_connected); + + if (!info) { + rm_err("%s", "CORE_GNL: error - info is null"); + pid_bps_resp.valid = 0; + } else { + na = info->attrs[RMNET_CORE_GENL_ATTR_PID_BPS]; + if (na) { + if (nla_memcpy(&pid_bps_req, na, + sizeof(pid_bps_req)) > 0) { + is_req_valid = 1; + } else { + rm_err("CORE_GNL: nla_memcpy failed %d\n", + RMNET_CORE_GENL_ATTR_PID_BPS); + } + } else { + rm_err("CORE_GNL: no info->attrs %d\n", + RMNET_CORE_GENL_ATTR_PID_BPS); + } + } + + if (!rmnet_core_userspace_connected) + rmnet_core_userspace_connected = 1; + + /* Copy to pid/byte list to the payload */ + memset(&pid_bps_resp, 0x0, + sizeof(pid_bps_resp)); + if (is_req_valid) { + rmnet_create_pid_bps_resp(&pid_bps_resp); + } + pid_bps_resp.valid = 1; + + rmnet_core_genl_send_resp(info, &pid_bps_resp); + + return RMNET_GENL_SUCCESS; +} + +int rmnet_core_genl_pid_boost_req_hdlr(struct sk_buff *skb_2, + struct genl_info *info) +{ + struct nlattr *na; + struct rmnet_core_pid_boost_req pid_boost_req; + int is_req_valid = 0; + u16 boost_pid_cnt = RMNET_CORE_GENL_MAX_PIDS; + u16 i = 0; + + rm_err("%s", "CORE_GNL: %s", __func__); + + if (!info) { + rm_err("%s", "CORE_GNL: error - info is null"); + return RMNET_GENL_FAILURE; + } + + na = info->attrs[RMNET_CORE_GENL_ATTR_PID_BOOST]; + if (na) { + if (nla_memcpy(&pid_boost_req, na, sizeof(pid_boost_req)) > 0) { + is_req_valid = 1; + } else { + rm_err("CORE_GNL: nla_memcpy failed %d\n", + RMNET_CORE_GENL_ATTR_PID_BOOST); + return RMNET_GENL_FAILURE; + } + } else { + rm_err("CORE_GNL: no info->attrs %d\n", + RMNET_CORE_GENL_ATTR_PID_BOOST); + return RMNET_GENL_FAILURE; + } + + if (pid_boost_req.list_len < RMNET_CORE_GENL_MAX_PIDS) + boost_pid_cnt = pid_boost_req.list_len; + + if (!pid_boost_req.valid) + boost_pid_cnt = 0; + + for (i = 0; i < boost_pid_cnt; i++) { + if (pid_boost_req.list[i].boost_enabled) { + rmnet_boost_for_pid(pid_boost_req.list[i].pid, 1, + pid_boost_req.list[i].boost_period); + } + } + + return RMNET_GENL_SUCCESS; +} + +/* register new rmnet core driver generic netlink family */ +int rmnet_core_genl_init(void) +{ + int ret; + + ret = genl_register_family(&rmnet_core_genl_family); + if (ret != 0) { + rm_err("CORE_GNL: register family failed: %i", ret); + genl_unregister_family(&rmnet_core_genl_family); + return RMNET_GENL_FAILURE; + } + + rm_err("CORE_GNL: successfully registered generic netlink family: %s", + RMNET_CORE_GENL_FAMILY_NAME); + + return RMNET_GENL_SUCCESS; +} + +/* Unregister the generic netlink family */ +int rmnet_core_genl_deinit(void) +{ + int ret; + + ret = genl_unregister_family(&rmnet_core_genl_family); + if (ret != 0) + rm_err("CORE_GNL: unregister family failed: %i\n", ret); + + return RMNET_GENL_SUCCESS; +} diff --git a/techpack/datarmnet/core/rmnet_genl.h b/techpack/datarmnet/core/rmnet_genl.h new file mode 100644 index 000000000000..2a735fa2ff1e --- /dev/null +++ b/techpack/datarmnet/core/rmnet_genl.h @@ -0,0 +1,96 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* Copyright (c) 2019, The Linux Foundation. All rights reserved. + * + * RMNET Data Generic Netlink + * + */ + +#ifndef _RMNET_GENL_H_ +#define _RMNET_GENL_H_ + +#include + +#define RMNET_CORE_DEBUG 0 + +#define rm_err(fmt, ...) \ + do { if (RMNET_CORE_DEBUG) pr_err(fmt, __VA_ARGS__); } while (0) + +/* Generic Netlink Definitions */ +#define RMNET_CORE_GENL_VERSION 1 +#define RMNET_CORE_GENL_FAMILY_NAME "RMNET_CORE" + +#define RMNET_CORE_GENL_MAX_PIDS 32 + +#define RMNET_GENL_SUCCESS (0) +#define RMNET_GENL_FAILURE (-1) + +extern int rmnet_core_userspace_connected; + +enum { + RMNET_CORE_GENL_CMD_UNSPEC, + RMNET_CORE_GENL_CMD_PID_BPS_REQ, + RMNET_CORE_GENL_CMD_PID_BOOST_REQ, + __RMNET_CORE_GENL_CMD_MAX, +}; + +enum { + RMNET_CORE_GENL_ATTR_UNSPEC, + RMNET_CORE_GENL_ATTR_STR, + RMNET_CORE_GENL_ATTR_INT, + RMNET_CORE_GENL_ATTR_PID_BPS, + RMNET_CORE_GENL_ATTR_PID_BOOST, + __RMNET_CORE_GENL_ATTR_MAX, +}; + +#define RMNET_CORE_GENL_ATTR_MAX (__RMNET_CORE_GENL_ATTR_MAX - 1) + +struct rmnet_core_pid_bps_info { + u64 tx_bps; + u32 pid; + u32 boost_remaining_ms; +}; + +struct rmnet_core_pid_boost_info { + u32 boost_enabled; + /* Boost period in ms */ + u32 boost_period; + u32 pid; +}; + +struct rmnet_core_pid_bps_req { + struct rmnet_core_pid_bps_info list[RMNET_CORE_GENL_MAX_PIDS]; + u64 timestamp; + u16 list_len; + u8 valid; +}; + +struct rmnet_core_pid_bps_resp { + struct rmnet_core_pid_bps_info list[RMNET_CORE_GENL_MAX_PIDS]; + u64 timestamp; + u16 list_len; + u8 valid; +}; + +struct rmnet_core_pid_boost_req { + struct rmnet_core_pid_boost_info list[RMNET_CORE_GENL_MAX_PIDS]; + u64 timestamp; + u16 list_len; + u8 valid; +}; + +/* Function Prototypes */ +int rmnet_core_genl_pid_bps_req_hdlr(struct sk_buff *skb_2, + struct genl_info *info); + +int rmnet_core_genl_pid_boost_req_hdlr(struct sk_buff *skb_2, + struct genl_info *info); + +/* Called by vnd select queue */ +void rmnet_update_pid_and_check_boost(pid_t pid, unsigned int len, + int *boost_enable, u64 *boost_period); + +int rmnet_core_genl_init(void); + +int rmnet_core_genl_deinit(void); + +#endif /*_RMNET_CORE_GENL_H_*/ diff --git a/techpack/datarmnet/core/rmnet_handlers.c b/techpack/datarmnet/core/rmnet_handlers.c new file mode 100755 index 000000000000..0399116017f6 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_handlers.c @@ -0,0 +1,485 @@ +/* Copyright (c) 2013-2021, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Data ingress/egress handler + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include "rmnet_private.h" +#include "rmnet_config.h" +#include "rmnet_vnd.h" +#include "rmnet_map.h" +#include "rmnet_handlers.h" +#include "rmnet_descriptor.h" + +#include "rmnet_qmi.h" +#include "qmi_rmnet.h" + +#define RMNET_IP_VERSION_4 0x40 +#define RMNET_IP_VERSION_6 0x60 +#define CREATE_TRACE_POINTS +#include "rmnet_trace.h" + +EXPORT_TRACEPOINT_SYMBOL(rmnet_shs_low); +EXPORT_TRACEPOINT_SYMBOL(rmnet_shs_high); +EXPORT_TRACEPOINT_SYMBOL(rmnet_shs_err); +EXPORT_TRACEPOINT_SYMBOL(rmnet_shs_wq_low); +EXPORT_TRACEPOINT_SYMBOL(rmnet_shs_wq_high); +EXPORT_TRACEPOINT_SYMBOL(rmnet_shs_wq_err); +EXPORT_TRACEPOINT_SYMBOL(rmnet_perf_low); +EXPORT_TRACEPOINT_SYMBOL(rmnet_perf_high); +EXPORT_TRACEPOINT_SYMBOL(rmnet_perf_err); +EXPORT_TRACEPOINT_SYMBOL(rmnet_low); +EXPORT_TRACEPOINT_SYMBOL(rmnet_high); +EXPORT_TRACEPOINT_SYMBOL(rmnet_err); +EXPORT_TRACEPOINT_SYMBOL(rmnet_freq_update); +EXPORT_TRACEPOINT_SYMBOL(rmnet_freq_reset); +EXPORT_TRACEPOINT_SYMBOL(rmnet_freq_boost); + + + +/* Helper Functions */ + +void rmnet_set_skb_proto(struct sk_buff *skb) +{ + switch (rmnet_map_data_ptr(skb)[0] & 0xF0) { + case RMNET_IP_VERSION_4: + skb->protocol = htons(ETH_P_IP); + break; + case RMNET_IP_VERSION_6: + skb->protocol = htons(ETH_P_IPV6); + break; + default: + skb->protocol = htons(ETH_P_MAP); + break; + } +} +EXPORT_SYMBOL(rmnet_set_skb_proto); + +bool (*rmnet_shs_slow_start_detect)(u32 hash_key) __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_shs_slow_start_detect); + +bool rmnet_slow_start_on(u32 hash_key) +{ + bool (*rmnet_shs_slow_start_on)(u32 hash_key); + + rmnet_shs_slow_start_on = rcu_dereference(rmnet_shs_slow_start_detect); + if (rmnet_shs_slow_start_on) + return rmnet_shs_slow_start_on(hash_key); + + return false; +} +EXPORT_SYMBOL(rmnet_slow_start_on); + +/* Shs hook handler */ +int (*rmnet_shs_skb_entry)(struct sk_buff *skb, + struct rmnet_shs_clnt_s *cfg) __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_shs_skb_entry); + +int (*rmnet_shs_switch)(struct sk_buff *skb, + struct rmnet_shs_clnt_s *cfg) __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_shs_switch); + + +/* Shs hook handler for work queue*/ +int (*rmnet_shs_skb_entry_wq)(struct sk_buff *skb, + struct rmnet_shs_clnt_s *cfg) __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_shs_skb_entry_wq); + +/* Generic handler */ + +void +rmnet_deliver_skb(struct sk_buff *skb, struct rmnet_port *port) +{ + int (*rmnet_shs_stamp)(struct sk_buff *skb, + struct rmnet_shs_clnt_s *cfg); + + trace_rmnet_low(RMNET_MODULE, RMNET_DLVR_SKB, 0xDEF, 0xDEF, + 0xDEF, 0xDEF, (void *)skb, NULL); + skb_reset_transport_header(skb); + skb_reset_network_header(skb); + rmnet_vnd_rx_fixup(skb->dev, skb->len); + + skb->pkt_type = PACKET_HOST; + skb_set_mac_header(skb, 0); + + rcu_read_lock(); + rmnet_shs_stamp = rcu_dereference(rmnet_shs_skb_entry); + if (rmnet_shs_stamp) { + rmnet_shs_stamp(skb, &port->shs_cfg); + rcu_read_unlock(); + return; + } + rcu_read_unlock(); + + netif_receive_skb(skb); +} +EXPORT_SYMBOL(rmnet_deliver_skb); + +/* Important to note, port cannot be used here if it has gone stale */ +void +rmnet_deliver_skb_wq(struct sk_buff *skb, struct rmnet_port *port, + enum rmnet_packet_context ctx) +{ + int (*rmnet_shs_stamp)(struct sk_buff *skb, + struct rmnet_shs_clnt_s *cfg); + struct rmnet_priv *priv = netdev_priv(skb->dev); + + trace_rmnet_low(RMNET_MODULE, RMNET_DLVR_SKB, 0xDEF, 0xDEF, + 0xDEF, 0xDEF, (void *)skb, NULL); + skb_reset_transport_header(skb); + skb_reset_network_header(skb); + rmnet_vnd_rx_fixup(skb->dev, skb->len); + + skb->pkt_type = PACKET_HOST; + skb_set_mac_header(skb, 0); + + /* packets coming from work queue context due to packet flush timer + * must go through the special workqueue path in SHS driver + */ + rcu_read_lock(); + rmnet_shs_stamp = (!ctx) ? rcu_dereference(rmnet_shs_skb_entry) : + rcu_dereference(rmnet_shs_skb_entry_wq); + if (rmnet_shs_stamp) { + rmnet_shs_stamp(skb, &port->shs_cfg); + rcu_read_unlock(); + return; + } + rcu_read_unlock(); + + if (ctx == RMNET_NET_RX_CTX) + netif_receive_skb(skb); + else + gro_cells_receive(&priv->gro_cells, skb); +} +EXPORT_SYMBOL(rmnet_deliver_skb_wq); + +/* Deliver a list of skbs after undoing coalescing */ +static void rmnet_deliver_skb_list(struct sk_buff_head *head, + struct rmnet_port *port) +{ + struct sk_buff *skb; + + while ((skb = __skb_dequeue(head))) { + rmnet_set_skb_proto(skb); + rmnet_deliver_skb(skb, port); + } +} + +/* MAP handler */ + +static void +__rmnet_map_ingress_handler(struct sk_buff *skb, + struct rmnet_port *port) +{ + struct rmnet_map_header *qmap; + struct rmnet_endpoint *ep; + struct sk_buff_head list; + u16 len, pad; + u8 mux_id; + + /* We don't need the spinlock since only we touch this */ + __skb_queue_head_init(&list); + + qmap = (struct rmnet_map_header *)rmnet_map_data_ptr(skb); + if (qmap->cd_bit) { + qmi_rmnet_set_dl_msg_active(port); + if (port->data_format & RMNET_INGRESS_FORMAT_DL_MARKER) { + if (!rmnet_map_flow_command(skb, port, false)) + return; + } + + if (port->data_format & RMNET_FLAGS_INGRESS_MAP_COMMANDS) + return rmnet_map_command(skb, port); + + goto free_skb; + } + + mux_id = qmap->mux_id; + pad = qmap->pad_len; + len = ntohs(qmap->pkt_len) - pad; + + if (mux_id >= RMNET_MAX_LOGICAL_EP) + goto free_skb; + + ep = rmnet_get_endpoint(port, mux_id); + if (!ep) + goto free_skb; + + skb->dev = ep->egress_dev; + + /* Handle QMAPv5 packet */ + if (qmap->next_hdr && + (port->data_format & (RMNET_FLAGS_INGRESS_COALESCE | + RMNET_FLAGS_INGRESS_MAP_CKSUMV5))) { + if (rmnet_map_process_next_hdr_packet(skb, &list, len)) + goto free_skb; + } else { + /* We only have the main QMAP header to worry about */ + pskb_pull(skb, sizeof(*qmap)); + + rmnet_set_skb_proto(skb); + + if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) { + if (!rmnet_map_checksum_downlink_packet(skb, len + pad)) + skb->ip_summed = CHECKSUM_UNNECESSARY; + } + + pskb_trim(skb, len); + + /* Push the single packet onto the list */ + __skb_queue_tail(&list, skb); + } + + if (port->data_format & RMNET_INGRESS_FORMAT_PS) + qmi_rmnet_work_maybe_restart(port); + + rmnet_deliver_skb_list(&list, port); + return; + +free_skb: + kfree_skb(skb); +} + +int (*rmnet_perf_deag_entry)(struct sk_buff *skb, + struct rmnet_port *port) __rcu __read_mostly; +EXPORT_SYMBOL(rmnet_perf_deag_entry); + +static void +rmnet_map_ingress_handler(struct sk_buff *skb, + struct rmnet_port *port) +{ + struct sk_buff *skbn; + int (*rmnet_perf_core_deaggregate)(struct sk_buff *skb, + struct rmnet_port *port); + + if (skb->dev->type == ARPHRD_ETHER) { + if (pskb_expand_head(skb, ETH_HLEN, 0, GFP_ATOMIC)) { + kfree_skb(skb); + return; + } + + skb_push(skb, ETH_HLEN); + } + + if (port->data_format & (RMNET_FLAGS_INGRESS_COALESCE | + RMNET_FLAGS_INGRESS_MAP_CKSUMV5)) { + if (skb_is_nonlinear(skb)) { + rmnet_frag_ingress_handler(skb, port); + return; + } + } + + /* No aggregation. Pass the frame on as is */ + if (!(port->data_format & RMNET_FLAGS_INGRESS_DEAGGREGATION)) { + __rmnet_map_ingress_handler(skb, port); + return; + } + + /* Pass off handling to rmnet_perf module, if present */ + rcu_read_lock(); + rmnet_perf_core_deaggregate = rcu_dereference(rmnet_perf_deag_entry); + if (rmnet_perf_core_deaggregate) { + rmnet_perf_core_deaggregate(skb, port); + rcu_read_unlock(); + return; + } + rcu_read_unlock(); + + /* Deaggregation and freeing of HW originating + * buffers is done within here + */ + while (skb) { + struct sk_buff *skb_frag = skb_shinfo(skb)->frag_list; + + skb_shinfo(skb)->frag_list = NULL; + while ((skbn = rmnet_map_deaggregate(skb, port)) != NULL) { + __rmnet_map_ingress_handler(skbn, port); + + if (skbn == skb) + goto next_skb; + } + + consume_skb(skb); +next_skb: + skb = skb_frag; + } +} + +static int rmnet_map_egress_handler(struct sk_buff *skb, + struct rmnet_port *port, u8 mux_id, + struct net_device *orig_dev) +{ + int required_headroom, additional_header_len, csum_type; + struct rmnet_map_header *map_header; + + additional_header_len = 0; + required_headroom = sizeof(struct rmnet_map_header); + csum_type = 0; + + if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV4) { + additional_header_len = sizeof(struct rmnet_map_ul_csum_header); + csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV4; + } else if ((port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5) || + (port->data_format & RMNET_EGRESS_FORMAT_PRIORITY)) { + additional_header_len = sizeof(struct rmnet_map_v5_csum_header); + csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV5; + } + + required_headroom += additional_header_len; + + if (skb_headroom(skb) < required_headroom) { + if (pskb_expand_head(skb, required_headroom, 0, GFP_ATOMIC)) + return -ENOMEM; + } + + if (port->data_format & RMNET_INGRESS_FORMAT_PS) + qmi_rmnet_work_maybe_restart(port); + + if (csum_type) + rmnet_map_checksum_uplink_packet(skb, port, orig_dev, + csum_type); + + map_header = rmnet_map_add_map_header(skb, additional_header_len, 0, + port); + if (!map_header) + return -ENOMEM; + + map_header->mux_id = mux_id; + + if (port->data_format & RMNET_EGRESS_FORMAT_AGGREGATION) { + if (rmnet_map_tx_agg_skip(skb, required_headroom)) + goto done; + + rmnet_map_tx_aggregate(skb, port); + return -EINPROGRESS; + } + +done: + skb->protocol = htons(ETH_P_MAP); + return 0; +} + +static void +rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev) +{ + if (bridge_dev) { + skb->dev = bridge_dev; + dev_queue_xmit(skb); + } +} + +/* Ingress / Egress Entry Points */ + +/* Processes packet as per ingress data format for receiving device. Logical + * endpoint is determined from packet inspection. Packet is then sent to the + * egress device listed in the logical endpoint configuration. + */ +rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb) +{ + struct sk_buff *skb = *pskb; + struct rmnet_port *port; + struct net_device *dev; + int (*rmnet_core_shs_switch)(struct sk_buff *skb, + struct rmnet_shs_clnt_s *cfg); + + if (!skb) + goto done; + + if (skb->pkt_type == PACKET_LOOPBACK) + return RX_HANDLER_PASS; + + trace_rmnet_low(RMNET_MODULE, RMNET_RCV_FROM_PND, 0xDEF, + 0xDEF, 0xDEF, 0xDEF, NULL, NULL); + dev = skb->dev; + port = rmnet_get_port(dev); + if (unlikely(!port)) { + atomic_long_inc(&skb->dev->rx_nohandler); + kfree_skb(skb); + goto done; + } + + switch (port->rmnet_mode) { + case RMNET_EPMODE_VND: + + rcu_read_lock(); + rmnet_core_shs_switch = rcu_dereference(rmnet_shs_switch); + if (rmnet_core_shs_switch && !skb->cb[1]) { + skb->cb[1] = 1; + rmnet_core_shs_switch(skb, &port->phy_shs_cfg); + rcu_read_unlock(); + return RX_HANDLER_CONSUMED; + } + rcu_read_unlock(); + + rmnet_map_ingress_handler(skb, port); + break; + case RMNET_EPMODE_BRIDGE: + rmnet_bridge_handler(skb, port->bridge_ep); + break; + } + +done: + return RX_HANDLER_CONSUMED; +} +EXPORT_SYMBOL(rmnet_rx_handler); + +/* Modifies packet as per logical endpoint configuration and egress data format + * for egress device configured in logical endpoint. Packet is then transmitted + * on the egress device. + */ +void rmnet_egress_handler(struct sk_buff *skb) +{ + struct net_device *orig_dev; + struct rmnet_port *port; + struct rmnet_priv *priv; + u8 mux_id; + int err; + u32 skb_len; + + trace_rmnet_low(RMNET_MODULE, RMNET_TX_UL_PKT, 0xDEF, 0xDEF, 0xDEF, + 0xDEF, (void *)skb, NULL); + sk_pacing_shift_update(skb->sk, 8); + + orig_dev = skb->dev; + priv = netdev_priv(orig_dev); + skb->dev = priv->real_dev; + mux_id = priv->mux_id; + + port = rmnet_get_port(skb->dev); + if (!port) + goto drop; + + skb_len = skb->len; + err = rmnet_map_egress_handler(skb, port, mux_id, orig_dev); + if (err == -ENOMEM) { + goto drop; + } else if (err == -EINPROGRESS) { + rmnet_vnd_tx_fixup(orig_dev, skb_len); + return; + } + + rmnet_vnd_tx_fixup(orig_dev, skb_len); + + dev_queue_xmit(skb); + return; + +drop: + this_cpu_inc(priv->pcpu_stats->stats.tx_drops); + kfree_skb(skb); +} diff --git a/techpack/datarmnet/core/rmnet_handlers.h b/techpack/datarmnet/core/rmnet_handlers.h new file mode 100644 index 000000000000..4cc4cff2de72 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_handlers.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2013, 2016-2017, 2019-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Data ingress/egress handler + * + */ + +#ifndef _RMNET_HANDLERS_H_ +#define _RMNET_HANDLERS_H_ + +#include "rmnet_config.h" + +enum rmnet_packet_context { + RMNET_NET_RX_CTX, + RMNET_WQ_CTX, +}; + +void rmnet_egress_handler(struct sk_buff *skb); +void rmnet_deliver_skb(struct sk_buff *skb, struct rmnet_port *port); +void rmnet_deliver_skb_wq(struct sk_buff *skb, struct rmnet_port *port, + enum rmnet_packet_context ctx); +void rmnet_set_skb_proto(struct sk_buff *skb); +bool rmnet_slow_start_on(u32 hash_key); +rx_handler_result_t _rmnet_map_ingress_handler(struct sk_buff *skb, + struct rmnet_port *port); +rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb); + +#endif /* _RMNET_HANDLERS_H_ */ diff --git a/techpack/datarmnet/core/rmnet_map.h b/techpack/datarmnet/core/rmnet_map.h new file mode 100644 index 000000000000..853318da8f2c --- /dev/null +++ b/techpack/datarmnet/core/rmnet_map.h @@ -0,0 +1,283 @@ +/* Copyright (c) 2013-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _RMNET_MAP_H_ +#define _RMNET_MAP_H_ + +#include +#include "rmnet_config.h" + +struct rmnet_map_control_command { + u8 command_name; + u8 cmd_type:2; + u8 reserved:6; + u16 reserved2; + u32 transaction_id; + union { + struct { + u16 ip_family:2; + u16 reserved:14; + __be16 flow_control_seq_num; + __be32 qos_id; + } flow_control; + u8 data[0]; + }; +} __aligned(1); + +enum rmnet_map_commands { + RMNET_MAP_COMMAND_NONE, + RMNET_MAP_COMMAND_FLOW_DISABLE, + RMNET_MAP_COMMAND_FLOW_ENABLE, + RMNET_MAP_COMMAND_FLOW_START = 7, + RMNET_MAP_COMMAND_FLOW_END = 8, + /* These should always be the last 2 elements */ + RMNET_MAP_COMMAND_UNKNOWN, + RMNET_MAP_COMMAND_ENUM_LENGTH +}; + +enum rmnet_map_v5_header_type { + RMNET_MAP_HEADER_TYPE_UNKNOWN, + RMNET_MAP_HEADER_TYPE_COALESCING = 0x1, + RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD = 0x2, + RMNET_MAP_HEADER_TYPE_ENUM_LENGTH +}; + +enum rmnet_map_v5_close_type { + RMNET_MAP_COAL_CLOSE_NON_COAL, + RMNET_MAP_COAL_CLOSE_IP_MISS, + RMNET_MAP_COAL_CLOSE_TRANS_MISS, + RMNET_MAP_COAL_CLOSE_HW, + RMNET_MAP_COAL_CLOSE_COAL, +}; + +enum rmnet_map_v5_close_value { + RMNET_MAP_COAL_CLOSE_HW_NL, + RMNET_MAP_COAL_CLOSE_HW_PKT, + RMNET_MAP_COAL_CLOSE_HW_BYTE, + RMNET_MAP_COAL_CLOSE_HW_TIME, + RMNET_MAP_COAL_CLOSE_HW_EVICT, +}; + +/* Main QMAP header */ +struct rmnet_map_header { + u8 pad_len:6; + u8 next_hdr:1; + u8 cd_bit:1; + u8 mux_id; + __be16 pkt_len; +} __aligned(1); + +/* QMAP v5 headers */ +struct rmnet_map_v5_csum_header { + u8 next_hdr:1; + u8 header_type:7; + u8 hw_reserved:5; + u8 priority:1; + u8 hw_reserved_bit:1; + u8 csum_valid_required:1; + __be16 reserved; +} __aligned(1); + +struct rmnet_map_v5_nl_pair { + __be16 pkt_len; + u8 csum_error_bitmap; + u8 num_packets; +} __aligned(1); + +/* NLO: Number-length object */ +#define RMNET_MAP_V5_MAX_NLOS (6) +#define RMNET_MAP_V5_MAX_PACKETS (48) + +struct rmnet_map_v5_coal_header { + u8 next_hdr:1; + u8 header_type:7; + u8 reserved1:4; + u8 num_nlos:3; + u8 csum_valid:1; + u8 close_type:4; + u8 close_value:4; + u8 reserved2:4; + u8 virtual_channel_id:4; + + struct rmnet_map_v5_nl_pair nl_pairs[RMNET_MAP_V5_MAX_NLOS]; +} __aligned(1); + +/* QMAP v4 headers */ +struct rmnet_map_dl_csum_trailer { + u8 reserved1; + u8 valid:1; + u8 reserved2:7; + u16 csum_start_offset; + u16 csum_length; + __be16 csum_value; +} __aligned(1); + +struct rmnet_map_ul_csum_header { + __be16 csum_start_offset; + u16 csum_insert_offset:14; + u16 udp_ind:1; + u16 csum_enabled:1; +} __aligned(1); + +struct rmnet_map_control_command_header { + u8 command_name; + u8 cmd_type:2; + u8 reserved:5; + u8 e:1; + u16 source_id:15; + u16 ext:1; + u32 transaction_id; +} __aligned(1); + +struct rmnet_map_flow_info_le { + __be32 mux_id; + __be32 flow_id; + __be32 bytes; + __be32 pkts; +} __aligned(1); + +struct rmnet_map_flow_info_be { + u32 mux_id; + u32 flow_id; + u32 bytes; + u32 pkts; +} __aligned(1); + +struct rmnet_map_dl_ind_hdr { + union { + struct { + u32 seq; + u32 bytes; + u32 pkts; + u32 flows; + struct rmnet_map_flow_info_le flow[0]; + } le __aligned(1); + struct { + __be32 seq; + __be32 bytes; + __be32 pkts; + __be32 flows; + struct rmnet_map_flow_info_be flow[0]; + } be __aligned(1); + } __aligned(1); +} __aligned(1); + +struct rmnet_map_dl_ind_trl { + union { + __be32 seq_be; + u32 seq_le; + } __aligned(1); +} __aligned(1); + +struct rmnet_map_dl_ind { + u8 priority; + void (*dl_hdr_handler_v2)(struct rmnet_map_dl_ind_hdr *dlhdr, + struct rmnet_map_control_command_header *qcmd); + void (*dl_trl_handler_v2)(struct rmnet_map_dl_ind_trl *dltrl, + struct rmnet_map_control_command_header *qcmd); + struct list_head list; +}; + +#define RMNET_MAP_GET_MUX_ID(Y) (((struct rmnet_map_header *) \ + (Y)->data)->mux_id) +#define RMNET_MAP_GET_CD_BIT(Y) (((struct rmnet_map_header *) \ + (Y)->data)->cd_bit) +#define RMNET_MAP_GET_PAD(Y) (((struct rmnet_map_header *) \ + (Y)->data)->pad_len) +#define RMNET_MAP_GET_CMD_START(Y) ((struct rmnet_map_control_command *) \ + ((Y)->data + \ + sizeof(struct rmnet_map_header))) +#define RMNET_MAP_GET_LENGTH(Y) (ntohs(((struct rmnet_map_header *) \ + (Y)->data)->pkt_len)) + +#define RMNET_MAP_DEAGGR_SPACING 64 +#define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2) + +#define RMNET_MAP_COMMAND_REQUEST 0 +#define RMNET_MAP_COMMAND_ACK 1 +#define RMNET_MAP_COMMAND_UNSUPPORTED 2 +#define RMNET_MAP_COMMAND_INVALID 3 + +#define RMNET_MAP_NO_PAD_BYTES 0 +#define RMNET_MAP_ADD_PAD_BYTES 1 + +static inline unsigned char *rmnet_map_data_ptr(struct sk_buff *skb) +{ + /* Nonlinear packets we receive are entirely within frag 0 */ + if (skb_is_nonlinear(skb) && skb->len == skb->data_len) + return skb_frag_address(skb_shinfo(skb)->frags); + + return skb->data; +} + +static inline struct rmnet_map_control_command * +rmnet_map_get_cmd_start(struct sk_buff *skb) +{ + unsigned char *data = rmnet_map_data_ptr(skb); + + data += sizeof(struct rmnet_map_header); + return (struct rmnet_map_control_command *)data; +} + +static inline u8 rmnet_map_get_next_hdr_type(struct sk_buff *skb) +{ + unsigned char *data = rmnet_map_data_ptr(skb); + + data += sizeof(struct rmnet_map_header); + return ((struct rmnet_map_v5_coal_header *)data)->header_type; +} + +static inline bool rmnet_map_get_csum_valid(struct sk_buff *skb) +{ + unsigned char *data = rmnet_map_data_ptr(skb); + + data += sizeof(struct rmnet_map_header); + return ((struct rmnet_map_v5_csum_header *)data)->csum_valid_required; +} + +struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb, + struct rmnet_port *port); +struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb, + int hdrlen, int pad, + struct rmnet_port *port); +void rmnet_map_command(struct sk_buff *skb, struct rmnet_port *port); +int rmnet_map_checksum_downlink_packet(struct sk_buff *skb, u16 len); +void rmnet_map_checksum_uplink_packet(struct sk_buff *skb, + struct rmnet_port *port, + struct net_device *orig_dev, + int csum_type); +bool rmnet_map_v5_csum_buggy(struct rmnet_map_v5_coal_header *coal_hdr); +int rmnet_map_process_next_hdr_packet(struct sk_buff *skb, + struct sk_buff_head *list, + u16 len); +int rmnet_map_tx_agg_skip(struct sk_buff *skb, int offset); +void rmnet_map_tx_aggregate(struct sk_buff *skb, struct rmnet_port *port); +void rmnet_map_tx_aggregate_init(struct rmnet_port *port); +void rmnet_map_tx_aggregate_exit(struct rmnet_port *port); +void rmnet_map_update_ul_agg_config(struct rmnet_port *port, u16 size, + u8 count, u8 features, u32 time); +void rmnet_map_dl_hdr_notify_v2(struct rmnet_port *port, + struct rmnet_map_dl_ind_hdr *dl_hdr, + struct rmnet_map_control_command_header *qcmd); +void rmnet_map_dl_trl_notify_v2(struct rmnet_port *port, + struct rmnet_map_dl_ind_trl *dltrl, + struct rmnet_map_control_command_header *qcmd); +int rmnet_map_flow_command(struct sk_buff *skb, + struct rmnet_port *port, + bool rmnet_perf); +void rmnet_map_cmd_init(struct rmnet_port *port); +int rmnet_map_dl_ind_register(struct rmnet_port *port, + struct rmnet_map_dl_ind *dl_ind); +int rmnet_map_dl_ind_deregister(struct rmnet_port *port, + struct rmnet_map_dl_ind *dl_ind); +void rmnet_map_cmd_exit(struct rmnet_port *port); +#endif /* _RMNET_MAP_H_ */ diff --git a/techpack/datarmnet/core/rmnet_map_command.c b/techpack/datarmnet/core/rmnet_map_command.c new file mode 100644 index 000000000000..be60422c37a9 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_map_command.c @@ -0,0 +1,338 @@ +/* Copyright (c) 2013-2019, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include "rmnet_config.h" +#include "rmnet_map.h" +#include "rmnet_private.h" +#include "rmnet_vnd.h" + +#define RMNET_DL_IND_HDR_SIZE (sizeof(struct rmnet_map_dl_ind_hdr) + \ + sizeof(struct rmnet_map_header) + \ + sizeof(struct rmnet_map_control_command_header)) + +#define RMNET_MAP_CMD_SIZE (sizeof(struct rmnet_map_header) + \ + sizeof(struct rmnet_map_control_command_header)) + +#define RMNET_DL_IND_TRL_SIZE (sizeof(struct rmnet_map_dl_ind_trl) + \ + sizeof(struct rmnet_map_header) + \ + sizeof(struct rmnet_map_control_command_header)) + +static u8 rmnet_map_do_flow_control(struct sk_buff *skb, + struct rmnet_port *port, + int enable) +{ + struct rmnet_map_header *qmap; + struct rmnet_map_control_command *cmd; + struct rmnet_endpoint *ep; + struct net_device *vnd; + u16 ip_family; + u16 fc_seq; + u32 qos_id; + u8 mux_id; + int r; + + qmap = (struct rmnet_map_header *)rmnet_map_data_ptr(skb); + mux_id = qmap->mux_id; + cmd = rmnet_map_get_cmd_start(skb); + + if (mux_id >= RMNET_MAX_LOGICAL_EP) { + kfree_skb(skb); + return RX_HANDLER_CONSUMED; + } + + ep = rmnet_get_endpoint(port, mux_id); + if (!ep) { + kfree_skb(skb); + return RX_HANDLER_CONSUMED; + } + + vnd = ep->egress_dev; + + ip_family = cmd->flow_control.ip_family; + fc_seq = ntohs(cmd->flow_control.flow_control_seq_num); + qos_id = ntohl(cmd->flow_control.qos_id); + + /* Ignore the ip family and pass the sequence number for both v4 and v6 + * sequence. User space does not support creating dedicated flows for + * the 2 protocols + */ + r = rmnet_vnd_do_flow_control(vnd, enable); + if (r) { + kfree_skb(skb); + return RMNET_MAP_COMMAND_UNSUPPORTED; + } else { + return RMNET_MAP_COMMAND_ACK; + } +} + +static void rmnet_map_send_ack(struct sk_buff *skb, + unsigned char type, + struct rmnet_port *port) +{ + struct rmnet_map_control_command *cmd; + struct net_device *dev = skb->dev; + + if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) + pskb_trim(skb, + skb->len - sizeof(struct rmnet_map_dl_csum_trailer)); + + skb->protocol = htons(ETH_P_MAP); + + cmd = rmnet_map_get_cmd_start(skb); + cmd->cmd_type = type & 0x03; + + netif_tx_lock(dev); + dev->netdev_ops->ndo_start_xmit(skb, dev); + netif_tx_unlock(dev); +} + +void +rmnet_map_dl_hdr_notify_v2(struct rmnet_port *port, + struct rmnet_map_dl_ind_hdr *dlhdr, + struct rmnet_map_control_command_header *qcmd) +{ + struct rmnet_map_dl_ind *tmp; + + list_for_each_entry(tmp, &port->dl_list, list) + tmp->dl_hdr_handler_v2(dlhdr, qcmd); +} + +void +rmnet_map_dl_trl_notify_v2(struct rmnet_port *port, + struct rmnet_map_dl_ind_trl *dltrl, + struct rmnet_map_control_command_header *qcmd) +{ + struct rmnet_map_dl_ind *tmp; + + list_for_each_entry(tmp, &port->dl_list, list) + tmp->dl_trl_handler_v2(dltrl, qcmd); +} + +static void rmnet_map_process_flow_start(struct sk_buff *skb, + struct rmnet_port *port, + bool rmnet_perf) +{ + struct rmnet_map_dl_ind_hdr *dlhdr; + struct rmnet_map_control_command_header *qcmd; + u32 data_format; + bool is_dl_mark_v2; + + if (skb->len < RMNET_DL_IND_HDR_SIZE) + return; + + data_format = port->data_format; + is_dl_mark_v2 = data_format & RMNET_INGRESS_FORMAT_DL_MARKER_V2; + if (is_dl_mark_v2) { + pskb_pull(skb, sizeof(struct rmnet_map_header)); + qcmd = (struct rmnet_map_control_command_header *) + rmnet_map_data_ptr(skb); + port->stats.dl_hdr_last_ep_id = qcmd->source_id; + port->stats.dl_hdr_last_qmap_vers = qcmd->reserved; + port->stats.dl_hdr_last_trans_id = qcmd->transaction_id; + pskb_pull(skb, sizeof(struct rmnet_map_control_command_header)); + } + + dlhdr = (struct rmnet_map_dl_ind_hdr *)rmnet_map_data_ptr(skb); + + port->stats.dl_hdr_last_seq = dlhdr->le.seq; + port->stats.dl_hdr_last_bytes = dlhdr->le.bytes; + port->stats.dl_hdr_last_pkts = dlhdr->le.pkts; + port->stats.dl_hdr_last_flows = dlhdr->le.flows; + port->stats.dl_hdr_total_bytes += port->stats.dl_hdr_last_bytes; + port->stats.dl_hdr_total_pkts += port->stats.dl_hdr_last_pkts; + port->stats.dl_hdr_count++; + + if (is_dl_mark_v2) + rmnet_map_dl_hdr_notify_v2(port, dlhdr, qcmd); + + if (rmnet_perf) { + unsigned int pull_size; + + pull_size = sizeof(struct rmnet_map_dl_ind_hdr); + if (data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) + pull_size += sizeof(struct rmnet_map_dl_csum_trailer); + pskb_pull(skb, pull_size); + } +} + +static void rmnet_map_process_flow_end(struct sk_buff *skb, + struct rmnet_port *port, + bool rmnet_perf) +{ + struct rmnet_map_dl_ind_trl *dltrl; + struct rmnet_map_control_command_header *qcmd; + u32 data_format; + bool is_dl_mark_v2; + + if (skb->len < RMNET_DL_IND_TRL_SIZE) + return; + + data_format = port->data_format; + is_dl_mark_v2 = data_format & RMNET_INGRESS_FORMAT_DL_MARKER_V2; + if (is_dl_mark_v2) { + pskb_pull(skb, sizeof(struct rmnet_map_header)); + qcmd = (struct rmnet_map_control_command_header *) + rmnet_map_data_ptr(skb); + pskb_pull(skb, sizeof(struct rmnet_map_control_command_header)); + } + + dltrl = (struct rmnet_map_dl_ind_trl *)rmnet_map_data_ptr(skb); + + port->stats.dl_trl_last_seq = dltrl->seq_le; + port->stats.dl_trl_count++; + + if (is_dl_mark_v2) + rmnet_map_dl_trl_notify_v2(port, dltrl, qcmd); + + if (rmnet_perf) { + unsigned int pull_size; + + pull_size = sizeof(struct rmnet_map_dl_ind_trl); + if (data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) + pull_size += sizeof(struct rmnet_map_dl_csum_trailer); + pskb_pull(skb, pull_size); + } +} + +/* Process MAP command frame and send N/ACK message as appropriate. Message cmd + * name is decoded here and appropriate handler is called. + */ +void rmnet_map_command(struct sk_buff *skb, struct rmnet_port *port) +{ + struct rmnet_map_control_command *cmd; + unsigned char command_name; + unsigned char rc = 0; + + cmd = rmnet_map_get_cmd_start(skb); + command_name = cmd->command_name; + + switch (command_name) { + case RMNET_MAP_COMMAND_FLOW_ENABLE: + rc = rmnet_map_do_flow_control(skb, port, 1); + break; + + case RMNET_MAP_COMMAND_FLOW_DISABLE: + rc = rmnet_map_do_flow_control(skb, port, 0); + break; + + default: + rc = RMNET_MAP_COMMAND_UNSUPPORTED; + kfree_skb(skb); + break; + } + if (rc == RMNET_MAP_COMMAND_ACK) + rmnet_map_send_ack(skb, rc, port); +} + +int rmnet_map_flow_command(struct sk_buff *skb, struct rmnet_port *port, + bool rmnet_perf) +{ + struct rmnet_map_control_command *cmd; + unsigned char command_name; + + cmd = rmnet_map_get_cmd_start(skb); + command_name = cmd->command_name; + + switch (command_name) { + case RMNET_MAP_COMMAND_FLOW_START: + rmnet_map_process_flow_start(skb, port, rmnet_perf); + break; + + case RMNET_MAP_COMMAND_FLOW_END: + rmnet_map_process_flow_end(skb, port, rmnet_perf); + break; + + default: + return 1; + } + + /* rmnet_perf module will handle the consuming */ + if (!rmnet_perf) + consume_skb(skb); + + return 0; +} +EXPORT_SYMBOL(rmnet_map_flow_command); + +void rmnet_map_cmd_exit(struct rmnet_port *port) +{ + struct rmnet_map_dl_ind *tmp, *idx; + + list_for_each_entry_safe(tmp, idx, &port->dl_list, list) + list_del_rcu(&tmp->list); +} + +void rmnet_map_cmd_init(struct rmnet_port *port) +{ + INIT_LIST_HEAD(&port->dl_list); +} + +int rmnet_map_dl_ind_register(struct rmnet_port *port, + struct rmnet_map_dl_ind *dl_ind) +{ + struct rmnet_map_dl_ind *dl_ind_iterator; + bool empty_ind_list = true; + + if (!port || !dl_ind || !dl_ind->dl_hdr_handler_v2 || + !dl_ind->dl_trl_handler_v2) + return -EINVAL; + + list_for_each_entry_rcu(dl_ind_iterator, &port->dl_list, list) { + empty_ind_list = false; + if (dl_ind_iterator->priority < dl_ind->priority) { + if (dl_ind_iterator->list.next) { + if (dl_ind->priority + < list_entry_rcu(dl_ind_iterator->list.next, + typeof(*dl_ind_iterator), list)->priority) { + list_add_rcu(&dl_ind->list, + &dl_ind_iterator->list); + break; + } + } else { + list_add_rcu(&dl_ind->list, + &dl_ind_iterator->list); + break; + } + } else { + list_add_tail_rcu(&dl_ind->list, + &dl_ind_iterator->list); + break; + } + } + + if (empty_ind_list) + list_add_rcu(&dl_ind->list, &port->dl_list); + + return 0; +} +EXPORT_SYMBOL(rmnet_map_dl_ind_register); + +int rmnet_map_dl_ind_deregister(struct rmnet_port *port, + struct rmnet_map_dl_ind *dl_ind) +{ + struct rmnet_map_dl_ind *tmp; + + if (!port || !dl_ind) + return -EINVAL; + + list_for_each_entry(tmp, &port->dl_list, list) { + if (tmp == dl_ind) { + list_del_rcu(&dl_ind->list); + goto done; + } + } + +done: + return 0; +} +EXPORT_SYMBOL(rmnet_map_dl_ind_deregister); diff --git a/techpack/datarmnet/core/rmnet_map_data.c b/techpack/datarmnet/core/rmnet_map_data.c new file mode 100644 index 000000000000..8388ce31c446 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_map_data.c @@ -0,0 +1,1623 @@ +/* Copyright (c) 2013-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Data MAP protocol + * + */ + +#include +#include +#include +#include +#include "rmnet_config.h" +#include "rmnet_map.h" +#include "rmnet_private.h" +#include "rmnet_handlers.h" + +#define RMNET_MAP_PKT_COPY_THRESHOLD 64 +#define RMNET_MAP_DEAGGR_SPACING 64 +#define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2) + +struct rmnet_map_coal_metadata { + void *ip_header; + void *trans_header; + u16 ip_len; + u16 trans_len; + u16 data_offset; + u16 data_len; + u8 ip_proto; + u8 trans_proto; + u8 pkt_id; + u8 pkt_count; +}; + +static __sum16 *rmnet_map_get_csum_field(unsigned char protocol, + const void *txporthdr) +{ + __sum16 *check = NULL; + + switch (protocol) { + case IPPROTO_TCP: + check = &(((struct tcphdr *)txporthdr)->check); + break; + + case IPPROTO_UDP: + check = &(((struct udphdr *)txporthdr)->check); + break; + + default: + check = NULL; + break; + } + + return check; +} + +static int +rmnet_map_ipv4_dl_csum_trailer(struct sk_buff *skb, + struct rmnet_map_dl_csum_trailer *csum_trailer, + struct rmnet_priv *priv) +{ + __sum16 *csum_field, csum_temp, pseudo_csum, hdr_csum, ip_payload_csum; + u16 csum_value, csum_value_final; + struct iphdr *ip4h; + void *txporthdr; + __be16 addend; + + ip4h = (struct iphdr *)rmnet_map_data_ptr(skb); + if ((ntohs(ip4h->frag_off) & IP_MF) || + ((ntohs(ip4h->frag_off) & IP_OFFSET) > 0)) { + priv->stats.csum_fragmented_pkt++; + return -EOPNOTSUPP; + } + + txporthdr = rmnet_map_data_ptr(skb) + ip4h->ihl * 4; + + csum_field = rmnet_map_get_csum_field(ip4h->protocol, txporthdr); + + if (!csum_field) { + priv->stats.csum_err_invalid_transport++; + return -EPROTONOSUPPORT; + } + + /* RFC 768 - Skip IPv4 UDP packets where sender checksum field is 0 */ + if (*csum_field == 0 && ip4h->protocol == IPPROTO_UDP) { + priv->stats.csum_skipped++; + return 0; + } + + csum_value = ~ntohs(csum_trailer->csum_value); + hdr_csum = ~ip_fast_csum(ip4h, (int)ip4h->ihl); + ip_payload_csum = csum16_sub((__force __sum16)csum_value, + (__force __be16)hdr_csum); + + pseudo_csum = ~csum_tcpudp_magic(ip4h->saddr, ip4h->daddr, + ntohs(ip4h->tot_len) - ip4h->ihl * 4, + ip4h->protocol, 0); + addend = (__force __be16)ntohs((__force __be16)pseudo_csum); + pseudo_csum = csum16_add(ip_payload_csum, addend); + + addend = (__force __be16)ntohs((__force __be16)*csum_field); + csum_temp = ~csum16_sub(pseudo_csum, addend); + csum_value_final = (__force u16)csum_temp; + + if (unlikely(csum_value_final == 0)) { + switch (ip4h->protocol) { + case IPPROTO_UDP: + /* RFC 768 - DL4 1's complement rule for UDP csum 0 */ + csum_value_final = ~csum_value_final; + break; + + case IPPROTO_TCP: + /* DL4 Non-RFC compliant TCP checksum found */ + if (*csum_field == (__force __sum16)0xFFFF) + csum_value_final = ~csum_value_final; + break; + } + } + + if (csum_value_final == ntohs((__force __be16)*csum_field)) { + priv->stats.csum_ok++; + return 0; + } else { + priv->stats.csum_validation_failed++; + return -EINVAL; + } +} + +#if IS_ENABLED(CONFIG_IPV6) +static int +rmnet_map_ipv6_dl_csum_trailer(struct sk_buff *skb, + struct rmnet_map_dl_csum_trailer *csum_trailer, + struct rmnet_priv *priv) +{ + __sum16 *csum_field, ip6_payload_csum, pseudo_csum, csum_temp; + u16 csum_value, csum_value_final; + __be16 ip6_hdr_csum, addend; + struct ipv6hdr *ip6h; + void *txporthdr, *data = rmnet_map_data_ptr(skb); + u32 length; + + ip6h = data; + + txporthdr = data + sizeof(struct ipv6hdr); + csum_field = rmnet_map_get_csum_field(ip6h->nexthdr, txporthdr); + + if (!csum_field) { + priv->stats.csum_err_invalid_transport++; + return -EPROTONOSUPPORT; + } + + csum_value = ~ntohs(csum_trailer->csum_value); + ip6_hdr_csum = (__force __be16) + ~ntohs((__force __be16)ip_compute_csum(ip6h, + (int)(txporthdr - data))); + ip6_payload_csum = csum16_sub((__force __sum16)csum_value, + ip6_hdr_csum); + + length = (ip6h->nexthdr == IPPROTO_UDP) ? + ntohs(((struct udphdr *)txporthdr)->len) : + ntohs(ip6h->payload_len); + pseudo_csum = ~(csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + length, ip6h->nexthdr, 0)); + addend = (__force __be16)ntohs((__force __be16)pseudo_csum); + pseudo_csum = csum16_add(ip6_payload_csum, addend); + + addend = (__force __be16)ntohs((__force __be16)*csum_field); + csum_temp = ~csum16_sub(pseudo_csum, addend); + csum_value_final = (__force u16)csum_temp; + + if (unlikely(csum_value_final == 0)) { + switch (ip6h->nexthdr) { + case IPPROTO_UDP: + /* RFC 2460 section 8.1 + * DL6 One's complement rule for UDP checksum 0 + */ + csum_value_final = ~csum_value_final; + break; + + case IPPROTO_TCP: + /* DL6 Non-RFC compliant TCP checksum found */ + if (*csum_field == (__force __sum16)0xFFFF) + csum_value_final = ~csum_value_final; + break; + } + } + + if (csum_value_final == ntohs((__force __be16)*csum_field)) { + priv->stats.csum_ok++; + return 0; + } else { + priv->stats.csum_validation_failed++; + return -EINVAL; + } +} +#endif + +static void rmnet_map_complement_ipv4_txporthdr_csum_field(void *iphdr) +{ + struct iphdr *ip4h = (struct iphdr *)iphdr; + void *txphdr; + u16 *csum; + + txphdr = iphdr + ip4h->ihl * 4; + + if (ip4h->protocol == IPPROTO_TCP || ip4h->protocol == IPPROTO_UDP) { + csum = (u16 *)rmnet_map_get_csum_field(ip4h->protocol, txphdr); + *csum = ~(*csum); + } +} + +static void +rmnet_map_ipv4_ul_csum_header(void *iphdr, + struct rmnet_map_ul_csum_header *ul_header, + struct sk_buff *skb) +{ + struct iphdr *ip4h = (struct iphdr *)iphdr; + __be16 *hdr = (__be16 *)ul_header, offset; + + offset = htons((__force u16)(skb_transport_header(skb) - + (unsigned char *)iphdr)); + ul_header->csum_start_offset = offset; + ul_header->csum_insert_offset = skb->csum_offset; + ul_header->csum_enabled = 1; + if (ip4h->protocol == IPPROTO_UDP) + ul_header->udp_ind = 1; + else + ul_header->udp_ind = 0; + + /* Changing remaining fields to network order */ + hdr++; + *hdr = htons((__force u16)*hdr); + + skb->ip_summed = CHECKSUM_NONE; + + rmnet_map_complement_ipv4_txporthdr_csum_field(iphdr); +} + +#if IS_ENABLED(CONFIG_IPV6) +static void rmnet_map_complement_ipv6_txporthdr_csum_field(void *ip6hdr) +{ + struct ipv6hdr *ip6h = (struct ipv6hdr *)ip6hdr; + void *txphdr; + u16 *csum; + + txphdr = ip6hdr + sizeof(struct ipv6hdr); + + if (ip6h->nexthdr == IPPROTO_TCP || ip6h->nexthdr == IPPROTO_UDP) { + csum = (u16 *)rmnet_map_get_csum_field(ip6h->nexthdr, txphdr); + *csum = ~(*csum); + } +} + +static void +rmnet_map_ipv6_ul_csum_header(void *ip6hdr, + struct rmnet_map_ul_csum_header *ul_header, + struct sk_buff *skb) +{ + struct ipv6hdr *ip6h = (struct ipv6hdr *)ip6hdr; + __be16 *hdr = (__be16 *)ul_header, offset; + + offset = htons((__force u16)(skb_transport_header(skb) - + (unsigned char *)ip6hdr)); + ul_header->csum_start_offset = offset; + ul_header->csum_insert_offset = skb->csum_offset; + ul_header->csum_enabled = 1; + + if (ip6h->nexthdr == IPPROTO_UDP) + ul_header->udp_ind = 1; + else + ul_header->udp_ind = 0; + + /* Changing remaining fields to network order */ + hdr++; + *hdr = htons((__force u16)*hdr); + + skb->ip_summed = CHECKSUM_NONE; + + rmnet_map_complement_ipv6_txporthdr_csum_field(ip6hdr); +} +#endif + +/* Adds MAP header to front of skb->data + * Padding is calculated and set appropriately in MAP header. Mux ID is + * initialized to 0. + */ +struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb, + int hdrlen, int pad, + struct rmnet_port *port) +{ + struct rmnet_map_header *map_header; + u32 padding, map_datalen; + u8 *padbytes; + + map_datalen = skb->len - hdrlen; + map_header = (struct rmnet_map_header *) + skb_push(skb, sizeof(struct rmnet_map_header)); + memset(map_header, 0, sizeof(struct rmnet_map_header)); + + /* Set next_hdr bit for csum offload packets */ + if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5) + map_header->next_hdr = 1; + + if (pad == RMNET_MAP_NO_PAD_BYTES) { + map_header->pkt_len = htons(map_datalen); + return map_header; + } + + padding = ALIGN(map_datalen, 4) - map_datalen; + + if (padding == 0) + goto done; + + if (skb_tailroom(skb) < padding) + return NULL; + + padbytes = (u8 *)skb_put(skb, padding); + memset(padbytes, 0, padding); + +done: + map_header->pkt_len = htons(map_datalen + padding); + map_header->pad_len = padding & 0x3F; + + return map_header; +} + +/* Deaggregates a single packet + * A whole new buffer is allocated for each portion of an aggregated frame. + * Caller should keep calling deaggregate() on the source skb until 0 is + * returned, indicating that there are no more packets to deaggregate. Caller + * is responsible for freeing the original skb. + */ +struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb, + struct rmnet_port *port) +{ + struct rmnet_map_header *maph; + struct sk_buff *skbn; + unsigned char *data = rmnet_map_data_ptr(skb), *next_hdr = NULL; + u32 packet_len; + + if (skb->len == 0) + return NULL; + + maph = (struct rmnet_map_header *)data; + packet_len = ntohs(maph->pkt_len) + sizeof(struct rmnet_map_header); + + if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) + packet_len += sizeof(struct rmnet_map_dl_csum_trailer); + else if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV5) { + if (!maph->cd_bit) { + packet_len += sizeof(struct rmnet_map_v5_csum_header); + + /* Coalescing headers require MAPv5 */ + next_hdr = data + sizeof(*maph); + } + } + + if (((int)skb->len - (int)packet_len) < 0) + return NULL; + + /* Some hardware can send us empty frames. Catch them */ + if (ntohs(maph->pkt_len) == 0) + return NULL; + + if (next_hdr && + ((struct rmnet_map_v5_coal_header *)next_hdr)->header_type == + RMNET_MAP_HEADER_TYPE_COALESCING) + return skb; + + if (skb_is_nonlinear(skb)) { + skb_frag_t *frag0 = skb_shinfo(skb)->frags; + struct page *page = skb_frag_page(frag0); + + skbn = alloc_skb(RMNET_MAP_DEAGGR_HEADROOM, GFP_ATOMIC); + if (!skbn) + return NULL; + + skb_append_pagefrags(skbn, page, frag0->bv_offset, + packet_len); + skbn->data_len += packet_len; + skbn->len += packet_len; + } else { + skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, + GFP_ATOMIC); + if (!skbn) + return NULL; + + skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM); + skb_put(skbn, packet_len); + memcpy(skbn->data, data, packet_len); + } + + pskb_pull(skb, packet_len); + + return skbn; +} + +/* Validates packet checksums. Function takes a pointer to + * the beginning of a buffer which contains the IP payload + + * padding + checksum trailer. + * Only IPv4 and IPv6 are supported along with TCP & UDP. + * Fragmented or tunneled packets are not supported. + */ +int rmnet_map_checksum_downlink_packet(struct sk_buff *skb, u16 len) +{ + struct rmnet_priv *priv = netdev_priv(skb->dev); + struct rmnet_map_dl_csum_trailer *csum_trailer; + + if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) { + priv->stats.csum_sw++; + return -EOPNOTSUPP; + } + + csum_trailer = (struct rmnet_map_dl_csum_trailer *) + (rmnet_map_data_ptr(skb) + len); + + if (!csum_trailer->valid) { + priv->stats.csum_valid_unset++; + return -EINVAL; + } + + if (skb->protocol == htons(ETH_P_IP)) { + return rmnet_map_ipv4_dl_csum_trailer(skb, csum_trailer, priv); + } else if (skb->protocol == htons(ETH_P_IPV6)) { +#if IS_ENABLED(CONFIG_IPV6) + return rmnet_map_ipv6_dl_csum_trailer(skb, csum_trailer, priv); +#else + priv->stats.csum_err_invalid_ip_version++; + return -EPROTONOSUPPORT; +#endif + } else { + priv->stats.csum_err_invalid_ip_version++; + return -EPROTONOSUPPORT; + } + + return 0; +} +EXPORT_SYMBOL(rmnet_map_checksum_downlink_packet); + +void rmnet_map_v4_checksum_uplink_packet(struct sk_buff *skb, + struct net_device *orig_dev) +{ + struct rmnet_priv *priv = netdev_priv(orig_dev); + struct rmnet_map_ul_csum_header *ul_header; + void *iphdr; + + ul_header = (struct rmnet_map_ul_csum_header *) + skb_push(skb, sizeof(struct rmnet_map_ul_csum_header)); + + if (unlikely(!(orig_dev->features & + (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)))) + goto sw_csum; + + if (skb->ip_summed == CHECKSUM_PARTIAL) { + iphdr = (char *)ul_header + + sizeof(struct rmnet_map_ul_csum_header); + + if (skb->protocol == htons(ETH_P_IP)) { + rmnet_map_ipv4_ul_csum_header(iphdr, ul_header, skb); + priv->stats.csum_hw++; + return; + } else if (skb->protocol == htons(ETH_P_IPV6)) { +#if IS_ENABLED(CONFIG_IPV6) + rmnet_map_ipv6_ul_csum_header(iphdr, ul_header, skb); + priv->stats.csum_hw++; + return; +#else + priv->stats.csum_err_invalid_ip_version++; + goto sw_csum; +#endif + } else { + priv->stats.csum_err_invalid_ip_version++; + } + } + +sw_csum: + ul_header->csum_start_offset = 0; + ul_header->csum_insert_offset = 0; + ul_header->csum_enabled = 0; + ul_header->udp_ind = 0; + + priv->stats.csum_sw++; +} + +static void rmnet_map_v5_check_priority(struct sk_buff *skb, + struct net_device *orig_dev, + struct rmnet_map_v5_csum_header *hdr) +{ + struct rmnet_priv *priv = netdev_priv(orig_dev); + + if (skb->priority) { + priv->stats.ul_prio++; + hdr->priority = 1; + } +} + +void rmnet_map_v5_checksum_uplink_packet(struct sk_buff *skb, + struct rmnet_port *port, + struct net_device *orig_dev) +{ + struct rmnet_priv *priv = netdev_priv(orig_dev); + struct rmnet_map_v5_csum_header *ul_header; + + ul_header = (struct rmnet_map_v5_csum_header *) + skb_push(skb, sizeof(*ul_header)); + memset(ul_header, 0, sizeof(*ul_header)); + ul_header->header_type = RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD; + + if (port->data_format & RMNET_EGRESS_FORMAT_PRIORITY) + rmnet_map_v5_check_priority(skb, orig_dev, ul_header); + + /* Allow priority w/o csum offload */ + if (!(port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5)) + return; + + if (skb->ip_summed == CHECKSUM_PARTIAL) { + void *iph = (char *)ul_header + sizeof(*ul_header); + void *trans; + __sum16 *check; + u8 proto; + + if (skb->protocol == htons(ETH_P_IP)) { + u16 ip_len = ((struct iphdr *)iph)->ihl * 4; + + proto = ((struct iphdr *)iph)->protocol; + trans = iph + ip_len; + } else if (skb->protocol == htons(ETH_P_IPV6)) { + u16 ip_len = sizeof(struct ipv6hdr); + + proto = ((struct ipv6hdr *)iph)->nexthdr; + trans = iph + ip_len; + } else { + priv->stats.csum_err_invalid_ip_version++; + goto sw_csum; + } + + check = rmnet_map_get_csum_field(proto, trans); + if (check) { + skb->ip_summed = CHECKSUM_NONE; + /* Ask for checksum offloading */ + ul_header->csum_valid_required = 1; + priv->stats.csum_hw++; + return; + } + } + +sw_csum: + priv->stats.csum_sw++; +} + +/* Generates UL checksum meta info header for IPv4 and IPv6 over TCP and UDP + * packets that are supported for UL checksum offload. + */ +void rmnet_map_checksum_uplink_packet(struct sk_buff *skb, + struct rmnet_port *port, + struct net_device *orig_dev, + int csum_type) +{ + switch (csum_type) { + case RMNET_FLAGS_EGRESS_MAP_CKSUMV4: + rmnet_map_v4_checksum_uplink_packet(skb, orig_dev); + break; + case RMNET_FLAGS_EGRESS_MAP_CKSUMV5: + rmnet_map_v5_checksum_uplink_packet(skb, port, orig_dev); + break; + default: + break; + } +} + +bool rmnet_map_v5_csum_buggy(struct rmnet_map_v5_coal_header *coal_hdr) +{ + /* Only applies to frames with a single packet */ + if (coal_hdr->num_nlos != 1 || coal_hdr->nl_pairs[0].num_packets != 1) + return false; + + /* TCP header has FIN or PUSH set */ + if (coal_hdr->close_type == RMNET_MAP_COAL_CLOSE_COAL) + return true; + + /* Hit packet limit, byte limit, or time limit/EOF on DMA */ + if (coal_hdr->close_type == RMNET_MAP_COAL_CLOSE_HW) { + switch (coal_hdr->close_value) { + case RMNET_MAP_COAL_CLOSE_HW_PKT: + case RMNET_MAP_COAL_CLOSE_HW_BYTE: + case RMNET_MAP_COAL_CLOSE_HW_TIME: + return true; + } + } + + return false; +} + +static void rmnet_map_move_headers(struct sk_buff *skb) +{ + struct iphdr *iph; + u16 ip_len; + u16 trans_len = 0; + u8 proto; + + /* This only applies to non-linear SKBs */ + if (!skb_is_nonlinear(skb)) + return; + + iph = (struct iphdr *)rmnet_map_data_ptr(skb); + if (iph->version == 4) { + ip_len = iph->ihl * 4; + proto = iph->protocol; + if (iph->frag_off & htons(IP_OFFSET)) + /* No transport header information */ + goto pull; + } else if (iph->version == 6) { + struct ipv6hdr *ip6h = (struct ipv6hdr *)iph; + __be16 frag_off; + u8 nexthdr = ip6h->nexthdr; + + ip_len = ipv6_skip_exthdr(skb, sizeof(*ip6h), &nexthdr, + &frag_off); + if (ip_len < 0) + return; + + proto = nexthdr; + } else { + return; + } + + if (proto == IPPROTO_TCP) { + struct tcphdr *tp = (struct tcphdr *)((u8 *)iph + ip_len); + + trans_len = tp->doff * 4; + } else if (proto == IPPROTO_UDP) { + trans_len = sizeof(struct udphdr); + } else if (proto == NEXTHDR_FRAGMENT) { + /* Non-first fragments don't have the fragment length added by + * ipv6_skip_exthdr() and sho up as proto NEXTHDR_FRAGMENT, so + * we account for the length here. + */ + ip_len += sizeof(struct frag_hdr); + } + +pull: + __pskb_pull_tail(skb, ip_len + trans_len); + skb_reset_network_header(skb); + if (trans_len) + skb_set_transport_header(skb, ip_len); +} + +static void rmnet_map_nonlinear_copy(struct sk_buff *coal_skb, + struct rmnet_map_coal_metadata *coal_meta, + struct sk_buff *dest) +{ + unsigned char *data_start = rmnet_map_data_ptr(coal_skb) + + coal_meta->ip_len + coal_meta->trans_len; + u32 copy_len = coal_meta->data_len * coal_meta->pkt_count; + + if (skb_is_nonlinear(coal_skb)) { + skb_frag_t *frag0 = skb_shinfo(coal_skb)->frags; + struct page *page = skb_frag_page(frag0); + + skb_append_pagefrags(dest, page, + frag0->bv_offset + coal_meta->ip_len + + coal_meta->trans_len + + coal_meta->data_offset, + copy_len); + dest->data_len += copy_len; + dest->len += copy_len; + } else { + skb_put_data(dest, data_start + coal_meta->data_offset, + copy_len); + } +} + +/* Fill in GSO metadata to allow the SKB to be segmented by the NW stack + * if needed (i.e. forwarding, UDP GRO) + */ +static void rmnet_map_gso_stamp(struct sk_buff *skb, + struct rmnet_map_coal_metadata *coal_meta) +{ + struct skb_shared_info *shinfo = skb_shinfo(skb); + + if (coal_meta->trans_proto == IPPROTO_TCP) + shinfo->gso_type = (coal_meta->ip_proto == 4) ? + SKB_GSO_TCPV4 : SKB_GSO_TCPV6; + else + shinfo->gso_type = SKB_GSO_UDP_L4; + + shinfo->gso_size = coal_meta->data_len; + shinfo->gso_segs = coal_meta->pkt_count; +} + +/* Handles setting up the partial checksum in the skb. Sets the transport + * checksum to the pseudoheader checksum and sets the csum offload metadata + */ +static void rmnet_map_partial_csum(struct sk_buff *skb, + struct rmnet_map_coal_metadata *coal_meta) +{ + unsigned char *data = skb->data; + __sum16 pseudo; + u16 pkt_len = skb->len - coal_meta->ip_len; + + if (coal_meta->ip_proto == 4) { + struct iphdr *iph = (struct iphdr *)data; + + pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr, + pkt_len, coal_meta->trans_proto, + 0); + } else { + struct ipv6hdr *ip6h = (struct ipv6hdr *)data; + + pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + pkt_len, coal_meta->trans_proto, 0); + } + + if (coal_meta->trans_proto == IPPROTO_TCP) { + struct tcphdr *tp = (struct tcphdr *)(data + coal_meta->ip_len); + + tp->check = pseudo; + skb->csum_offset = offsetof(struct tcphdr, check); + } else { + struct udphdr *up = (struct udphdr *)(data + coal_meta->ip_len); + + up->check = pseudo; + skb->csum_offset = offsetof(struct udphdr, check); + } + + skb->ip_summed = CHECKSUM_PARTIAL; + skb->csum_start = skb->data + coal_meta->ip_len - skb->head; +} + +static void +__rmnet_map_segment_coal_skb(struct sk_buff *coal_skb, + struct rmnet_map_coal_metadata *coal_meta, + struct sk_buff_head *list, u8 pkt_id, + bool csum_valid) +{ + struct sk_buff *skbn; + struct rmnet_priv *priv = netdev_priv(coal_skb->dev); + __sum16 *check = NULL; + u32 alloc_len; + u32 dlen = coal_meta->data_len * coal_meta->pkt_count; + u32 hlen = coal_meta->ip_len + coal_meta->trans_len; + bool zero_csum = false; + + /* We can avoid copying the data if the SKB we got from the lower-level + * drivers was nonlinear. + */ + if (skb_is_nonlinear(coal_skb)) + alloc_len = hlen; + else + alloc_len = hlen + dlen; + + skbn = alloc_skb(alloc_len, GFP_ATOMIC); + if (!skbn) + return; + + skb_reserve(skbn, hlen); + rmnet_map_nonlinear_copy(coal_skb, coal_meta, skbn); + + /* Push transport header and update necessary fields */ + skb_push(skbn, coal_meta->trans_len); + memcpy(skbn->data, coal_meta->trans_header, coal_meta->trans_len); + skb_reset_transport_header(skbn); + if (coal_meta->trans_proto == IPPROTO_TCP) { + struct tcphdr *th = tcp_hdr(skbn); + + th->seq = htonl(ntohl(th->seq) + coal_meta->data_offset); + check = &th->check; + + /* Don't allow dangerous flags to be set in any segment but the + * last one. + */ + if (th->fin || th->psh) { + if (hlen + coal_meta->data_offset + dlen < + coal_skb->len) { + th->fin = 0; + th->psh = 0; + } + } + } else if (coal_meta->trans_proto == IPPROTO_UDP) { + struct udphdr *uh = udp_hdr(skbn); + + uh->len = htons(skbn->len); + check = &uh->check; + if (coal_meta->ip_proto == 4 && !uh->check) + zero_csum = true; + } + + /* Push IP header and update necessary fields */ + skb_push(skbn, coal_meta->ip_len); + memcpy(skbn->data, coal_meta->ip_header, coal_meta->ip_len); + skb_reset_network_header(skbn); + if (coal_meta->ip_proto == 4) { + struct iphdr *iph = ip_hdr(skbn); + + iph->id = htons(ntohs(iph->id) + coal_meta->pkt_id); + iph->tot_len = htons(skbn->len); + iph->check = 0; + iph->check = ip_fast_csum(iph, iph->ihl); + } else { + /* Payload length includes any extension headers */ + ipv6_hdr(skbn)->payload_len = htons(skbn->len - + sizeof(struct ipv6hdr)); + } + + /* Handle checksum status */ + if (likely(csum_valid) || zero_csum) { + /* Set the partial checksum information */ + rmnet_map_partial_csum(skbn, coal_meta); + } else if (check) { + /* Unfortunately, we have to fake a bad checksum here, since + * the original bad value is lost by the hardware. The only + * reliable way to do it is to calculate the actual checksum + * and corrupt it. + */ + __wsum csum; + unsigned int offset = skb_transport_offset(skbn); + __sum16 pseudo; + + /* Calculate pseudo header */ + if (coal_meta->ip_proto == 4) { + struct iphdr *iph = ip_hdr(skbn); + + pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr, + skbn->len - + coal_meta->ip_len, + coal_meta->trans_proto, 0); + } else { + struct ipv6hdr *ip6h = ipv6_hdr(skbn); + + pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + skbn->len - coal_meta->ip_len, + coal_meta->trans_proto, 0); + } + + *check = pseudo; + csum = skb_checksum(skbn, offset, skbn->len - offset, 0); + /* Add 1 to corrupt. This cannot produce a final value of 0 + * since csum_fold() can't return a value of 0xFFFF. + */ + *check = csum16_add(csum_fold(csum), htons(1)); + skbn->ip_summed = CHECKSUM_NONE; + } + + skbn->dev = coal_skb->dev; + priv->stats.coal.coal_reconstruct++; + + /* Stamp GSO information if necessary */ + if (coal_meta->pkt_count > 1) + rmnet_map_gso_stamp(skbn, coal_meta); + + __skb_queue_tail(list, skbn); + + /* Update meta information to move past the data we just segmented */ + coal_meta->data_offset += dlen; + coal_meta->pkt_id = pkt_id + 1; + coal_meta->pkt_count = 0; +} + +static bool rmnet_map_validate_csum(struct sk_buff *skb, + struct rmnet_map_coal_metadata *meta) +{ + u8 *data = rmnet_map_data_ptr(skb); + unsigned int datagram_len; + __wsum csum; + __sum16 pseudo; + + datagram_len = skb->len - meta->ip_len; + if (meta->ip_proto == 4) { + struct iphdr *iph = (struct iphdr *)data; + + pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr, + datagram_len, + meta->trans_proto, 0); + } else { + struct ipv6hdr *ip6h = (struct ipv6hdr *)data; + + pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr, + datagram_len, meta->trans_proto, + 0); + } + + csum = skb_checksum(skb, meta->ip_len, datagram_len, + csum_unfold(pseudo)); + return !csum_fold(csum); +} + +/* Converts the coalesced SKB into a list of SKBs. + * NLOs containing csum erros will not be included. + * The original coalesced SKB should be treated as invalid and + * must be freed by the caller + */ +static void rmnet_map_segment_coal_skb(struct sk_buff *coal_skb, + u64 nlo_err_mask, + struct sk_buff_head *list) +{ + struct iphdr *iph; + struct rmnet_priv *priv = netdev_priv(coal_skb->dev); + struct rmnet_map_v5_coal_header *coal_hdr; + struct rmnet_map_coal_metadata coal_meta; + u16 pkt_len; + u8 pkt, total_pkt = 0; + u8 nlo; + bool gro = coal_skb->dev->features & NETIF_F_GRO_HW; + bool zero_csum = false; + + memset(&coal_meta, 0, sizeof(coal_meta)); + + /* Pull off the headers we no longer need */ + pskb_pull(coal_skb, sizeof(struct rmnet_map_header)); + coal_hdr = (struct rmnet_map_v5_coal_header *) + rmnet_map_data_ptr(coal_skb); + pskb_pull(coal_skb, sizeof(*coal_hdr)); + + iph = (struct iphdr *)rmnet_map_data_ptr(coal_skb); + + if (iph->version == 4) { + coal_meta.ip_proto = 4; + coal_meta.ip_len = iph->ihl * 4; + coal_meta.trans_proto = iph->protocol; + coal_meta.ip_header = iph; + + /* Don't allow coalescing of any packets with IP options */ + if (iph->ihl != 5) + gro = false; + } else if (iph->version == 6) { + struct ipv6hdr *ip6h = (struct ipv6hdr *)iph; + __be16 frag_off; + u8 protocol = ip6h->nexthdr; + + coal_meta.ip_proto = 6; + coal_meta.ip_len = ipv6_skip_exthdr(coal_skb, sizeof(*ip6h), + &protocol, &frag_off); + coal_meta.trans_proto = protocol; + coal_meta.ip_header = ip6h; + + /* If we run into a problem, or this has a fragment header + * (which should technically not be possible, if the HW + * works as intended...), bail. + */ + if (coal_meta.ip_len < 0 || frag_off) { + priv->stats.coal.coal_ip_invalid++; + return; + } else if (coal_meta.ip_len > sizeof(*ip6h)) { + /* Don't allow coalescing of any packets with IPv6 + * extension headers. + */ + gro = false; + } + } else { + priv->stats.coal.coal_ip_invalid++; + return; + } + + if (coal_meta.trans_proto == IPPROTO_TCP) { + struct tcphdr *th; + + th = (struct tcphdr *)((u8 *)iph + coal_meta.ip_len); + coal_meta.trans_len = th->doff * 4; + coal_meta.trans_header = th; + } else if (coal_meta.trans_proto == IPPROTO_UDP) { + struct udphdr *uh; + + uh = (struct udphdr *)((u8 *)iph + coal_meta.ip_len); + coal_meta.trans_len = sizeof(*uh); + coal_meta.trans_header = uh; + /* Check for v4 zero checksum */ + if (coal_meta.ip_proto == 4 && !uh->check) + zero_csum = true; + } else { + priv->stats.coal.coal_trans_invalid++; + return; + } + + if (rmnet_map_v5_csum_buggy(coal_hdr) && !zero_csum) { + rmnet_map_move_headers(coal_skb); + /* Mark as valid if it checks out */ + if (rmnet_map_validate_csum(coal_skb, &coal_meta)) + coal_skb->ip_summed = CHECKSUM_UNNECESSARY; + + __skb_queue_tail(list, coal_skb); + return; + } + + /* Fast-forward the case where we have 1 NLO (i.e. 1 packet length), + * no checksum errors, and are allowing GRO. We can just reuse this + * SKB unchanged. + */ + if (gro && coal_hdr->num_nlos == 1 && coal_hdr->csum_valid) { + rmnet_map_move_headers(coal_skb); + coal_skb->ip_summed = CHECKSUM_UNNECESSARY; + coal_meta.data_len = ntohs(coal_hdr->nl_pairs[0].pkt_len); + coal_meta.data_len -= coal_meta.ip_len + coal_meta.trans_len; + coal_meta.pkt_count = coal_hdr->nl_pairs[0].num_packets; + if (coal_meta.pkt_count > 1) { + rmnet_map_partial_csum(coal_skb, &coal_meta); + rmnet_map_gso_stamp(coal_skb, &coal_meta); + } + + __skb_queue_tail(list, coal_skb); + return; + } + + /* Segment the coalesced SKB into new packets */ + for (nlo = 0; nlo < coal_hdr->num_nlos; nlo++) { + pkt_len = ntohs(coal_hdr->nl_pairs[nlo].pkt_len); + pkt_len -= coal_meta.ip_len + coal_meta.trans_len; + coal_meta.data_len = pkt_len; + for (pkt = 0; pkt < coal_hdr->nl_pairs[nlo].num_packets; + pkt++, total_pkt++, nlo_err_mask >>= 1) { + bool csum_err = nlo_err_mask & 1; + + /* Segment the packet if we're not sending the larger + * packet up the stack. + */ + if (!gro) { + coal_meta.pkt_count = 1; + if (csum_err) + priv->stats.coal.coal_csum_err++; + + __rmnet_map_segment_coal_skb(coal_skb, + &coal_meta, list, + total_pkt, + !csum_err); + continue; + } + + if (csum_err) { + priv->stats.coal.coal_csum_err++; + + /* Segment out the good data */ + if (gro && coal_meta.pkt_count) + __rmnet_map_segment_coal_skb(coal_skb, + &coal_meta, + list, + total_pkt, + true); + + /* Segment out the bad checksum */ + coal_meta.pkt_count = 1; + __rmnet_map_segment_coal_skb(coal_skb, + &coal_meta, list, + total_pkt, false); + } else { + coal_meta.pkt_count++; + } + } + + /* If we're switching NLOs, we need to send out everything from + * the previous one, if we haven't done so. NLOs only switch + * when the packet length changes. + */ + if (coal_meta.pkt_count) + __rmnet_map_segment_coal_skb(coal_skb, &coal_meta, list, + total_pkt, true); + } +} + +/* Record reason for coalescing pipe closure */ +static void rmnet_map_data_log_close_stats(struct rmnet_priv *priv, u8 type, + u8 code) +{ + struct rmnet_coal_close_stats *stats = &priv->stats.coal.close; + + switch (type) { + case RMNET_MAP_COAL_CLOSE_NON_COAL: + stats->non_coal++; + break; + case RMNET_MAP_COAL_CLOSE_IP_MISS: + stats->ip_miss++; + break; + case RMNET_MAP_COAL_CLOSE_TRANS_MISS: + stats->trans_miss++; + break; + case RMNET_MAP_COAL_CLOSE_HW: + switch (code) { + case RMNET_MAP_COAL_CLOSE_HW_NL: + stats->hw_nl++; + break; + case RMNET_MAP_COAL_CLOSE_HW_PKT: + stats->hw_pkt++; + break; + case RMNET_MAP_COAL_CLOSE_HW_BYTE: + stats->hw_byte++; + break; + case RMNET_MAP_COAL_CLOSE_HW_TIME: + stats->hw_time++; + break; + case RMNET_MAP_COAL_CLOSE_HW_EVICT: + stats->hw_evict++; + break; + default: + break; + } + break; + case RMNET_MAP_COAL_CLOSE_COAL: + stats->coal++; + break; + default: + break; + } +} + +/* Check if the coalesced header has any incorrect values, in which case, the + * entire coalesced skb must be dropped. Then check if there are any + * checksum issues + */ +static int rmnet_map_data_check_coal_header(struct sk_buff *skb, + u64 *nlo_err_mask) +{ + struct rmnet_map_v5_coal_header *coal_hdr; + unsigned char *data = rmnet_map_data_ptr(skb); + struct rmnet_priv *priv = netdev_priv(skb->dev); + u64 mask = 0; + int i; + u8 veid, pkts = 0; + + coal_hdr = ((struct rmnet_map_v5_coal_header *) + (data + sizeof(struct rmnet_map_header))); + veid = coal_hdr->virtual_channel_id; + + if (coal_hdr->num_nlos == 0 || + coal_hdr->num_nlos > RMNET_MAP_V5_MAX_NLOS) { + priv->stats.coal.coal_hdr_nlo_err++; + return -EINVAL; + } + + for (i = 0; i < RMNET_MAP_V5_MAX_NLOS; i++) { + /* If there is a checksum issue, we need to split + * up the skb. Rebuild the full csum error field + */ + u8 err = coal_hdr->nl_pairs[i].csum_error_bitmap; + u8 pkt = coal_hdr->nl_pairs[i].num_packets; + + mask |= ((u64)err) << (8 * i); + + /* Track total packets in frame */ + pkts += pkt; + if (pkts > RMNET_MAP_V5_MAX_PACKETS) { + priv->stats.coal.coal_hdr_pkt_err++; + return -EINVAL; + } + } + + /* Track number of packets we get inside of coalesced frames */ + priv->stats.coal.coal_pkts += pkts; + + /* Update ethtool stats */ + rmnet_map_data_log_close_stats(priv, + coal_hdr->close_type, + coal_hdr->close_value); + if (veid < RMNET_MAX_VEID) + priv->stats.coal.coal_veid[veid]++; + + *nlo_err_mask = mask; + + return 0; +} + +/* Process a QMAPv5 packet header */ +int rmnet_map_process_next_hdr_packet(struct sk_buff *skb, + struct sk_buff_head *list, + u16 len) +{ + struct rmnet_priv *priv = netdev_priv(skb->dev); + u64 nlo_err_mask; + int rc = 0; + + switch (rmnet_map_get_next_hdr_type(skb)) { + case RMNET_MAP_HEADER_TYPE_COALESCING: + priv->stats.coal.coal_rx++; + rc = rmnet_map_data_check_coal_header(skb, &nlo_err_mask); + if (rc) + return rc; + + rmnet_map_segment_coal_skb(skb, nlo_err_mask, list); + if (skb_peek(list) != skb) + consume_skb(skb); + break; + case RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD: + if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) { + priv->stats.csum_sw++; + } else if (rmnet_map_get_csum_valid(skb)) { + priv->stats.csum_ok++; + skb->ip_summed = CHECKSUM_UNNECESSARY; + } else { + priv->stats.csum_valid_unset++; + } + + /* Pull unnecessary headers and move the rest to the linear + * section of the skb. + */ + pskb_pull(skb, + (sizeof(struct rmnet_map_header) + + sizeof(struct rmnet_map_v5_csum_header))); + rmnet_map_move_headers(skb); + + /* Remove padding only for csum offload packets. + * Coalesced packets should never have padding. + */ + pskb_trim(skb, len); + __skb_queue_tail(list, skb); + break; + default: + rc = -EINVAL; + break; + } + + return rc; +} + +long rmnet_agg_time_limit __read_mostly = 1000000L; +long rmnet_agg_bypass_time __read_mostly = 10000000L; + +int rmnet_map_tx_agg_skip(struct sk_buff *skb, int offset) +{ + u8 *packet_start = skb->data + offset; + int is_icmp = 0; + + if (skb->protocol == htons(ETH_P_IP)) { + struct iphdr *ip4h = (struct iphdr *)(packet_start); + + if (ip4h->protocol == IPPROTO_ICMP) + is_icmp = 1; + } else if (skb->protocol == htons(ETH_P_IPV6)) { + struct ipv6hdr *ip6h = (struct ipv6hdr *)(packet_start); + + if (ip6h->nexthdr == IPPROTO_ICMPV6) { + is_icmp = 1; + } else if (ip6h->nexthdr == NEXTHDR_FRAGMENT) { + struct frag_hdr *frag; + + frag = (struct frag_hdr *)(packet_start + + sizeof(struct ipv6hdr)); + if (frag->nexthdr == IPPROTO_ICMPV6) + is_icmp = 1; + } + } + + return is_icmp; +} + +static void rmnet_map_flush_tx_packet_work(struct work_struct *work) +{ + struct sk_buff *skb = NULL; + struct rmnet_port *port; + unsigned long flags; + + port = container_of(work, struct rmnet_port, agg_wq); + + spin_lock_irqsave(&port->agg_lock, flags); + if (likely(port->agg_state == -EINPROGRESS)) { + /* Buffer may have already been shipped out */ + if (likely(port->agg_skb)) { + skb = port->agg_skb; + port->agg_skb = NULL; + port->agg_count = 0; + memset(&port->agg_time, 0, sizeof(port->agg_time)); + } + port->agg_state = 0; + } + + spin_unlock_irqrestore(&port->agg_lock, flags); + if (skb) + dev_queue_xmit(skb); +} + +enum hrtimer_restart rmnet_map_flush_tx_packet_queue(struct hrtimer *t) +{ + struct rmnet_port *port; + + port = container_of(t, struct rmnet_port, hrtimer); + + schedule_work(&port->agg_wq); + return HRTIMER_NORESTART; +} + +static void rmnet_map_linearize_copy(struct sk_buff *dst, struct sk_buff *src) +{ + unsigned int linear = src->len - src->data_len, target = src->len; + unsigned char *src_buf; + struct sk_buff *skb; + + src_buf = src->data; + skb_put_data(dst, src_buf, linear); + target -= linear; + + skb = src; + + while (target) { + unsigned int i = 0, non_linear = 0; + + for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) { + non_linear = skb_frag_size(&skb_shinfo(skb)->frags[i]); + src_buf = skb_frag_address(&skb_shinfo(skb)->frags[i]); + + skb_put_data(dst, src_buf, non_linear); + target -= non_linear; + } + + if (skb_shinfo(skb)->frag_list) { + skb = skb_shinfo(skb)->frag_list; + continue; + } + + if (skb->next) + skb = skb->next; + } +} + +static void rmnet_free_agg_pages(struct rmnet_port *port) +{ + struct rmnet_agg_page *agg_page, *idx; + + list_for_each_entry_safe(agg_page, idx, &port->agg_list, list) { + list_del(&agg_page->list); + put_page(agg_page->page); + kfree(agg_page); + } + + port->agg_head = NULL; +} + +static struct page *rmnet_get_agg_pages(struct rmnet_port *port) +{ + struct rmnet_agg_page *agg_page; + struct page *page = NULL; + int i = 0; + + if (!(port->egress_agg_params.agg_features & RMNET_PAGE_RECYCLE)) + goto alloc; + + do { + agg_page = port->agg_head; + if (unlikely(!agg_page)) + break; + + if (page_ref_count(agg_page->page) == 1) { + page = agg_page->page; + page_ref_inc(agg_page->page); + + port->stats.agg.ul_agg_reuse++; + port->agg_head = list_next_entry(agg_page, list); + break; + } + + port->agg_head = list_next_entry(agg_page, list); + i++; + } while (i <= 5); + +alloc: + if (!page) { + page = __dev_alloc_pages(GFP_ATOMIC, port->agg_size_order); + port->stats.agg.ul_agg_alloc++; + } + + return page; +} + +static struct rmnet_agg_page *__rmnet_alloc_agg_pages(struct rmnet_port *port) +{ + struct rmnet_agg_page *agg_page; + struct page *page; + + agg_page = kzalloc(sizeof(*agg_page), GFP_ATOMIC); + if (!agg_page) + return NULL; + + page = __dev_alloc_pages(GFP_ATOMIC, port->agg_size_order); + if (!page) { + kfree(agg_page); + return NULL; + } + + agg_page->page = page; + INIT_LIST_HEAD(&agg_page->list); + + return agg_page; +} + +static void rmnet_alloc_agg_pages(struct rmnet_port *port) +{ + struct rmnet_agg_page *agg_page = NULL; + int i = 0; + + for (i = 0; i < 512; i++) { + agg_page = __rmnet_alloc_agg_pages(port); + + if (agg_page) + list_add_tail(&agg_page->list, &port->agg_list); + } + + port->agg_head = list_first_entry_or_null(&port->agg_list, + struct rmnet_agg_page, list); +} + +static struct sk_buff *rmnet_map_build_skb(struct rmnet_port *port) +{ + struct sk_buff *skb; + unsigned int size; + struct page *page; + void *vaddr; + + page = rmnet_get_agg_pages(port); + if (!page) + return NULL; + + vaddr = page_address(page); + size = PAGE_SIZE << port->agg_size_order; + + skb = build_skb(vaddr, size); + if (!skb) { + put_page(page); + return NULL; + } + + return skb; +} + +static void rmnet_map_send_agg_skb(struct rmnet_port *port, unsigned long flags) +{ + struct sk_buff *agg_skb; + + if (!port->agg_skb) { + spin_unlock_irqrestore(&port->agg_lock, flags); + return; + } + + agg_skb = port->agg_skb; + /* Reset the aggregation state */ + port->agg_skb = NULL; + port->agg_count = 0; + memset(&port->agg_time, 0, sizeof(port->agg_time)); + port->agg_state = 0; + spin_unlock_irqrestore(&port->agg_lock, flags); + hrtimer_cancel(&port->hrtimer); + dev_queue_xmit(agg_skb); +} + +void rmnet_map_tx_aggregate(struct sk_buff *skb, struct rmnet_port *port) +{ + struct timespec64 diff, last; + int size; + unsigned long flags; + +new_packet: + spin_lock_irqsave(&port->agg_lock, flags); + memcpy(&last, &port->agg_last, sizeof(last)); + ktime_get_real_ts64(&port->agg_last); + + if ((port->data_format & RMNET_EGRESS_FORMAT_PRIORITY) && + skb->priority) { + /* Send out any aggregated SKBs we have */ + rmnet_map_send_agg_skb(port, flags); + /* Send out the priority SKB. Not holding agg_lock anymore */ + skb->protocol = htons(ETH_P_MAP); + dev_queue_xmit(skb); + return; + } + + if (!port->agg_skb) { + /* Check to see if we should agg first. If the traffic is very + * sparse, don't aggregate. We will need to tune this later + */ + diff = timespec64_sub(port->agg_last, last); + size = port->egress_agg_params.agg_size - skb->len; + + if (diff.tv_sec > 0 || diff.tv_nsec > rmnet_agg_bypass_time || + size <= 0) { + spin_unlock_irqrestore(&port->agg_lock, flags); + skb->protocol = htons(ETH_P_MAP); + dev_queue_xmit(skb); + return; + } + + port->agg_skb = rmnet_map_build_skb(port); + if (!port->agg_skb) { + port->agg_skb = 0; + port->agg_count = 0; + memset(&port->agg_time, 0, sizeof(port->agg_time)); + spin_unlock_irqrestore(&port->agg_lock, flags); + skb->protocol = htons(ETH_P_MAP); + dev_queue_xmit(skb); + return; + } + + rmnet_map_linearize_copy(port->agg_skb, skb); + port->agg_skb->dev = skb->dev; + port->agg_skb->protocol = htons(ETH_P_MAP); + port->agg_count = 1; + ktime_get_real_ts64(&port->agg_time); + dev_kfree_skb_any(skb); + goto schedule; + } + diff = timespec64_sub(port->agg_last, port->agg_time); + size = skb_tailroom(port->agg_skb); + + if (skb->len > size || + port->agg_count >= port->egress_agg_params.agg_count || + diff.tv_sec > 0 || diff.tv_nsec > rmnet_agg_time_limit) { + rmnet_map_send_agg_skb(port, flags); + goto new_packet; + } + + rmnet_map_linearize_copy(port->agg_skb, skb); + port->agg_count++; + dev_kfree_skb_any(skb); + +schedule: + if (port->agg_state != -EINPROGRESS) { + port->agg_state = -EINPROGRESS; + hrtimer_start(&port->hrtimer, + ns_to_ktime(port->egress_agg_params.agg_time), + HRTIMER_MODE_REL); + } + spin_unlock_irqrestore(&port->agg_lock, flags); +} + +void rmnet_map_update_ul_agg_config(struct rmnet_port *port, u16 size, + u8 count, u8 features, u32 time) +{ + unsigned long irq_flags; + + spin_lock_irqsave(&port->agg_lock, irq_flags); + port->egress_agg_params.agg_count = count; + port->egress_agg_params.agg_time = time; + port->egress_agg_params.agg_size = size; + port->egress_agg_params.agg_features = features; + + rmnet_free_agg_pages(port); + + /* This effectively disables recycling in case the UL aggregation + * size is lesser than PAGE_SIZE. + */ + if (size < PAGE_SIZE) + goto done; + + port->agg_size_order = get_order(size); + + size = PAGE_SIZE << port->agg_size_order; + size -= SKB_DATA_ALIGN(sizeof(struct skb_shared_info)); + port->egress_agg_params.agg_size = size; + + if (port->egress_agg_params.agg_features == RMNET_PAGE_RECYCLE) + rmnet_alloc_agg_pages(port); + +done: + spin_unlock_irqrestore(&port->agg_lock, irq_flags); +} + +void rmnet_map_tx_aggregate_init(struct rmnet_port *port) +{ + hrtimer_init(&port->hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + port->hrtimer.function = rmnet_map_flush_tx_packet_queue; + spin_lock_init(&port->agg_lock); + INIT_LIST_HEAD(&port->agg_list); + + /* Since PAGE_SIZE - 1 is specified here, no pages are pre-allocated. + * This is done to reduce memory usage in cases where + * UL aggregation is disabled. + * Additionally, the features flag is also set to 0. + */ + rmnet_map_update_ul_agg_config(port, PAGE_SIZE - 1, 20, 0, 3000000); + + INIT_WORK(&port->agg_wq, rmnet_map_flush_tx_packet_work); +} + +void rmnet_map_tx_aggregate_exit(struct rmnet_port *port) +{ + unsigned long flags; + + hrtimer_cancel(&port->hrtimer); + cancel_work_sync(&port->agg_wq); + + spin_lock_irqsave(&port->agg_lock, flags); + if (port->agg_state == -EINPROGRESS) { + if (port->agg_skb) { + kfree_skb(port->agg_skb); + port->agg_skb = NULL; + port->agg_count = 0; + memset(&port->agg_time, 0, sizeof(port->agg_time)); + } + + port->agg_state = 0; + } + + rmnet_free_agg_pages(port); + spin_unlock_irqrestore(&port->agg_lock, flags); +} + +void rmnet_map_tx_qmap_cmd(struct sk_buff *qmap_skb) +{ + struct rmnet_port *port; + struct sk_buff *agg_skb; + unsigned long flags; + + port = rmnet_get_port(qmap_skb->dev); + + if (port && (port->data_format & RMNET_EGRESS_FORMAT_AGGREGATION)) { + spin_lock_irqsave(&port->agg_lock, flags); + if (port->agg_skb) { + agg_skb = port->agg_skb; + port->agg_skb = 0; + port->agg_count = 0; + memset(&port->agg_time, 0, sizeof(port->agg_time)); + port->agg_state = 0; + spin_unlock_irqrestore(&port->agg_lock, flags); + hrtimer_cancel(&port->hrtimer); + dev_queue_xmit(agg_skb); + } else { + spin_unlock_irqrestore(&port->agg_lock, flags); + } + } + + dev_queue_xmit(qmap_skb); +} +EXPORT_SYMBOL(rmnet_map_tx_qmap_cmd); diff --git a/techpack/datarmnet/core/rmnet_private.h b/techpack/datarmnet/core/rmnet_private.h new file mode 100644 index 000000000000..03d297a9ed71 --- /dev/null +++ b/techpack/datarmnet/core/rmnet_private.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2013-2014, 2016-2020 The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _RMNET_PRIVATE_H_ +#define _RMNET_PRIVATE_H_ + +#define RMNET_MAX_PACKET_SIZE 16384 +#define RMNET_DFLT_PACKET_SIZE 1500 +#define RMNET_NEEDED_HEADROOM 16 +#define RMNET_TX_QUEUE_LEN 1000 + +/* Constants */ +#define RMNET_EGRESS_FORMAT_AGGREGATION BIT(31) +#define RMNET_INGRESS_FORMAT_DL_MARKER_V1 BIT(30) +#define RMNET_INGRESS_FORMAT_DL_MARKER_V2 BIT(29) + +#define RMNET_FLAGS_INGRESS_COALESCE BIT(4) +#define RMNET_FLAGS_INGRESS_MAP_CKSUMV5 BIT(5) +#define RMNET_FLAGS_EGRESS_MAP_CKSUMV5 BIT(6) + +#define RMNET_INGRESS_FORMAT_DL_MARKER (RMNET_INGRESS_FORMAT_DL_MARKER_V1 |\ +RMNET_INGRESS_FORMAT_DL_MARKER_V2) + +/* UL Packet prioritization */ +#define RMNET_EGRESS_FORMAT_PRIORITY BIT(28) + +/* Power save feature*/ +#define RMNET_INGRESS_FORMAT_PS BIT(27) +#define RMNET_FORMAT_PS_NOTIF BIT(26) + +/* UL Aggregation parameters */ +#define RMNET_PAGE_RECYCLE BIT(0) + +/* Replace skb->dev to a virtual rmnet device and pass up the stack */ +#define RMNET_EPMODE_VND (1) +/* Pass the frame directly to another device with dev_queue_xmit() */ +#define RMNET_EPMODE_BRIDGE (2) + +#endif /* _RMNET_PRIVATE_H_ */ diff --git a/techpack/datarmnet/core/rmnet_qmi.h b/techpack/datarmnet/core/rmnet_qmi.h new file mode 100644 index 000000000000..5c2a4953e75e --- /dev/null +++ b/techpack/datarmnet/core/rmnet_qmi.h @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2018-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _RMNET_QMI_H +#define _RMNET_QMI_H + +#include +#include +#define CONFIG_QTI_QMI_RMNET 1 + +void rmnet_map_tx_qmap_cmd(struct sk_buff *qmap_skb); + +#ifdef CONFIG_QTI_QMI_RMNET +void *rmnet_get_qmi_pt(void *port); +void *rmnet_get_qos_pt(struct net_device *dev); +void *rmnet_get_rmnet_port(struct net_device *dev); +struct net_device *rmnet_get_rmnet_dev(void *port, u8 mux_id); +void rmnet_reset_qmi_pt(void *port); +void rmnet_init_qmi_pt(void *port, void *qmi); +void rmnet_enable_all_flows(void *port); +bool rmnet_all_flows_enabled(void *port); +void rmnet_set_powersave_format(void *port); +void rmnet_clear_powersave_format(void *port); +void rmnet_get_packets(void *port, u64 *rx, u64 *tx); +int rmnet_get_powersave_notif(void *port); +struct net_device *rmnet_get_real_dev(void *port); +int rmnet_get_dlmarker_info(void *port); +#else +static inline void *rmnet_get_qmi_pt(void *port) +{ + return NULL; +} + +static inline void *rmnet_get_qos_pt(struct net_device *dev) +{ + return NULL; +} + +static inline void *rmnet_get_rmnet_port(struct net_device *dev) +{ + return NULL; +} + +static inline struct net_device *rmnet_get_rmnet_dev(void *port, + u8 mux_id) +{ + return NULL; +} + +static inline void rmnet_reset_qmi_pt(void *port) +{ +} + +static inline void rmnet_init_qmi_pt(void *port, void *qmi) +{ +} + +static inline void rmnet_enable_all_flows(void *port) +{ +} + +static inline bool rmnet_all_flows_enabled(void *port) +{ + return true; +} + +static inline void rmnet_set_port_format(void *port) +{ +} + +static inline void rmnet_get_packets(void *port, u64 *rx, u64 *tx) +{ +} + +static inline int rmnet_get_powersave_notif(void *port) +{ + return 0; +} + +static inline struct net_device *rmnet_get_real_dev(void *port) +{ + return NULL; +} + +static inline int rmnet_get_dlmarker_info(void *port) +{ + return 0; +} +#endif /* CONFIG_QTI_QMI_RMNET */ +#endif /*_RMNET_QMI_H*/ diff --git a/techpack/datarmnet/core/rmnet_trace.h b/techpack/datarmnet/core/rmnet_trace.h new file mode 100644 index 000000000000..28a2a2c6003c --- /dev/null +++ b/techpack/datarmnet/core/rmnet_trace.h @@ -0,0 +1,280 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* Copyright (c) 2018-2020, The Linux Foundation. All rights reserved. + */ + +#undef TRACE_SYSTEM +#define TRACE_SYSTEM rmnet +#undef TRACE_INCLUDE_PATH +#define TRACE_INCLUDE_PATH ../../../../vendor/qcom/opensource/datarmnet/core +#define TRACE_INCLUDE_FILE rmnet_trace + +#if !defined(_TRACE_RMNET_H) || defined(TRACE_HEADER_MULTI_READ) +#define _TRACE_RMNET_H + +#include +#include + +/*****************************************************************************/ +/* Trace events for rmnet module */ +/*****************************************************************************/ +TRACE_EVENT(rmnet_xmit_skb, + + TP_PROTO(struct sk_buff *skb), + + TP_ARGS(skb), + + TP_STRUCT__entry( + __string(dev_name, skb->dev->name) + __field(unsigned int, len) + ), + + TP_fast_assign( + __assign_str(dev_name, skb->dev->name); + __entry->len = skb->len; + ), + + TP_printk("dev_name=%s len=%u", __get_str(dev_name), __entry->len) +); + +DECLARE_EVENT_CLASS + (rmnet_mod_template, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2), + + TP_STRUCT__entry(__field(u8, func) + __field(u8, evt) + __field(u32, uint1) + __field(u32, uint2) + __field(u64, ulong1) + __field(u64, ulong2) + __field(void *, ptr1) + __field(void *, ptr2) + ), + + TP_fast_assign(__entry->func = func; + __entry->evt = evt; + __entry->uint1 = uint1; + __entry->uint2 = uint2; + __entry->ulong1 = ulong1; + __entry->ulong2 = ulong2; + __entry->ptr1 = ptr1; + __entry->ptr2 = ptr2; + ), + +TP_printk("fun:%u ev:%u u1:%u u2:%u ul1:%llu ul2:%llu p1:0x%pK p2:0x%pK", + __entry->func, __entry->evt, + __entry->uint1, __entry->uint2, + __entry->ulong1, __entry->ulong2, + __entry->ptr1, __entry->ptr2) +) + +DEFINE_EVENT + (rmnet_mod_template, rmnet_low, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) + +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_high, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) + +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_err, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) + +); + +TRACE_EVENT(print_skb_gso, + + TP_PROTO(struct sk_buff *skb, __be16 src, __be16 dest), + + TP_ARGS(skb, src, dest), + + TP_STRUCT__entry( + __field(void *, skbaddr) + __field(int, len) + __field(int, data_len) + __field(__be16, src) + __field(__be16, dest) + ), + + TP_fast_assign( + __entry->skbaddr = skb; + __entry->len = skb->len; + __entry->data_len = skb->data_len; + __entry->src = src; + __entry->dest = dest; + ), + + TP_printk("GSO: skbaddr=%pK, len=%d, data_len=%d, src=%u, dest=%u", + __entry->skbaddr, __entry->len, __entry->data_len, + be16_to_cpu(__entry->src), be16_to_cpu(__entry->dest)) +); + +/*****************************************************************************/ +/* Trace events for rmnet_perf module */ +/*****************************************************************************/ +DEFINE_EVENT + (rmnet_mod_template, rmnet_perf_low, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) + +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_perf_high, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) + +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_perf_err, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) + +); + +/*****************************************************************************/ +/* Trace events for rmnet_shs module */ +/*****************************************************************************/ +DEFINE_EVENT + (rmnet_mod_template, rmnet_shs_low, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_shs_high, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_shs_err, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_shs_wq_low, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_shs_wq_high, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) +); + +DEFINE_EVENT + (rmnet_mod_template, rmnet_shs_wq_err, + + TP_PROTO(u8 func, u8 evt, u32 uint1, u32 uint2, + u64 ulong1, u64 ulong2, void *ptr1, void *ptr2), + + TP_ARGS(func, evt, uint1, uint2, ulong1, ulong2, ptr1, ptr2) +); + +DECLARE_EVENT_CLASS + (rmnet_freq_template, + + TP_PROTO(u8 core, u32 newfreq), + + TP_ARGS(core, newfreq), + + TP_STRUCT__entry(__field(u8, core) + __field(u32, newfreq) + ), + + TP_fast_assign(__entry->core = core; + __entry->newfreq = newfreq; + ), + +TP_printk("freq policy core:%u freq floor :%u", + __entry->core, __entry->newfreq) + +); + +DEFINE_EVENT + (rmnet_freq_template, rmnet_freq_boost, + + TP_PROTO(u8 core, u32 newfreq), + + TP_ARGS(core, newfreq) +); + +DEFINE_EVENT + (rmnet_freq_template, rmnet_freq_reset, + + TP_PROTO(u8 core, u32 newfreq), + + TP_ARGS(core, newfreq) +); + +TRACE_EVENT + (rmnet_freq_update, + + TP_PROTO(u8 core, u32 lowfreq, u32 highfreq), + + TP_ARGS(core, lowfreq, highfreq), + + TP_STRUCT__entry(__field(u8, core) + __field(u32, lowfreq) + __field(u32, highfreq) + ), + + TP_fast_assign(__entry->core = core; + __entry->lowfreq = lowfreq; + __entry->highfreq = highfreq; + + ), + +TP_printk("freq policy update core:%u policy freq floor :%u freq ceil :%u", + __entry->core, __entry->lowfreq, __entry->highfreq) +); +#endif /* _TRACE_RMNET_H */ + +#include diff --git a/techpack/datarmnet/core/rmnet_vnd.c b/techpack/datarmnet/core/rmnet_vnd.c new file mode 100644 index 000000000000..d5187be0497d --- /dev/null +++ b/techpack/datarmnet/core/rmnet_vnd.c @@ -0,0 +1,454 @@ +/* Copyright (c) 2013-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * + * RMNET Data virtual network driver + * + */ + +#include +#include +#include +#include +#include +#include +#include "rmnet_config.h" +#include "rmnet_handlers.h" +#include "rmnet_private.h" +#include "rmnet_map.h" +#include "rmnet_vnd.h" +#include "rmnet_genl.h" + +#include "qmi_rmnet.h" +#include "rmnet_qmi.h" +#include "rmnet_trace.h" + +/* RX/TX Fixup */ + +void rmnet_vnd_rx_fixup(struct net_device *dev, u32 skb_len) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct rmnet_pcpu_stats *pcpu_ptr; + + pcpu_ptr = this_cpu_ptr(priv->pcpu_stats); + + u64_stats_update_begin(&pcpu_ptr->syncp); + pcpu_ptr->stats.rx_pkts++; + pcpu_ptr->stats.rx_bytes += skb_len; + u64_stats_update_end(&pcpu_ptr->syncp); +} + +void rmnet_vnd_tx_fixup(struct net_device *dev, u32 skb_len) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct rmnet_pcpu_stats *pcpu_ptr; + + pcpu_ptr = this_cpu_ptr(priv->pcpu_stats); + + u64_stats_update_begin(&pcpu_ptr->syncp); + pcpu_ptr->stats.tx_pkts++; + pcpu_ptr->stats.tx_bytes += skb_len; + u64_stats_update_end(&pcpu_ptr->syncp); +} + +/* Network Device Operations */ + +static netdev_tx_t rmnet_vnd_start_xmit(struct sk_buff *skb, + struct net_device *dev) +{ + struct rmnet_priv *priv; + int ip_type; + u32 mark; + unsigned int len; + + priv = netdev_priv(dev); + if (priv->real_dev) { + ip_type = (ip_hdr(skb)->version == 4) ? + AF_INET : AF_INET6; + mark = skb->mark; + len = skb->len; + trace_rmnet_xmit_skb(skb); + rmnet_egress_handler(skb); + qmi_rmnet_burst_fc_check(dev, ip_type, mark, len); + qmi_rmnet_work_maybe_restart(rmnet_get_rmnet_port(dev)); + } else { + this_cpu_inc(priv->pcpu_stats->stats.tx_drops); + kfree_skb(skb); + } + return NETDEV_TX_OK; +} + +static int rmnet_vnd_change_mtu(struct net_device *rmnet_dev, int new_mtu) +{ + if (new_mtu < 0 || new_mtu > RMNET_MAX_PACKET_SIZE) + return -EINVAL; + + rmnet_dev->mtu = new_mtu; + return 0; +} + +static int rmnet_vnd_get_iflink(const struct net_device *dev) +{ + struct rmnet_priv *priv = netdev_priv(dev); + + return priv->real_dev->ifindex; +} + +static int rmnet_vnd_init(struct net_device *dev) +{ + struct rmnet_priv *priv = netdev_priv(dev); + int err; + + priv->pcpu_stats = alloc_percpu(struct rmnet_pcpu_stats); + if (!priv->pcpu_stats) + return -ENOMEM; + + err = gro_cells_init(&priv->gro_cells, dev); + if (err) { + free_percpu(priv->pcpu_stats); + return err; + } + + return 0; +} + +static void rmnet_vnd_uninit(struct net_device *dev) +{ + struct rmnet_priv *priv = netdev_priv(dev); + void *qos; + + gro_cells_destroy(&priv->gro_cells); + free_percpu(priv->pcpu_stats); + + qos = rcu_dereference(priv->qos_info); + RCU_INIT_POINTER(priv->qos_info, NULL); + qmi_rmnet_qos_exit_pre(qos); +} + +static void rmnet_get_stats64(struct net_device *dev, + struct rtnl_link_stats64 *s) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct rmnet_vnd_stats total_stats; + struct rmnet_pcpu_stats *pcpu_ptr; + unsigned int cpu, start; + + memset(&total_stats, 0, sizeof(struct rmnet_vnd_stats)); + + for_each_possible_cpu(cpu) { + pcpu_ptr = per_cpu_ptr(priv->pcpu_stats, cpu); + + do { + start = u64_stats_fetch_begin_irq(&pcpu_ptr->syncp); + total_stats.rx_pkts += pcpu_ptr->stats.rx_pkts; + total_stats.rx_bytes += pcpu_ptr->stats.rx_bytes; + total_stats.tx_pkts += pcpu_ptr->stats.tx_pkts; + total_stats.tx_bytes += pcpu_ptr->stats.tx_bytes; + } while (u64_stats_fetch_retry_irq(&pcpu_ptr->syncp, start)); + + total_stats.tx_drops += pcpu_ptr->stats.tx_drops; + } + + s->rx_packets = total_stats.rx_pkts; + s->rx_bytes = total_stats.rx_bytes; + s->tx_packets = total_stats.tx_pkts; + s->tx_bytes = total_stats.tx_bytes; + s->tx_dropped = total_stats.tx_drops; +} + +static u16 rmnet_vnd_select_queue(struct net_device *dev, + struct sk_buff *skb, + struct net_device *sb_dev) +{ + struct rmnet_priv *priv = netdev_priv(dev); + u64 boost_period = 0; + int boost_trigger = 0; + int txq = 0; + + if (trace_print_skb_gso_enabled()) { + if (!skb_shinfo(skb)->gso_size) + goto skip_trace; + + if (skb->protocol == htons(ETH_P_IP)) { + if (ip_hdr(skb)->protocol != IPPROTO_TCP) + goto skip_trace; + } + + if (skb->protocol == htons(ETH_P_IPV6)) { + if (ipv6_hdr(skb)->nexthdr != IPPROTO_TCP) + goto skip_trace; + } + + trace_print_skb_gso(skb, tcp_hdr(skb)->source, + tcp_hdr(skb)->dest); + } + +skip_trace: + if (priv->real_dev) + txq = qmi_rmnet_get_queue(dev, skb); + + if (rmnet_core_userspace_connected) { + rmnet_update_pid_and_check_boost(task_pid_nr(current), + skb->len, + &boost_trigger, + &boost_period); + + if (boost_trigger) + (void) boost_period; + } + + return (txq < dev->real_num_tx_queues) ? txq : 0; +} + +static const struct net_device_ops rmnet_vnd_ops = { + .ndo_start_xmit = rmnet_vnd_start_xmit, + .ndo_change_mtu = rmnet_vnd_change_mtu, + .ndo_get_iflink = rmnet_vnd_get_iflink, + .ndo_add_slave = rmnet_add_bridge, + .ndo_del_slave = rmnet_del_bridge, + .ndo_init = rmnet_vnd_init, + .ndo_uninit = rmnet_vnd_uninit, + .ndo_get_stats64 = rmnet_get_stats64, + .ndo_select_queue = rmnet_vnd_select_queue, +}; + +static const char rmnet_gstrings_stats[][ETH_GSTRING_LEN] = { + "Checksum ok", + "Checksum valid bit not set", + "Checksum validation failed", + "Checksum error bad buffer", + "Checksum error bad ip version", + "Checksum error bad transport", + "Checksum skipped on ip fragment", + "Checksum skipped", + "Checksum computed in software", + "Checksum computed in hardware", + "Coalescing packets received", + "Coalesced packets", + "Coalescing header NLO errors", + "Coalescing header pcount errors", + "Coalescing checksum errors", + "Coalescing packet reconstructs", + "Coalescing IP version invalid", + "Coalescing L4 header invalid", + "Coalescing close Non-coalescable", + "Coalescing close L3 mismatch", + "Coalescing close L4 mismatch", + "Coalescing close HW NLO limit", + "Coalescing close HW packet limit", + "Coalescing close HW byte limit", + "Coalescing close HW time limit", + "Coalescing close HW eviction", + "Coalescing close Coalescable", + "Coalescing packets over VEID0", + "Coalescing packets over VEID1", + "Coalescing packets over VEID2", + "Coalescing packets over VEID3", + "Coalescing TCP frames", + "Coalescing TCP bytes", + "Coalescing UDP frames", + "Coalescing UDP bytes", + "Uplink priority packets", +}; + +static const char rmnet_port_gstrings_stats[][ETH_GSTRING_LEN] = { + "MAP Cmd last version", + "MAP Cmd last ep id", + "MAP Cmd last transaction id", + "DL header last seen sequence", + "DL header last seen bytes", + "DL header last seen packets", + "DL header last seen flows", + "DL header pkts received", + "DL header total bytes received", + "DL header total pkts received", + "DL trailer last seen sequence", + "DL trailer pkts received", + "UL agg reuse", + "UL agg alloc", +}; + +static void rmnet_get_strings(struct net_device *dev, u32 stringset, u8 *buf) +{ + switch (stringset) { + case ETH_SS_STATS: + memcpy(buf, &rmnet_gstrings_stats, + sizeof(rmnet_gstrings_stats)); + memcpy(buf + sizeof(rmnet_gstrings_stats), + &rmnet_port_gstrings_stats, + sizeof(rmnet_port_gstrings_stats)); + break; + } +} + +static int rmnet_get_sset_count(struct net_device *dev, int sset) +{ + switch (sset) { + case ETH_SS_STATS: + return ARRAY_SIZE(rmnet_gstrings_stats) + + ARRAY_SIZE(rmnet_port_gstrings_stats); + default: + return -EOPNOTSUPP; + } +} + +static void rmnet_get_ethtool_stats(struct net_device *dev, + struct ethtool_stats *stats, u64 *data) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct rmnet_priv_stats *st = &priv->stats; + struct rmnet_port_priv_stats *stp; + struct rmnet_port *port; + + port = rmnet_get_port(priv->real_dev); + + if (!data || !port) + return; + + stp = &port->stats; + + memcpy(data, st, ARRAY_SIZE(rmnet_gstrings_stats) * sizeof(u64)); + memcpy(data + ARRAY_SIZE(rmnet_gstrings_stats), stp, + ARRAY_SIZE(rmnet_port_gstrings_stats) * sizeof(u64)); +} + +static int rmnet_stats_reset(struct net_device *dev) +{ + struct rmnet_priv *priv = netdev_priv(dev); + struct rmnet_port_priv_stats *stp; + struct rmnet_priv_stats *st; + struct rmnet_port *port; + + port = rmnet_get_port(priv->real_dev); + if (!port) + return -EINVAL; + + stp = &port->stats; + + memset(stp, 0, sizeof(*stp)); + + st = &priv->stats; + + memset(st, 0, sizeof(*st)); + + return 0; +} + +static const struct ethtool_ops rmnet_ethtool_ops = { + .get_ethtool_stats = rmnet_get_ethtool_stats, + .get_strings = rmnet_get_strings, + .get_sset_count = rmnet_get_sset_count, + .nway_reset = rmnet_stats_reset, +}; + +/* Called by kernel whenever a new rmnet device is created. Sets MTU, + * flags, ARP type, needed headroom, etc... + */ +void rmnet_vnd_setup(struct net_device *rmnet_dev) +{ + rmnet_dev->netdev_ops = &rmnet_vnd_ops; + rmnet_dev->mtu = RMNET_DFLT_PACKET_SIZE; + rmnet_dev->needed_headroom = RMNET_NEEDED_HEADROOM; + random_ether_addr(rmnet_dev->perm_addr); + rmnet_dev->tx_queue_len = RMNET_TX_QUEUE_LEN; + + /* Raw IP mode */ + rmnet_dev->header_ops = NULL; /* No header */ + rmnet_dev->type = ARPHRD_RAWIP; + rmnet_dev->hard_header_len = 0; + rmnet_dev->flags &= ~(IFF_BROADCAST | IFF_MULTICAST); + + rmnet_dev->needs_free_netdev = true; + rmnet_dev->ethtool_ops = &rmnet_ethtool_ops; +} + +/* Exposed API */ + +int rmnet_vnd_newlink(u8 id, struct net_device *rmnet_dev, + struct rmnet_port *port, + struct net_device *real_dev, + struct rmnet_endpoint *ep) +{ + struct rmnet_priv *priv = netdev_priv(rmnet_dev); + int rc; + + if (ep->egress_dev) + return -EINVAL; + + if (rmnet_get_endpoint(port, id)) + return -EBUSY; + + rmnet_dev->hw_features = NETIF_F_RXCSUM; + rmnet_dev->hw_features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM; + rmnet_dev->hw_features |= NETIF_F_SG; + rmnet_dev->hw_features |= NETIF_F_GRO_HW; + + priv->real_dev = real_dev; + + rc = register_netdevice(rmnet_dev); + if (!rc) { + ep->egress_dev = rmnet_dev; + ep->mux_id = id; + port->nr_rmnet_devs++; + + rmnet_dev->rtnl_link_ops = &rmnet_link_ops; + + priv->mux_id = id; + rcu_assign_pointer(priv->qos_info, + qmi_rmnet_qos_init(real_dev, rmnet_dev, id)); + + netdev_dbg(rmnet_dev, "rmnet dev created\n"); + } + + return rc; +} + +int rmnet_vnd_dellink(u8 id, struct rmnet_port *port, + struct rmnet_endpoint *ep) +{ + if (id >= RMNET_MAX_LOGICAL_EP || !ep->egress_dev) + return -EINVAL; + + ep->egress_dev = NULL; + port->nr_rmnet_devs--; + return 0; +} + +u8 rmnet_vnd_get_mux(struct net_device *rmnet_dev) +{ + struct rmnet_priv *priv; + + priv = netdev_priv(rmnet_dev); + return priv->mux_id; +} + +int rmnet_vnd_do_flow_control(struct net_device *rmnet_dev, int enable) +{ + netdev_dbg(rmnet_dev, "Setting VND TX queue state to %d\n", enable); + /* Although we expect similar number of enable/disable + * commands, optimize for the disable. That is more + * latency sensitive than enable + */ + if (unlikely(enable)) + netif_wake_queue(rmnet_dev); + else + netif_stop_queue(rmnet_dev); + + return 0; +} + +void rmnet_vnd_reset_mac_addr(struct net_device *dev) +{ + if (dev->netdev_ops != &rmnet_vnd_ops) + return; + + random_ether_addr(dev->perm_addr); +} diff --git a/techpack/datarmnet/core/rmnet_vnd.h b/techpack/datarmnet/core/rmnet_vnd.h new file mode 100644 index 000000000000..7fa9500a2aca --- /dev/null +++ b/techpack/datarmnet/core/rmnet_vnd.h @@ -0,0 +1,31 @@ +/* Copyright (c) 2013-2018, 2020 The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * RMNET Data Virtual Network Device APIs + * + */ + +#ifndef _RMNET_VND_H_ +#define _RMNET_VND_H_ + +int rmnet_vnd_do_flow_control(struct net_device *dev, int enable); +int rmnet_vnd_newlink(u8 id, struct net_device *rmnet_dev, + struct rmnet_port *port, + struct net_device *real_dev, + struct rmnet_endpoint *ep); +int rmnet_vnd_dellink(u8 id, struct rmnet_port *port, + struct rmnet_endpoint *ep); +void rmnet_vnd_rx_fixup(struct net_device *dev, u32 skb_len); +void rmnet_vnd_tx_fixup(struct net_device *dev, u32 skb_len); +u8 rmnet_vnd_get_mux(struct net_device *rmnet_dev); +void rmnet_vnd_setup(struct net_device *dev); +void rmnet_vnd_reset_mac_addr(struct net_device *dev); +#endif /* _RMNET_VND_H_ */ diff --git a/techpack/datarmnet/core/wda.h b/techpack/datarmnet/core/wda.h new file mode 100644 index 000000000000..b6038ad819af --- /dev/null +++ b/techpack/datarmnet/core/wda.h @@ -0,0 +1,77 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* Copyright (c) 2018, The Linux Foundation. All rights reserved. + */ + +#undef TRACE_SYSTEM +#define TRACE_SYSTEM wda +#undef TRACE_INCLUDE_PATH +#define TRACE_INCLUDE_PATH ../../../../vendor/qcom/opensource/datarmnet/core +#define TRACE_INCLUDE_FILE wda + +#if !defined(_TRACE_WDA_H) || defined(TRACE_HEADER_MULTI_READ) +#define _TRACE_WDA_H + +#include + +TRACE_EVENT(wda_set_powersave_mode, + + TP_PROTO(int enable), + + TP_ARGS(enable), + + TP_STRUCT__entry( + __field(int, enable) + ), + + TP_fast_assign( + __entry->enable = enable; + ), + + TP_printk("set powersave mode to %s", + __entry->enable ? "enable" : "disable") +); + +TRACE_EVENT(wda_client_state_up, + + TP_PROTO(u32 instance, u32 ep_type, u32 iface), + + TP_ARGS(instance, ep_type, iface), + + TP_STRUCT__entry( + __field(u32, instance) + __field(u32, ep_type) + __field(u32, iface) + ), + + TP_fast_assign( + __entry->instance = instance; + __entry->ep_type = ep_type; + __entry->iface = iface; + ), + + TP_printk("Client: Connected with WDA instance=%u ep_type=%u i_id=%u", + __entry->instance, __entry->ep_type, __entry->iface) +); + +TRACE_EVENT(wda_client_state_down, + + TP_PROTO(int from_cb), + + TP_ARGS(from_cb), + + TP_STRUCT__entry( + __field(int, from_cb) + ), + + TP_fast_assign( + __entry->from_cb = from_cb; + ), + + TP_printk("Client: Connection with WDA lost Exit by callback %d", + __entry->from_cb) +); + +#endif /* _TRACE_WDA_H */ + +/* This part must be outside protection */ +#include diff --git a/techpack/datarmnet/core/wda_qmi.c b/techpack/datarmnet/core/wda_qmi.c new file mode 100644 index 000000000000..ab0313a3a782 --- /dev/null +++ b/techpack/datarmnet/core/wda_qmi.c @@ -0,0 +1,479 @@ +/* Copyright (c) 2018-2020, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include +#include +#include "rmnet_qmi.h" +#define CREATE_TRACE_POINTS +#include "wda.h" +#undef CREATE_TRACE_POINTS +#include "qmi_rmnet_i.h" + +struct wda_qmi_data { + void *rmnet_port; + struct workqueue_struct *wda_wq; + struct work_struct svc_arrive; + struct qmi_handle handle; + struct sockaddr_qrtr ssctl; + struct svc_info svc; + int restart_state; +}; + +static void wda_svc_config(struct work_struct *work); +/* **************************************************** */ +#define WDA_SERVICE_ID_V01 0x1A +#define WDA_SERVICE_VERS_V01 0x01 +#define WDA_TIMEOUT_JF msecs_to_jiffies(1000) + +#define QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01 0x002D +#define QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01 0x002D +#define QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01_MAX_MSG_LEN 18 +#define QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01_MAX_MSG_LEN 14 + +#define QMI_WDA_SET_POWERSAVE_MODE_REQ_V01 0x002E +#define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01 0x002E +#define QMI_WDA_SET_POWERSAVE_MODE_REQ_V01_MAX_MSG_LEN 4 +#define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01_MAX_MSG_LEN 7 + +enum wda_powersave_config_mask_enum_v01 { + WDA_DATA_POWERSAVE_CONFIG_MASK_ENUM_MIN_ENUM_VAL_V01 = -2147483647, + WDA_DATA_POWERSAVE_CONFIG_NOT_SUPPORTED = 0x00, + WDA_DATA_POWERSAVE_CONFIG_DL_MARKER_V01 = 0x01, + WDA_DATA_POWERSAVE_CONFIG_FLOW_CTL_V01 = 0x02, + WDA_DATA_POWERSAVE_CONFIG_ALL_MASK_V01 = 0x7FFFFFFF, + WDA_DATA_POWERSAVE_CONFIG_MASK_ENUM_MAX_ENUM_VAL_V01 = 2147483647 +}; + +struct wda_set_powersave_config_req_msg_v01 { + /* Mandatory */ + struct data_ep_id_type_v01 ep_id; + /* Optional */ + uint8_t req_data_cfg_valid; + enum wda_powersave_config_mask_enum_v01 req_data_cfg; +}; + +struct wda_set_powersave_config_resp_msg_v01 { + /* Mandatory */ + struct qmi_response_type_v01 resp; + /* Optional */ + uint8_t data_cfg_valid; + enum wda_powersave_config_mask_enum_v01 data_cfg; +}; + +struct wda_set_powersave_mode_req_msg_v01 { + /* Mandatory */ + uint8_t powersave_control_flag; +}; + +struct wda_set_powersave_mode_resp_msg_v01 { + /* Mandatory */ + struct qmi_response_type_v01 resp; +}; + +static struct qmi_elem_info wda_set_powersave_config_req_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct data_ep_id_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x01, + .offset = offsetof(struct + wda_set_powersave_config_req_msg_v01, + ep_id), + .ei_array = data_ep_id_type_v01_ei, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(uint8_t), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + wda_set_powersave_config_req_msg_v01, + req_data_cfg_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_SIGNED_4_BYTE_ENUM, + .elem_len = 1, + .elem_size = sizeof(enum + wda_powersave_config_mask_enum_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + wda_set_powersave_config_req_msg_v01, + req_data_cfg), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info wda_set_powersave_config_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct + wda_set_powersave_config_resp_msg_v01, + resp), + .ei_array = qmi_response_type_v01_ei, + }, + { + .data_type = QMI_OPT_FLAG, + .elem_len = 1, + .elem_size = sizeof(uint8_t), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + wda_set_powersave_config_resp_msg_v01, + data_cfg_valid), + .ei_array = NULL, + }, + { + .data_type = QMI_SIGNED_4_BYTE_ENUM, + .elem_len = 1, + .elem_size = sizeof(enum + wda_powersave_config_mask_enum_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x10, + .offset = offsetof(struct + wda_set_powersave_config_resp_msg_v01, + data_cfg), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info wda_set_powersave_mode_req_msg_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_1_BYTE, + .elem_len = 1, + .elem_size = sizeof(uint8_t), + .array_type = NO_ARRAY, + .tlv_type = 0x01, + .offset = offsetof(struct + wda_set_powersave_mode_req_msg_v01, + powersave_control_flag), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static struct qmi_elem_info wda_set_powersave_mode_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .array_type = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct + wda_set_powersave_mode_resp_msg_v01, + resp), + .ei_array = qmi_response_type_v01_ei, + }, + { + .data_type = QMI_EOTI, + .array_type = NO_ARRAY, + .tlv_type = QMI_COMMON_TLV_TYPE, + }, +}; + +static int wda_set_powersave_mode_req(void *wda_data, uint8_t enable) +{ + struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data; + struct wda_set_powersave_mode_resp_msg_v01 *resp; + struct wda_set_powersave_mode_req_msg_v01 *req; + struct qmi_txn txn; + int ret; + + if (!data || !data->rmnet_port) + return -EINVAL; + + req = kzalloc(sizeof(*req), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + resp = kzalloc(sizeof(*resp), GFP_ATOMIC); + if (!resp) { + kfree(req); + return -ENOMEM; + } + + ret = qmi_txn_init(&data->handle, &txn, + wda_set_powersave_mode_resp_msg_v01_ei, resp); + if (ret < 0) { + pr_err("%s() Failed init for response, err: %d\n", + __func__, ret); + goto out; + } + + req->powersave_control_flag = enable; + ret = qmi_send_request(&data->handle, &data->ssctl, &txn, + QMI_WDA_SET_POWERSAVE_MODE_REQ_V01, + QMI_WDA_SET_POWERSAVE_MODE_REQ_V01_MAX_MSG_LEN, + wda_set_powersave_mode_req_msg_v01_ei, req); + if (ret < 0) { + qmi_txn_cancel(&txn); + pr_err("%s() Failed sending request, err: %d\n", + __func__, ret); + goto out; + } + + ret = qmi_txn_wait(&txn, WDA_TIMEOUT_JF); + if (ret < 0) { + pr_err("%s() Response waiting failed, err: %d\n", + __func__, ret); + } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) { + pr_err("%s() Request rejected, result: %d, err: %d\n", + __func__, resp->resp.result, resp->resp.error); + ret = -resp->resp.result; + } + +out: + kfree(resp); + kfree(req); + return ret; +} + +static int wda_set_powersave_config_req(struct qmi_handle *wda_handle, + int dl_marker) +{ + struct wda_qmi_data *data = container_of(wda_handle, + struct wda_qmi_data, handle); + struct wda_set_powersave_config_resp_msg_v01 *resp; + struct wda_set_powersave_config_req_msg_v01 *req; + struct qmi_txn txn; + int ret; + + req = kzalloc(sizeof(*req), GFP_ATOMIC); + if (!req) + return -ENOMEM; + + resp = kzalloc(sizeof(*resp), GFP_ATOMIC); + if (!resp) { + kfree(req); + return -ENOMEM; + } + + ret = qmi_txn_init(wda_handle, &txn, + wda_set_powersave_config_resp_msg_v01_ei, resp); + if (ret < 0) { + pr_err("%s() Failed init for response, err: %d\n", + __func__, ret); + goto out; + } + + req->ep_id.ep_type = data->svc.ep_type; + req->ep_id.iface_id = data->svc.iface_id; + req->req_data_cfg_valid = 1; + req->req_data_cfg = dl_marker ? WDA_DATA_POWERSAVE_CONFIG_ALL_MASK_V01 : + WDA_DATA_POWERSAVE_CONFIG_FLOW_CTL_V01; + ret = qmi_send_request(wda_handle, &data->ssctl, &txn, + QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01, + QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01_MAX_MSG_LEN, + wda_set_powersave_config_req_msg_v01_ei, req); + if (ret < 0) { + qmi_txn_cancel(&txn); + pr_err("%s() Failed sending request, err: %d\n", __func__, ret); + goto out; + } + + ret = qmi_txn_wait(&txn, WDA_TIMEOUT_JF); + if (ret < 0) { + pr_err("%s() Response waiting failed, err: %d\n", + __func__, ret); + } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) { + pr_err("%s() Request rejected, result: %d, error: %d\n", + __func__, resp->resp.result, resp->resp.error); + ret = -resp->resp.result; + } + +out: + kfree(resp); + kfree(req); + return ret; +} + +static void wda_svc_config(struct work_struct *work) +{ + struct wda_qmi_data *data = container_of(work, struct wda_qmi_data, + svc_arrive); + struct qmi_info *qmi; + int rc, dl_marker = 0; + + while (!rtnl_trylock()) { + if (!data->restart_state) + cond_resched(); + else + return; + } + + dl_marker = rmnet_get_dlmarker_info(data->rmnet_port); + rtnl_unlock(); + + if (data->restart_state == 1) + return; + + rc = wda_set_powersave_config_req(&data->handle, dl_marker); + if (rc < 0) { + pr_err("%s Failed to init service, err[%d]\n", __func__, rc); + return; + } + + if (data->restart_state == 1) + return; + while (!rtnl_trylock()) { + if (!data->restart_state) + cond_resched(); + else + return; + } + + qmi = (struct qmi_info *)rmnet_get_qmi_pt(data->rmnet_port); + if (!qmi) { + rtnl_unlock(); + return; + } + + qmi->wda_pending = NULL; + qmi->wda_client = (void *)data; + trace_wda_client_state_up(data->svc.instance, + data->svc.ep_type, + data->svc.iface_id); + + rtnl_unlock(); + + pr_info("Connection established with the WDA Service, DL Marker %s\n", + dl_marker ? "enabled" : "disabled"); +} + +static int wda_svc_arrive(struct qmi_handle *qmi, struct qmi_service *svc) +{ + struct wda_qmi_data *data = container_of(qmi, struct wda_qmi_data, + handle); + + data->ssctl.sq_family = AF_QIPCRTR; + data->ssctl.sq_node = svc->node; + data->ssctl.sq_port = svc->port; + + queue_work(data->wda_wq, &data->svc_arrive); + + return 0; +} + +static void wda_svc_exit(struct qmi_handle *qmi, struct qmi_service *svc) +{ + struct wda_qmi_data *data = container_of(qmi, struct wda_qmi_data, + handle); + + if (!data) + pr_info("%s() data is null\n", __func__); +} + +static struct qmi_ops server_ops = { + .new_server = wda_svc_arrive, + .del_server = wda_svc_exit, +}; + +int +wda_qmi_client_init(void *port, struct svc_info *psvc, struct qmi_info *qmi) +{ + struct wda_qmi_data *data; + int rc = -ENOMEM; + + if (!port || !qmi) + return -EINVAL; + + data = kzalloc(sizeof(*data), GFP_KERNEL); + if (!data) + return -ENOMEM; + + data->wda_wq = create_singlethread_workqueue("wda_wq"); + if (!data->wda_wq) { + pr_err("%s Could not create workqueue\n", __func__); + goto err0; + } + + data->rmnet_port = port; + data->restart_state = 0; + memcpy(&data->svc, psvc, sizeof(data->svc)); + INIT_WORK(&data->svc_arrive, wda_svc_config); + + rc = qmi_handle_init(&data->handle, + QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01_MAX_MSG_LEN, + &server_ops, NULL); + if (rc < 0) { + pr_err("%s: Failed qmi_handle_init, err: %d\n", __func__, rc); + goto err1; + } + + rc = qmi_add_lookup(&data->handle, WDA_SERVICE_ID_V01, + WDA_SERVICE_VERS_V01, psvc->instance); + if (rc < 0) { + pr_err("%s(): Failed qmi_add_lookup, err: %d\n", __func__, rc); + goto err2; + } + + qmi->wda_pending = (void *)data; + return 0; + +err2: + qmi_handle_release(&data->handle); +err1: + destroy_workqueue(data->wda_wq); +err0: + kfree(data); + return rc; +} + +void wda_qmi_client_exit(void *wda_data) +{ + struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data; + + if (!data) { + pr_info("%s() data is null\n", __func__); + return; + } + + data->restart_state = 1; + trace_wda_client_state_down(0); + destroy_workqueue(data->wda_wq); + kfree(data); +} + +int wda_set_powersave_mode(void *wda_data, uint8_t enable) +{ + trace_wda_set_powersave_mode(enable); + return wda_set_powersave_mode_req(wda_data, enable); +} + +void wda_qmi_client_release(void *wda_data) +{ + if (!wda_data) + return; + qmi_handle_release(&((struct wda_qmi_data *)wda_data)->handle); +} diff --git a/techpack/datarmnet/datarmnet_dlkm_vendor_board.mk b/techpack/datarmnet/datarmnet_dlkm_vendor_board.mk new file mode 100644 index 000000000000..60a81a7d351b --- /dev/null +++ b/techpack/datarmnet/datarmnet_dlkm_vendor_board.mk @@ -0,0 +1,13 @@ +#Build rmnet core +DATA_DLKM_BOARD_PLATFORMS_LIST := lahaina +DATA_DLKM_BOARD_PLATFORMS_LIST += holi +DATA_DLKM_BOARD_PLATFORMS_LIST += monaco +DATA_DLKM_BOARD_PLATFORMS_LIST += monaco_go +DATA_DLKM_BOARD_PLATFORMS_LIST += monaco_go_aon +ifneq ($(TARGET_BOARD_AUTO),true) +ifeq ($(call is-board-platform-in-list,$(DATA_DLKM_BOARD_PLATFORMS_LIST)),true) +BOARD_VENDOR_KERNEL_MODULES += $(KERNEL_MODULES_OUT)/rmnet_core.ko +BOARD_VENDOR_KERNEL_MODULES += $(KERNEL_MODULES_OUT)/rmnet_ctl.ko +endif +endif + diff --git a/techpack/datarmnet/datarmnet_dlkm_vendor_product.mk b/techpack/datarmnet/datarmnet_dlkm_vendor_product.mk new file mode 100644 index 000000000000..33343754c1e6 --- /dev/null +++ b/techpack/datarmnet/datarmnet_dlkm_vendor_product.mk @@ -0,0 +1,2 @@ +PRODUCT_PACKAGES += rmnet_core.ko +PRODUCT_PACKAGES += rmnet_ctl.ko