123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564 |
- // 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");
- }
|