android_kernel_samsung_sm8650-modules/core/dfc_qmap.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Copyright (c) 2019-2021, The Linux Foundation. All rights reserved.
 * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
 */

#include <net/pkt_sched.h>
#include <linux/module.h>
#include "rmnet_qmap.h"
#include "dfc_defs.h"
#include "rmnet_qmi.h"
#include "qmi_rmnet.h"
#include "dfc.h"
#include "rmnet_map.h"

#define QMAP_DFC_VER		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			ll_status:1;
	u8			reserved4:3;
	__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);

struct qmapv5_cmd_hdr {
	u8	pad_len:6;
	u8	next_hdr:1;
	u8	cd_bit:1;
	u8	mux_id;
	__be16	pkt_len;
	struct rmnet_map_v5_csum_header csum_hdr;
	u8	cmd_name;
	u8	cmd_type:2;
	u8	reserved:6;
	u16	reserved2;
	__be32	tx_id;
} __aligned(1);

struct qmapv5_dfc_end_marker_cnf {
	struct qmapv5_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_powersave_req {
	struct qmap_cmd_hdr	hdr;
	u8			cmd_ver;
	u8			allow:1;
	u8			autoshut:1;
	u8			reserved:6;
	u8			reserved2;
	u8			mode:1;
	u8			reserved3:7;
	__be32			ep_type;
	__be32			iface_id;
	u8			num_bearers;
	u8			bearer_id[PS_MAX_BEARERS];
	u8			reserved4[3];
} __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 bool dfc_config_acked;

static void dfc_qmap_send_config(struct dfc_qmi_data *data);
static void dfc_qmap_send_end_marker_cnf(struct qos_info *qos,
					 struct rmnet_bearer_map *bearer,
					 u16 seq, u32 tx_id);

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);
	qmap_flow_ind.flow_status[0].ll_status = cmd->ll_status;

	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 int 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;
	int rc = QMAP_CMD_ACK;

	dev = rmnet_get_rmnet_dev(dfc->rmnet_port, mux_id);
	if (!dev)
		return rc;

	qos = (struct qos_info *)rmnet_get_qos_pt(dev);
	if (!qos)
		return rc;

	spin_lock_bh(&qos->qos_lock);

	bearer = qmi_rmnet_get_bearer_map(qos, bearer_id);
	if (!bearer) {
		spin_unlock_bh(&qos->qos_lock);
		return rc;
	}

	if (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, seq_num, tx_id);
	}

	spin_unlock_bh(&qos->qos_lock);

	return QMAP_CMD_DONE;
}

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;
	return dfc_qmap_set_end_marker(dfc, cmd->hdr.mux_id, cmd->bearer_id,
				       ntohs(cmd->seq_num),
				       ntohl(cmd->hdr.tx_id));
}

int dfc_qmap_cmd_handler(struct sk_buff *skb)
{
	struct qmap_cmd_hdr *cmd;
	struct dfc_qmi_data *dfc;
	int rc = QMAP_CMD_DONE;

	cmd = (struct qmap_cmd_hdr *)skb->data;

	if (cmd->cmd_name == QMAP_DFC_QUERY) {
		if (cmd->cmd_type != QMAP_CMD_ACK)
			return rc;
	} 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;
		return rc;
	}

	dfc = rcu_dereference(qmap_dfc_data);
	if (!dfc || READ_ONCE(dfc->restart_state))
		return rc;

	/* 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:
		if (cmd->cmd_type == QMAP_CMD_REQUEST)
			rc = QMAP_CMD_UNSUPPORTED;
	}

	return rc;
}

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(rmnet_qmap_next_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);

	rmnet_qmap_send(skb, RMNET_CH_CTL, false);
}

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(rmnet_qmap_next_txid());

	dfc_query->cmd_ver = QMAP_DFC_VER;
	dfc_query->bearer_id = bearer_id;

	rmnet_qmap_send(skb, RMNET_CH_CTL, false);
}

static void dfc_qmap_send_end_marker_cnf(struct qos_info *qos,
					 struct rmnet_bearer_map *bearer,
					 u16 seq, u32 tx_id)
{
	struct sk_buff *skb;
	struct qmapv5_dfc_end_marker_cnf *em_cnf;
	unsigned int len = sizeof(struct qmapv5_dfc_end_marker_cnf);

	skb = alloc_skb(len, GFP_ATOMIC);
	if (!skb)
		return;

	em_cnf = (struct qmapv5_dfc_end_marker_cnf *)skb_put(skb, len);
	memset(em_cnf, 0, len);

	em_cnf->hdr.cd_bit = 1;
	em_cnf->hdr.next_hdr = 1;
	em_cnf->hdr.mux_id = qos->mux_id;
	em_cnf->hdr.pkt_len = htons(len -
				    (QMAP_HDR_LEN +
				     sizeof(struct rmnet_map_v5_csum_header)));
	em_cnf->hdr.csum_hdr.header_type = RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD;
	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->bearer_id;
	em_cnf->seq_num = htons(seq);

	rmnet_qmap_send(skb, bearer->ch_switch.current_ch, true);
}

static int dfc_qmap_send_powersave(u8 enable, u8 num_bearers, u8 *bearer_id)
{
	struct sk_buff *skb;
	struct qmap_dfc_powersave_req *dfc_powersave;
	unsigned int len = sizeof(struct qmap_dfc_powersave_req);
	struct dfc_qmi_data *dfc;
	u32 ep_type = 0;
	u32 iface_id = 0;

	rcu_read_lock();
	dfc = rcu_dereference(qmap_dfc_data);
	if (dfc) {
		ep_type = dfc->svc.ep_type;
		iface_id = dfc->svc.iface_id;
	} else {
		rcu_read_unlock();
		return -EINVAL;
	}
	rcu_read_unlock();

	skb = alloc_skb(len, GFP_ATOMIC);
	if (!skb)
		return -ENOMEM;

	skb->protocol = htons(ETH_P_MAP);
	dfc_powersave = (struct qmap_dfc_powersave_req *)skb_put(skb, len);
	memset(dfc_powersave, 0, len);

	dfc_powersave->hdr.cd_bit = 1;
	dfc_powersave->hdr.mux_id = 0;
	dfc_powersave->hdr.pkt_len = htons(len - QMAP_HDR_LEN);
	dfc_powersave->hdr.cmd_name = QMAP_DFC_POWERSAVE;
	dfc_powersave->hdr.cmd_type = QMAP_CMD_REQUEST;
	dfc_powersave->hdr.tx_id =  htonl(rmnet_qmap_next_txid());

	dfc_powersave->cmd_ver = 3;
	dfc_powersave->mode = enable ? 1 : 0;

	if (enable && num_bearers) {
		if (unlikely(num_bearers > PS_MAX_BEARERS))
			num_bearers = PS_MAX_BEARERS;
		dfc_powersave->allow = 1;
		dfc_powersave->autoshut = 1;
		dfc_powersave->num_bearers = num_bearers;
		memcpy(dfc_powersave->bearer_id, bearer_id, num_bearers);
	}

	dfc_powersave->ep_type = htonl(ep_type);
	dfc_powersave->iface_id = htonl(iface_id);

	return rmnet_qmap_send(skb, RMNET_CH_CTL, false);
}

int dfc_qmap_set_powersave(u8 enable, u8 num_bearers, u8 *bearer_id)
{
	trace_dfc_set_powersave_mode(enable);
	return dfc_qmap_send_powersave(enable, num_bearers, bearer_id);
}

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,
						     seq, bearer->ack_txid);
	} else if (type == DFC_ACK_TYPE_THRESHOLD) {
		dfc_qmap_send_query(qos->mux_id, bearer_id);
	}
}

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);

	rmnet_qmap_init(port);

	trace_dfc_client_state_up(data->index, data->svc.instance,
				  data->svc.ep_type, data->svc.iface_id);

	pr_info("DFC QMAP init\n");

	/* Currently if powersave ext is enabled, no need to do dfc config
	 * which only enables tx_info */
	if (qmi->ps_ext) {
		dfc_config_acked = true;
	} else {
		dfc_config_acked = false;
		dfc_qmap_send_config(data);
	}

	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);

	rmnet_qmap_exit();

	WRITE_ONCE(data->restart_state, 1);
	RCU_INIT_POINTER(qmap_dfc_data, NULL);
	synchronize_rcu();

	kfree(data);

	pr_info("DFC QMAP exit\n");
}