net: qrtr: Prevent wakeups using service id based filter

Filter data from adsp by using service id so that it will not
have wake up capable packets. Service id can be read from
DT file and wakeup could be avoided for corresponding packets.

Change-Id: Ibbb5f74bb153cdd333f8037b498c366d4f2ac5f8
Signed-off-by: Sarannya S <quic_sarannya@quicinc.com>
Signed-off-by: Kaushal Hooda <quic_khooda@quicinc.com>

net/qrtr/haven.c

@@ -612,7 +612,7 @@ static int qrtr_haven_probe(struct platform_device *pdev)
 	INIT_WORK(&qdev->work, qrtr_haven_retry_work);
 
 	qdev->ep.xmit = qrtr_haven_send;
-	ret = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NET_ID_AUTO, false);
+	ret = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NET_ID_AUTO, false, NULL);
 	if (ret)
 		goto register_fail;
 

net/qrtr/mhi.c

@@ -176,7 +176,7 @@ static int qcom_mhi_qrtr_probe(struct mhi_device *mhi_dev,
 
 	dev_set_drvdata(&mhi_dev->dev, qdev);
 
-	rc = qrtr_endpoint_register(&qdev->ep, net_id, rt);
+	rc = qrtr_endpoint_register(&qdev->ep, net_id, rt, NULL);
 	if (rc)
 		return rc;
 

net/qrtr/mhi_dev.c

@@ -182,7 +182,7 @@ static void qrtr_mhi_dev_state_cb(struct mhi_dev_client_cb_data *cb_data)
 			return;
 		}
 
-		rc = qrtr_endpoint_register(&qep->ep, qep->net_id, qep->rt);
+		rc = qrtr_endpoint_register(&qep->ep, qep->net_id, qep->rt, NULL);
 		if (rc) {
 			dev_err(qep->dev, "register failed %d\n", rc);
 			qrtr_mhi_dev_close_channels(qep);

net/qrtr/qrtr.c

@@ -142,6 +142,7 @@ static DEFINE_SPINLOCK(qrtr_port_lock);
 #define QRTR_BACKUP_HI_SIZE	SZ_16K
 #define QRTR_BACKUP_LO_NUM	20
 #define QRTR_BACKUP_LO_SIZE	SZ_1K
+
 static struct sk_buff_head qrtr_backup_lo;
 static struct sk_buff_head qrtr_backup_hi;
 static struct work_struct qrtr_backup_work;
@@ -190,6 +191,8 @@ struct qrtr_node {
 
 	struct wakeup_source *ws;
 	void *ilc;
+
+	u32 nonwake_svc[MAX_NON_WAKE_SVC_LEN];
 };
 
 struct qrtr_tx_flow_waiter {
@@ -838,7 +841,9 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
 	size_t size;
 	unsigned int ver;
 	size_t hdrlen;
-	int errcode;
+	int errcode, i;
+	bool wake = true;
+	int svc_id;
 
 	if (len == 0 || len & 3)
 		return -EINVAL;
@@ -938,9 +943,22 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
 			goto err;
 
 		/* Force wakeup for all packets except for sensors */
-		if (node->nid != 9)
+		if (node->nid != 9 && node->nid != 5)
 			pm_wakeup_ws_event(node->ws, qrtr_wakeup_ms, true);
 
+		if (node->nid == 5) {
+			svc_id = qrtr_get_service_id(cb->src_node, cb->src_port);
+			if (svc_id > 0) {
+				for (i = 0; i < MAX_NON_WAKE_SVC_LEN; i++) {
+					if (svc_id == node->nonwake_svc[i]) {
+						wake = false;
+						break;
+					}
+				}
+			}
+			if (wake)
+				pm_wakeup_ws_event(node->ws, qrtr_wakeup_ms, true);
+		}
 		qrtr_port_put(ipc);
 	}
 
@@ -1149,7 +1167,7 @@ static void qrtr_hello_work(struct kthread_work *work)
  * The specified endpoint must have the xmit function pointer set on call.
  */
 int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id,
-			   bool rt)
+			   bool rt, u32 *svc_arr)
 {
 	struct qrtr_node *node;
 	struct sched_param param = {.sched_priority = 1};
@@ -1180,6 +1198,9 @@ int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id,
 	if (rt)
 		sched_setscheduler(node->task, SCHED_FIFO, &param);
 
+	if (svc_arr)
+		memcpy(node->nonwake_svc, svc_arr, MAX_NON_WAKE_SVC_LEN * sizeof(int));
+
 	mutex_init(&node->qrtr_tx_lock);
 	INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);
 	init_waitqueue_head(&node->resume_tx);

net/qrtr/qrtr.h

@@ -12,6 +12,8 @@ struct sk_buff;
 
 #define QRTR_DEL_PROC_MAGIC	0xe111
 
+#define MAX_NON_WAKE_SVC_LEN    5
+
 /**
  * struct qrtr_endpoint - endpoint handle
  * @xmit: Callback for outgoing packets
@@ -27,7 +29,7 @@ struct qrtr_endpoint {
 };
 
 int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id,
-			   bool rt);
+			   bool rt, u32 *svc_arr);
 
 void qrtr_endpoint_unregister(struct qrtr_endpoint *ep);
 

net/qrtr/smd.c

@@ -64,7 +64,8 @@ static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev)
 	struct qrtr_smd_dev *qdev;
 	u32 net_id;
 	bool rt;
-	int rc;
+	int rc, size;
+	u32 *svc_arr = NULL;
 	pr_info("%s:Entered\n", __func__);
 
 	qdev = devm_kzalloc(&rpdev->dev, sizeof(*qdev), GFP_KERNEL);
@@ -81,7 +82,18 @@ static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev)
 
 	rt = of_property_read_bool(rpdev->dev.of_node, "qcom,low-latency");
 
-	rc = qrtr_endpoint_register(&qdev->ep, net_id, rt);
+	size = of_property_count_u32_elems(rpdev->dev.of_node, "qcom,non-wake-svc");
+
+	if (size > 0) {
+		if (size > MAX_NON_WAKE_SVC_LEN)
+			size = MAX_NON_WAKE_SVC_LEN;
+		svc_arr = kmalloc_array(size, sizeof(u32), GFP_KERNEL);
+
+		of_property_read_u32_array(rpdev->dev.of_node, "qcom,non-wake-svc",
+					   svc_arr, size);
+	}
+	rc = qrtr_endpoint_register(&qdev->ep, net_id, rt, svc_arr);
+
 	if (rc)
 		return rc;