remoteproc: qcom: Enable in-kernel sns-reg

Request in-kernel Sensors Registry to be started along with ADSP or
SLPI, and release it once DSP is stopped. Once all DSPs are
stopped, the Sensors Registry will be stopped too.

Signed-off-by: Alexey Minnekhanov <alexeymin@postmarketos.org>

Author: minlexx

drivers/remoteproc/qcom_common.c

@@ -27,6 +27,7 @@
 #define to_smd_subdev(d) container_of(d, struct qcom_rproc_subdev, subdev)
 #define to_ssr_subdev(d) container_of(d, struct qcom_rproc_ssr, subdev)
 #define to_pdm_subdev(d) container_of(d, struct qcom_rproc_pdm, subdev)
+#define to_sns_reg_subdev(d) container_of(d, struct qcom_rproc_sns_reg, subdev)
 
 #define MAX_NUM_OF_SS           10
 #define MAX_REGION_NAME_LENGTH  16
@@ -641,5 +642,83 @@ void qcom_remove_pdm_subdev(struct rproc *rproc, struct qcom_rproc_pdm *pdm)
 }
 EXPORT_SYMBOL_GPL(qcom_remove_pdm_subdev);
 
+static int sns_reg_prepare(struct rproc_subdev *subdev)
+{
+	struct qcom_rproc_sns_reg *sns_reg = to_sns_reg_subdev(subdev);
+	struct auxiliary_device *adev;
+	int ret;
+
+	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
+	if (!adev)
+		return -ENOMEM;
+
+	adev->dev.parent = sns_reg->rproc_dev;
+	/* yes, we can reuse release helper from PDM */
+	adev->dev.release = pdm_dev_release;
+	adev->name = "sns-reg";
+	adev->id = sns_reg->rproc_index;
+
+	ret = auxiliary_device_init(adev);
+	if (ret) {
+		kfree(adev);
+		return ret;
+	}
+
+	ret = auxiliary_device_add(adev);
+	if (ret) {
+		auxiliary_device_uninit(adev);
+		return ret;
+	}
+
+	sns_reg->adev = adev;
+
+	return 0;
+}
+
+static void sns_reg_unprepare(struct rproc_subdev *subdev)
+{
+	struct qcom_rproc_sns_reg *sns_reg = to_sns_reg_subdev(subdev);
+
+	if (!sns_reg->adev)
+		return;
+
+	auxiliary_device_delete(sns_reg->adev);
+	auxiliary_device_uninit(sns_reg->adev);
+	sns_reg->adev = NULL;
+}
+
+/**
+ * qcom_add_sns_reg_subdev() - register sns-reg subdevice
+ * @rproc:	rproc handle
+ * @sns_reg:	sns-reg subdevice handle
+ *
+ * Register @sns_reg so that Sensor Registry service is started when the
+ * DSP is started.
+ */
+void qcom_add_sns_reg_subdev(struct rproc *rproc, struct qcom_rproc_sns_reg *sns_reg)
+{
+	sns_reg->rproc_dev = &rproc->dev;
+	sns_reg->rproc_index = rproc->index;
+
+	sns_reg->subdev.prepare = sns_reg_prepare;
+	sns_reg->subdev.unprepare = sns_reg_unprepare;
+
+	rproc_add_subdev(rproc, &sns_reg->subdev);
+}
+EXPORT_SYMBOL_GPL(qcom_add_sns_reg_subdev);
+
+/**
+ * qcom_remove_sns_reg_subdev() - remove sns-reg subdevice
+ * @rproc:	rproc handle
+ * @sns_reg:	sns-reg subdevice handle
+ *
+ * Remove the Sensor Registry subdevice.
+ */
+void qcom_remove_sns_reg_subdev(struct rproc *rproc, struct qcom_rproc_sns_reg *sns_reg)
+{
+	rproc_remove_subdev(rproc, &sns_reg->subdev);
+}
+EXPORT_SYMBOL_GPL(qcom_remove_sns_reg_subdev);
+
 MODULE_DESCRIPTION("Qualcomm Remoteproc helper driver");
 MODULE_LICENSE("GPL v2");

drivers/remoteproc/qcom_common.h

@@ -41,6 +41,13 @@ struct qcom_rproc_pdm {
 	struct auxiliary_device *adev;
 };
 
+struct qcom_rproc_sns_reg {
+	struct rproc_subdev subdev;
+	struct device *rproc_dev;
+	int rproc_index;
+	struct auxiliary_device *adev;
+};
+
 void qcom_minidump(struct rproc *rproc, unsigned int minidump_id,
 			void (*rproc_dumpfn_t)(struct rproc *rproc,
 				struct rproc_dump_segment *segment, void *dest, size_t offset,
@@ -62,6 +69,9 @@ void qcom_remove_ssr_subdev(struct rproc *rproc, struct qcom_rproc_ssr *ssr);
 void qcom_add_pdm_subdev(struct rproc *rproc, struct qcom_rproc_pdm *pdm);
 void qcom_remove_pdm_subdev(struct rproc *rproc, struct qcom_rproc_pdm *pdm);
 
+void qcom_add_sns_reg_subdev(struct rproc *rproc, struct qcom_rproc_sns_reg *sns_reg);
+void qcom_remove_sns_reg_subdev(struct rproc *rproc, struct qcom_rproc_sns_reg *sns_reg);
+
 #if IS_ENABLED(CONFIG_QCOM_SYSMON)
 struct qcom_sysmon *qcom_add_sysmon_subdev(struct rproc *rproc,
 					   const char *name,

drivers/remoteproc/qcom_q6v5_pas.c

@@ -113,6 +113,7 @@ struct qcom_adsp {
 	struct qcom_rproc_subdev smd_subdev;
 	struct qcom_rproc_pdm pdm_subdev;
 	struct qcom_rproc_ssr ssr_subdev;
+	struct qcom_rproc_sns_reg sns_reg_subdev;
 	struct qcom_sysmon *sysmon;
 
 	struct qcom_scm_pas_metadata pas_metadata;
@@ -771,6 +772,7 @@ static int adsp_probe(struct platform_device *pdev)
 	qcom_add_glink_subdev(rproc, &adsp->glink_subdev, desc->ssr_name);
 	qcom_add_smd_subdev(rproc, &adsp->smd_subdev);
 	qcom_add_pdm_subdev(rproc, &adsp->pdm_subdev);
+	qcom_add_sns_reg_subdev(rproc, &adsp->sns_reg_subdev);
 	adsp->sysmon = qcom_add_sysmon_subdev(rproc,
 					      desc->sysmon_name,
 					      desc->ssctl_id);
@@ -790,6 +792,7 @@ static int adsp_probe(struct platform_device *pdev)
 	qcom_remove_ssr_subdev(rproc, &adsp->ssr_subdev);
 	qcom_remove_sysmon_subdev(adsp->sysmon);
 deinit_remove_pdm_smd_glink:
+	qcom_remove_sns_reg_subdev(rproc, &adsp->sns_reg_subdev);
 	qcom_remove_pdm_subdev(rproc, &adsp->pdm_subdev);
 	qcom_remove_smd_subdev(rproc, &adsp->smd_subdev);
 	qcom_remove_glink_subdev(rproc, &adsp->glink_subdev);
@@ -816,6 +819,7 @@ static void adsp_remove(struct platform_device *pdev)
 	qcom_remove_sysmon_subdev(adsp->sysmon);
 	qcom_remove_smd_subdev(adsp->rproc, &adsp->smd_subdev);
 	qcom_remove_pdm_subdev(adsp->rproc, &adsp->pdm_subdev);
+	qcom_remove_sns_reg_subdev(adsp->rproc, &adsp->sns_reg_subdev);
 	qcom_remove_ssr_subdev(adsp->rproc, &adsp->ssr_subdev);
 	adsp_pds_detach(adsp, adsp->proxy_pds, adsp->proxy_pd_count);
 	device_init_wakeup(adsp->dev, false);