From 95099d4622f8cb224d94e314c7a8e0df60b13f87 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Sat, 09 Dec 2023 08:38:01 +0000
Subject: [PATCH] enable docker ppp
---
kernel/drivers/soc/qcom/apr.c | 207 +++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 189 insertions(+), 18 deletions(-)
diff --git a/kernel/drivers/soc/qcom/apr.c b/kernel/drivers/soc/qcom/apr.c
index ee9197f..f736d20 100644
--- a/kernel/drivers/soc/qcom/apr.c
+++ b/kernel/drivers/soc/qcom/apr.c
@@ -8,8 +8,10 @@
#include <linux/spinlock.h>
#include <linux/idr.h>
#include <linux/slab.h>
+#include <linux/workqueue.h>
#include <linux/of_device.h>
#include <linux/soc/qcom/apr.h>
+#include <linux/soc/qcom/pdr.h>
#include <linux/rpmsg.h>
#include <linux/of.h>
@@ -17,8 +19,19 @@
struct rpmsg_endpoint *ch;
struct device *dev;
spinlock_t svcs_lock;
+ spinlock_t rx_lock;
struct idr svcs_idr;
int dest_domain_id;
+ struct pdr_handle *pdr;
+ struct workqueue_struct *rxwq;
+ struct work_struct rx_work;
+ struct list_head rx_list;
+};
+
+struct apr_rx_buf {
+ struct list_head node;
+ int len;
+ uint8_t buf[];
};
/**
@@ -62,11 +75,7 @@
int len, void *priv, u32 addr)
{
struct apr *apr = dev_get_drvdata(&rpdev->dev);
- uint16_t hdr_size, msg_type, ver, svc_id;
- struct apr_device *svc = NULL;
- struct apr_driver *adrv = NULL;
- struct apr_resp_pkt resp;
- struct apr_hdr *hdr;
+ struct apr_rx_buf *abuf;
unsigned long flags;
if (len <= APR_HDR_SIZE) {
@@ -74,6 +83,34 @@
buf, len);
return -EINVAL;
}
+
+ abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC);
+ if (!abuf)
+ return -ENOMEM;
+
+ abuf->len = len;
+ memcpy(abuf->buf, buf, len);
+
+ spin_lock_irqsave(&apr->rx_lock, flags);
+ list_add_tail(&abuf->node, &apr->rx_list);
+ spin_unlock_irqrestore(&apr->rx_lock, flags);
+
+ queue_work(apr->rxwq, &apr->rx_work);
+
+ return 0;
+}
+
+
+static int apr_do_rx_callback(struct apr *apr, struct apr_rx_buf *abuf)
+{
+ uint16_t hdr_size, msg_type, ver, svc_id;
+ struct apr_device *svc = NULL;
+ struct apr_driver *adrv = NULL;
+ struct apr_resp_pkt resp;
+ struct apr_hdr *hdr;
+ unsigned long flags;
+ void *buf = abuf->buf;
+ int len = abuf->len;
hdr = buf;
ver = APR_HDR_FIELD_VER(hdr->hdr_field);
@@ -87,7 +124,7 @@
}
if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {
- dev_err(apr->dev, "APR: Wrong paket size\n");
+ dev_err(apr->dev, "APR: Wrong packet size\n");
return -EINVAL;
}
@@ -130,6 +167,23 @@
adrv->callback(svc, &resp);
return 0;
+}
+
+static void apr_rxwq(struct work_struct *work)
+{
+ struct apr *apr = container_of(work, struct apr, rx_work);
+ struct apr_rx_buf *abuf, *b;
+ unsigned long flags;
+
+ if (!list_empty(&apr->rx_list)) {
+ list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {
+ apr_do_rx_callback(apr, abuf);
+ spin_lock_irqsave(&apr->rx_lock, flags);
+ list_del(&abuf->node);
+ spin_unlock_irqrestore(&apr->rx_lock, flags);
+ kfree(abuf);
+ }
+ }
}
static int apr_device_match(struct device *dev, struct device_driver *drv)
@@ -219,7 +273,7 @@
adev->domain_id = id->domain_id;
adev->version = id->svc_version;
if (np)
- strscpy(adev->name, np->name, APR_NAME_SIZE);
+ snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
else
strscpy(adev->name, id->name, APR_NAME_SIZE);
@@ -237,6 +291,9 @@
id->svc_id + 1, GFP_ATOMIC);
spin_unlock(&apr->svcs_lock);
+ of_property_read_string_index(np, "qcom,protection-domain",
+ 1, &adev->service_path);
+
dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
ret = device_register(&adev->dev);
@@ -248,13 +305,76 @@
return ret;
}
-static void of_register_apr_devices(struct device *dev)
+static int of_apr_add_pd_lookups(struct device *dev)
+{
+ const char *service_name, *service_path;
+ struct apr *apr = dev_get_drvdata(dev);
+ struct device_node *node;
+ struct pdr_service *pds;
+ int ret;
+
+ for_each_child_of_node(dev->of_node, node) {
+ ret = of_property_read_string_index(node, "qcom,protection-domain",
+ 0, &service_name);
+ if (ret < 0)
+ continue;
+
+ ret = of_property_read_string_index(node, "qcom,protection-domain",
+ 1, &service_path);
+ if (ret < 0) {
+ dev_err(dev, "pdr service path missing: %d\n", ret);
+ of_node_put(node);
+ return ret;
+ }
+
+ pds = pdr_add_lookup(apr->pdr, service_name, service_path);
+ if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
+ dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));
+ of_node_put(node);
+ return PTR_ERR(pds);
+ }
+ }
+
+ return 0;
+}
+
+static void of_register_apr_devices(struct device *dev, const char *svc_path)
{
struct apr *apr = dev_get_drvdata(dev);
struct device_node *node;
+ const char *service_path;
+ int ret;
for_each_child_of_node(dev->of_node, node) {
struct apr_device_id id = { {0} };
+
+ /*
+ * This function is called with svc_path NULL during
+ * apr_probe(), in which case we register any apr devices
+ * without a qcom,protection-domain specified.
+ *
+ * Then as the protection domains becomes available
+ * (if applicable) this function is again called, but with
+ * svc_path representing the service becoming available. In
+ * this case we register any apr devices with a matching
+ * qcom,protection-domain.
+ */
+
+ ret = of_property_read_string_index(node, "qcom,protection-domain",
+ 1, &service_path);
+ if (svc_path) {
+ /* skip APR services that are PD independent */
+ if (ret)
+ continue;
+
+ /* skip APR services whose PD paths don't match */
+ if (strcmp(service_path, svc_path))
+ continue;
+ } else {
+ /* skip APR services whose PD lookups are registered */
+ if (ret == 0)
+ continue;
+ }
if (of_property_read_u32(node, "reg", &id.svc_id))
continue;
@@ -263,6 +383,34 @@
if (apr_add_device(dev, node, &id))
dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
+ }
+}
+
+static int apr_remove_device(struct device *dev, void *svc_path)
+{
+ struct apr_device *adev = to_apr_device(dev);
+
+ if (svc_path && adev->service_path) {
+ if (!strcmp(adev->service_path, (char *)svc_path))
+ device_unregister(&adev->dev);
+ } else {
+ device_unregister(&adev->dev);
+ }
+
+ return 0;
+}
+
+static void apr_pd_status(int state, char *svc_path, void *priv)
+{
+ struct apr *apr = (struct apr *)priv;
+
+ switch (state) {
+ case SERVREG_SERVICE_STATE_UP:
+ of_register_apr_devices(apr->dev, svc_path);
+ break;
+ case SERVREG_SERVICE_STATE_DOWN:
+ device_for_each_child(apr->dev, svc_path, apr_remove_device);
+ break;
}
}
@@ -276,7 +424,7 @@
if (!apr)
return -ENOMEM;
- ret = of_property_read_u32(dev->of_node, "reg", &apr->dest_domain_id);
+ ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", &apr->dest_domain_id);
if (ret) {
dev_err(dev, "APR Domain ID not specified in DT\n");
return ret;
@@ -285,25 +433,48 @@
dev_set_drvdata(dev, apr);
apr->ch = rpdev->ept;
apr->dev = dev;
+ apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
+ if (!apr->rxwq) {
+ dev_err(apr->dev, "Failed to start Rx WQ\n");
+ return -ENOMEM;
+ }
+ INIT_WORK(&apr->rx_work, apr_rxwq);
+
+ apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
+ if (IS_ERR(apr->pdr)) {
+ dev_err(dev, "Failed to init PDR handle\n");
+ ret = PTR_ERR(apr->pdr);
+ goto destroy_wq;
+ }
+
+ INIT_LIST_HEAD(&apr->rx_list);
+ spin_lock_init(&apr->rx_lock);
spin_lock_init(&apr->svcs_lock);
idr_init(&apr->svcs_idr);
- of_register_apr_devices(dev);
+
+ ret = of_apr_add_pd_lookups(dev);
+ if (ret)
+ goto handle_release;
+
+ of_register_apr_devices(dev, NULL);
return 0;
-}
-static int apr_remove_device(struct device *dev, void *null)
-{
- struct apr_device *adev = to_apr_device(dev);
-
- device_unregister(&adev->dev);
-
- return 0;
+handle_release:
+ pdr_handle_release(apr->pdr);
+destroy_wq:
+ destroy_workqueue(apr->rxwq);
+ return ret;
}
static void apr_remove(struct rpmsg_device *rpdev)
{
+ struct apr *apr = dev_get_drvdata(&rpdev->dev);
+
+ pdr_handle_release(apr->pdr);
device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
+ flush_workqueue(apr->rxwq);
+ destroy_workqueue(apr->rxwq);
}
/*
--
Gitblit v1.6.2