dfc: Bearer based QMI powersave

Support bearer based powersave mechanism through QMI WDA.

Change-Id: Ife0685aa56605bb1bb52dacd66a7d235dc3faba8

core/dfc_qmap.c

@@ -12,7 +12,6 @@
 #include "dfc.h"
 
 #define QMAP_DFC_VER		1
-#define QMAP_PS_MAX_BEARERS	32
 
 struct qmap_dfc_config {
 	struct qmap_cmd_hdr	hdr;
@@ -102,7 +101,7 @@ struct qmap_dfc_powersave_req {
 	__be32			ep_type;
 	__be32			iface_id;
 	u8			num_bearers;
-	u8			bearer_id[QMAP_PS_MAX_BEARERS];
+	u8			bearer_id[PS_MAX_BEARERS];
 	u8			reserved4[3];
 } __aligned(1);
 
@@ -440,8 +439,8 @@ static int dfc_qmap_send_powersave(u8 enable, u8 num_bearers, u8 *bearer_id)
 	dfc_powersave->mode = enable ? 1 : 0;
 
 	if (enable && num_bearers) {
-		if (unlikely(num_bearers > QMAP_PS_MAX_BEARERS))
-			num_bearers = QMAP_PS_MAX_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;

core/dfc_qmi.c

@@ -44,6 +44,7 @@ struct dfc_ack_cmd {
 } __aligned(1);
 
 static void dfc_svc_init(struct work_struct *work);
+extern int dfc_ps_ext;
 
 /* **************************************************** */
 #define DFC_SERVICE_ID_V01 0x4E
@@ -775,8 +776,11 @@ dfc_indication_register_req(struct qmi_handle *dfc_handle,
 
 	req->report_flow_status_valid = 1;
 	req->report_flow_status = reg;
-	req->report_tx_link_status_valid = 1;
-	req->report_tx_link_status = reg;
+
+	if (!dfc_ps_ext) {
+		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,

core/qmi_rmnet.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2021, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2021 Qualcomm Innovation Center, Inc. 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
@@ -1203,7 +1204,8 @@ done:
 }
 EXPORT_SYMBOL(qmi_rmnet_ps_ind_deregister);
 
-int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable)
+int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable, u8 num_bearers,
+				 u8 *bearer_id)
 {
 	int rc = -EINVAL;
 	struct qmi_info *qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
@@ -1211,7 +1213,8 @@ int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable)
 	if (!qmi || !qmi->wda_client)
 		return rc;
 
-	rc = wda_set_powersave_mode(qmi->wda_client, enable);
+	rc = wda_set_powersave_mode(qmi->wda_client, enable, num_bearers,
+				    bearer_id);
 	if (rc < 0) {
 		pr_err("%s() failed set powersave mode[%u], err=%d\n",
 			__func__, enable, rc);
@@ -1292,7 +1295,8 @@ static void qmi_rmnet_check_stats(struct work_struct *work)
 		qmi->ps_ignore_grant = false;
 
 		/* Register to get QMI DFC and DL marker */
-		if (qmi_rmnet_set_powersave_mode(real_work->port, 0) < 0)
+		if (qmi_rmnet_set_powersave_mode(real_work->port, 0,
+						 0, NULL) < 0)
 			goto end;
 
 		qmi->ps_enabled = false;
@@ -1317,7 +1321,8 @@ static void qmi_rmnet_check_stats(struct work_struct *work)
 		}
 
 		/* Deregister to suppress QMI DFC and DL marker */
-		if (qmi_rmnet_set_powersave_mode(real_work->port, 1) < 0)
+		if (qmi_rmnet_set_powersave_mode(real_work->port, 1,
+						 0, NULL) < 0)
 			goto end;
 
 		qmi->ps_enabled = true;
@@ -1363,6 +1368,7 @@ static void qmi_rmnet_check_stats_2(struct work_struct *work)
 	u64 rxd, txd;
 	u64 rx, tx;
 	u8 num_bearers;
+	int rc;
 
 	real_work = container_of(to_delayed_work(work),
 				 struct rmnet_powersave_work, work);
@@ -1389,7 +1395,12 @@ static void qmi_rmnet_check_stats_2(struct work_struct *work)
 		qmi->ps_ignore_grant = false;
 
 		/* Out of powersave */
-		if (dfc_qmap_set_powersave(0, 0, NULL))
+		if (dfc_qmap)
+			rc = dfc_qmap_set_powersave(0, 0, NULL);
+		else
+			rc = qmi_rmnet_set_powersave_mode(real_work->port, 0,
+							  0, NULL);
+		if (rc)
 			goto end;
 
 		qmi->ps_enabled = false;
@@ -1413,7 +1424,11 @@ static void qmi_rmnet_check_stats_2(struct work_struct *work)
 					 ps_bearer_id);
 
 		/* Enter powersave */
-		dfc_qmap_set_powersave(1, num_bearers, ps_bearer_id);
+		if (dfc_qmap)
+			dfc_qmap_set_powersave(1, num_bearers, ps_bearer_id);
+		else
+			qmi_rmnet_set_powersave_mode(real_work->port, 1,
+						     num_bearers, ps_bearer_id);
 
 		if (rmnet_get_powersave_notif(real_work->port))
 			qmi_rmnet_ps_on_notify(real_work->port);
@@ -1462,7 +1477,7 @@ void qmi_rmnet_work_init(void *port)
 		return;
 	}
 
-	if (dfc_qmap && dfc_ps_ext)
+	if (dfc_ps_ext)
 		INIT_DEFERRABLE_WORK(&rmnet_work->work,
 				     qmi_rmnet_check_stats_2);
 	else

core/qmi_rmnet.h

@@ -115,7 +115,8 @@ static inline int qmi_rmnet_get_queue(struct net_device *dev,
 #endif
 
 #ifdef CONFIG_QTI_QMI_POWER_COLLAPSE
-int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable);
+int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable, u8 num_bearers,
+				 u8 *bearer_id);
 void qmi_rmnet_work_init(void *port);
 void qmi_rmnet_work_exit(void *port);
 void qmi_rmnet_work_maybe_restart(void *port);
@@ -130,7 +131,8 @@ 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)
+static inline int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable,
+					       u8 num_bearers, u8 *bearer_id)
 {
 	return 0;
 }

core/qmi_rmnet_i.h

@@ -32,6 +32,7 @@
 #define INVALID_MQ 0xFF
 
 #define DFC_MODE_SA 4
+#define PS_MAX_BEARERS 32
 
 #define CONFIG_QTI_QMI_RMNET 1
 #define CONFIG_QTI_QMI_DFC  1
@@ -269,7 +270,8 @@ static int rmnet_ll_switch(struct net_device *dev,
 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);
+int wda_set_powersave_mode(void *wda_data, u8 enable, u8 num_bearers,
+			   u8 *bearer_id);
 void qmi_rmnet_flush_ps_wq(void);
 void wda_qmi_client_release(void *wda_data);
 int dfc_qmap_set_powersave(u8 enable, u8 num_bearers, u8 *bearer_id);
@@ -284,7 +286,8 @@ static inline void wda_qmi_client_exit(void *wda_data)
 {
 }
 
-static inline int wda_set_powersave_mode(void *wda_data, u8 enable)
+static inline int wda_set_powersave_mode(void *wda_data, u8 enable,
+					 u8 num_bearers, u8 *bearer_id)
 {
 	return -EINVAL;
 }

core/wda_qmi.c

@@ -1,4 +1,5 @@
 /* Copyright (c) 2018-2021, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2021 Qualcomm Innovation Center, Inc. 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
@@ -42,7 +43,7 @@ static void wda_svc_config(struct work_struct *work);
 
 #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_REQ_V01_MAX_MSG_LEN 48
 #define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01_MAX_MSG_LEN 7
 
 enum wda_powersave_config_mask_enum_v01 {
@@ -73,6 +74,14 @@ struct wda_set_powersave_config_resp_msg_v01 {
 struct wda_set_powersave_mode_req_msg_v01 {
 	/* Mandatory */
 	uint8_t powersave_control_flag;
+	/* Optional */
+	uint8_t allow_dfc_notify_valid;
+	uint8_t allow_dfc_notify;
+	uint8_t allow_bearer_id_list_valid;
+	uint8_t allow_bearer_id_list_len;
+	uint8_t allow_bearer_id_list[PS_MAX_BEARERS];
+	uint8_t auto_shut_allow_bearer_valid;
+	uint8_t auto_shut_allow_bearer;
 };
 
 struct wda_set_powersave_mode_resp_msg_v01 {
@@ -176,6 +185,83 @@ static struct qmi_elem_info wda_set_powersave_mode_req_msg_v01_ei[] = {
 					   powersave_control_flag),
 		.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
+					   wda_set_powersave_mode_req_msg_v01,
+					   allow_dfc_notify_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
+					   wda_set_powersave_mode_req_msg_v01,
+					   allow_dfc_notify),
+		.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
+					   wda_set_powersave_mode_req_msg_v01,
+					   allow_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	= 0x11,
+		.offset		= offsetof(struct
+					   wda_set_powersave_mode_req_msg_v01,
+					   allow_bearer_id_list_len),
+		.ei_array	= NULL,
+	},
+	{
+		.data_type	= QMI_UNSIGNED_1_BYTE,
+		.elem_len	= PS_MAX_BEARERS,
+		.elem_size	= sizeof(u8),
+		.array_type	= VAR_LEN_ARRAY,
+		.tlv_type	= 0x11,
+		.offset		= offsetof(struct
+					   wda_set_powersave_mode_req_msg_v01,
+					   allow_bearer_id_list),
+		.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
+					   wda_set_powersave_mode_req_msg_v01,
+					   auto_shut_allow_bearer_valid),
+		.ei_array	= NULL,
+	},
+	{
+		.data_type	= QMI_UNSIGNED_1_BYTE,
+		.elem_len	= 1,
+		.elem_size	= sizeof(u8),
+		.array_type	= NO_ARRAY,
+		.tlv_type	= 0x12,
+		.offset		= offsetof(struct
+					   wda_set_powersave_mode_req_msg_v01,
+					   auto_shut_allow_bearer),
+		.ei_array	= NULL,
+	},
 	{
 		.data_type	= QMI_EOTI,
 		.array_type	= NO_ARRAY,
@@ -202,7 +288,8 @@ static struct qmi_elem_info wda_set_powersave_mode_resp_msg_v01_ei[] = {
 	},
 };
 
-static int wda_set_powersave_mode_req(void *wda_data, uint8_t enable)
+static int wda_set_powersave_mode_req(void *wda_data, uint8_t enable,
+				      u8 num_bearers, u8 *bearer_id)
 {
 	struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data;
 	struct wda_set_powersave_mode_resp_msg_v01 *resp;
@@ -232,6 +319,20 @@ static int wda_set_powersave_mode_req(void *wda_data, uint8_t enable)
 	}
 
 	req->powersave_control_flag = enable;
+
+	if (enable && num_bearers && bearer_id &&
+	    num_bearers <= PS_MAX_BEARERS) {
+		req->allow_dfc_notify_valid = 1;
+		req->allow_dfc_notify = 1;
+
+		req->allow_bearer_id_list_valid = 1;
+		req->allow_bearer_id_list_len = num_bearers;
+		memcpy(req->allow_bearer_id_list, bearer_id, num_bearers);
+
+		req->auto_shut_allow_bearer_valid = 1;
+		req->auto_shut_allow_bearer = 1;
+	}
+
 	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,
@@ -465,10 +566,12 @@ void wda_qmi_client_exit(void *wda_data)
 	kfree(data);
 }
 
-int wda_set_powersave_mode(void *wda_data, uint8_t enable)
+int wda_set_powersave_mode(void *wda_data, uint8_t enable, u8 num_bearers,
+			   u8 *bearer_id)
 {
 	trace_wda_set_powersave_mode(enable);
-	return wda_set_powersave_mode_req(wda_data, enable);
+	return wda_set_powersave_mode_req(wda_data, enable, num_bearers,
+					  bearer_id);
 }
 
 void wda_qmi_client_release(void *wda_data)