@@ -21,6 +21,7 @@
#include <linux/firmware/qcom/qcom_scm.h>
#include <uapi/misc/fastrpc.h>
#include <linux/of_reserved_mem.h>
+#include <linux/soc/qcom/pdr.h>
#define ADSP_DOMAIN_ID (0)
#define MDSP_DOMAIN_ID (1)
@@ -28,6 +29,7 @@
#define CDSP_DOMAIN_ID (3)
#define FASTRPC_DEV_MAX 4 /* adsp, mdsp, slpi, cdsp*/
#define FASTRPC_MAX_SESSIONS 14
+#define FASTRPC_MAX_SPD 4
#define FASTRPC_MAX_VMIDS 16
#define FASTRPC_ALIGN 128
#define FASTRPC_MAX_FDLIST 16
@@ -104,6 +106,18 @@
#define miscdev_to_fdevice(d) container_of(d, struct fastrpc_device, miscdev)
+#define AUDIO_PDR_SERVICE_LOCATION_CLIENT_NAME "audio_pdr_adsp"
+#define AUDIO_PDR_ADSP_SERVICE_NAME "avs/audio"
+#define ADSP_AUDIOPD_NAME "msm/adsp/audio_pd"
+
+#define SENSORS_PDR_ADSP_SERVICE_LOCATION_CLIENT_NAME "sensors_pdr_adsp"
+#define SENSORS_PDR_ADSP_SERVICE_NAME "tms/servreg"
+#define ADSP_SENSORPD_NAME "msm/adsp/sensor_pd"
+
+#define SENSORS_PDR_SLPI_SERVICE_LOCATION_CLIENT_NAME "sensors_pdr_slpi"
+#define SENSORS_PDR_SLPI_SERVICE_NAME SENSORS_PDR_ADSP_SERVICE_NAME
+#define SLPI_SENSORPD_NAME "msm/slpi/sensor_pd"
+
static const char *domains[FASTRPC_DEV_MAX] = { "adsp", "mdsp",
"sdsp", "cdsp"};
struct fastrpc_phy_page {
@@ -259,6 +273,17 @@ struct fastrpc_session_ctx {
bool valid;
};
+struct fastrpc_static_pd {
+ char *servloc_name;
+ char *spdname;
+ void *pdrhandle;
+ u64 pdrcount;
+ u64 prevpdrcount;
+ atomic_t ispdup;
+ int domain;
+ struct fastrpc_channel_ctx *cctx;
+};
+
struct fastrpc_channel_ctx {
int domain_id;
int sesscount;
@@ -267,6 +292,7 @@ struct fastrpc_channel_ctx {
struct qcom_scm_vmperm vmperms[FASTRPC_MAX_VMIDS];
struct rpmsg_device *rpdev;
struct fastrpc_session_ctx session[FASTRPC_MAX_SESSIONS];
+ struct fastrpc_static_pd spd[FASTRPC_MAX_SPD];
spinlock_t lock;
struct idr ctx_idr;
struct list_head users;
@@ -302,6 +328,7 @@ struct fastrpc_user {
int tgid;
int pd;
bool is_secure_dev;
+ char *servloc_name;
/* Lock for lists */
spinlock_t lock;
/* lock for allocations */
@@ -1260,6 +1287,41 @@ static int fastrpc_mmap_remove_ssr(struct fastrpc_channel_ctx *cctx)
return 0;
}
+static int fastrpc_mmap_remove_pdr(struct fastrpc_user *fl)
+{
+ int i, err = 0, session = -1;
+
+ if (!fl)
+ return -EBADF;
+
+ for (i = 0; i < FASTRPC_MAX_SPD ; i++) {
+ if (!fl->cctx->spd[i].servloc_name)
+ continue;
+ if (!strcmp(fl->servloc_name, fl->cctx->spd[i].servloc_name)) {
+ session = i;
+ break;
+ }
+ }
+
+ if (i >= FASTRPC_MAX_SPD)
+ return -EUSERS;
+
+ if (atomic_read(&fl->cctx->spd[session].ispdup) == 0)
+ return -ENOTCONN;
+
+ if (fl->cctx->spd[session].pdrcount !=
+ fl->cctx->spd[session].prevpdrcount) {
+ err = fastrpc_mmap_remove_ssr(fl->cctx);
+ if (err)
+ dev_info(&fl->cctx->rpdev->dev,
+ "failed to unmap remote heap (err %d)\n", err);
+ fl->cctx->spd[session].prevpdrcount =
+ fl->cctx->spd[session].pdrcount;
+ }
+
+ return err;
+}
+
static int fastrpc_init_create_static_process(struct fastrpc_user *fl,
char __user *argp)
{
@@ -1303,6 +1365,19 @@ static int fastrpc_init_create_static_process(struct fastrpc_user *fl,
goto err_name;
}
+
+ fl->servloc_name = AUDIO_PDR_SERVICE_LOCATION_CLIENT_NAME;
+
+ if (!strcmp(name, "audiopd")) {
+ /*
+ * Remove any previous mappings in case process is trying
+ * to reconnect after a PD restart on remote subsystem.
+ */
+ err = fastrpc_mmap_remove_pdr(fl);
+ if (err)
+ goto err_name;
+ }
+
if (!fl->cctx->staticpd_status) {
err = fastrpc_remote_heap_alloc(fl, fl->sctx->dev, init.memlen, &buf);
if (err)
@@ -1693,6 +1768,12 @@ static int fastrpc_init_attach(struct fastrpc_user *fl, int pd)
args[0].fd = -1;
sc = FASTRPC_SCALARS(FASTRPC_RMID_INIT_ATTACH, 1, 0);
fl->pd = pd;
+ if (pd == SENSORS_PD) {
+ if (fl->cctx->domain_id == ADSP_DOMAIN_ID)
+ fl->servloc_name = SENSORS_PDR_ADSP_SERVICE_LOCATION_CLIENT_NAME;
+ else if (fl->cctx->domain_id == SDSP_DOMAIN_ID)
+ fl->servloc_name = SENSORS_PDR_SLPI_SERVICE_LOCATION_CLIENT_NAME;
+ }
return fastrpc_internal_invoke(fl, true, FASTRPC_INIT_HANDLE,
sc, &args[0]);
@@ -2235,6 +2316,71 @@ static long fastrpc_device_ioctl(struct file *file, unsigned int cmd,
return err;
}
+static void fastrpc_notify_users(struct fastrpc_user *user)
+{
+ struct fastrpc_invoke_ctx *ctx;
+
+ spin_lock(&user->lock);
+ list_for_each_entry(ctx, &user->pending, node) {
+ ctx->retval = -EPIPE;
+ ctx->is_work_done = true;
+ complete(&ctx->work);
+ }
+ spin_unlock(&user->lock);
+}
+
+static void fastrpc_notify_pdr_drivers(struct fastrpc_channel_ctx *cctx,
+ char *servloc_name)
+{
+ struct fastrpc_user *fl;
+ unsigned long flags;
+
+ spin_lock_irqsave(&cctx->lock, flags);
+ list_for_each_entry(fl, &cctx->users, user) {
+ if (fl->servloc_name && !strcmp(servloc_name, fl->servloc_name))
+ fastrpc_notify_users(fl);
+ }
+ spin_unlock_irqrestore(&cctx->lock, flags);
+}
+
+static void fastrpc_pdr_cb(int state, char *service_path, void *priv)
+{
+ struct fastrpc_static_pd *spd = (struct fastrpc_static_pd *)priv;
+ unsigned long flags;
+
+ if (!spd)
+ return;
+
+ switch (state) {
+ case SERVREG_SERVICE_STATE_DOWN:
+ dev_info(&spd->cctx->rpdev->dev,
+ "%s: %s (%s) is down for PDR on %s\n",
+ __func__, spd->spdname,
+ spd->servloc_name,
+ domains[spd->domain]);
+ spin_lock_irqsave(&spd->cctx->lock, flags);
+ spd->pdrcount++;
+ atomic_set(&spd->ispdup, 0);
+ spin_unlock_irqrestore(&spd->cctx->lock, flags);
+ if (!strcmp(spd->servloc_name,
+ AUDIO_PDR_SERVICE_LOCATION_CLIENT_NAME))
+ spd->cctx->staticpd_status = false;
+
+ fastrpc_notify_pdr_drivers(spd->cctx, spd->servloc_name);
+ break;
+ case SERVREG_SERVICE_STATE_UP:
+ dev_info(&spd->cctx->rpdev->dev,
+ "%s: %s (%s) is up for PDR on %s\n",
+ __func__, spd->spdname,
+ spd->servloc_name,
+ domains[spd->domain]);
+ atomic_set(&spd->ispdup, 1);
+ break;
+ default:
+ break;
+ }
+}
+
static const struct file_operations fastrpc_fops = {
.open = fastrpc_device_open,
.release = fastrpc_device_release,
@@ -2356,6 +2502,40 @@ static int fastrpc_device_register(struct device *dev, struct fastrpc_channel_ct
return err;
}
+static int fastrpc_setup_service_locator(struct fastrpc_channel_ctx *cctx, char *client_name,
+ char *service_name, char *service_path, int domain, int spd_session)
+{
+ int err = 0;
+ struct pdr_handle *handle = NULL;
+ struct pdr_service *service = NULL;
+
+ /* Register the service locator's callback function */
+ handle = pdr_handle_alloc(fastrpc_pdr_cb, &cctx->spd[spd_session]);
+ if (IS_ERR(handle)) {
+ err = PTR_ERR(handle);
+ goto bail;
+ }
+ cctx->spd[spd_session].domain = domain;
+ cctx->spd[spd_session].pdrhandle = handle;
+ cctx->spd[spd_session].servloc_name = client_name;
+ cctx->spd[spd_session].spdname = service_path;
+ cctx->spd[spd_session].cctx = cctx;
+ service = pdr_add_lookup(handle, service_name, service_path);
+ if (IS_ERR(service)) {
+ err = PTR_ERR(service);
+ goto bail;
+ }
+ pr_info("fastrpc: %s: pdr_add_lookup enabled for %s (%s, %s)\n",
+ __func__, service_name, client_name, service_path);
+
+bail:
+ if (err) {
+ pr_warn("fastrpc: %s: failed for %s (%s, %s)with err %d\n",
+ __func__, service_name, client_name, service_path, err);
+ }
+ return err;
+}
+
static int fastrpc_rpmsg_probe(struct rpmsg_device *rpdev)
{
struct device *rdev = &rpdev->dev;
@@ -2435,6 +2615,23 @@ static int fastrpc_rpmsg_probe(struct rpmsg_device *rpdev)
goto fdev_error;
}
+ if (domain_id == ADSP_DOMAIN_ID) {
+ err = fastrpc_setup_service_locator(data, AUDIO_PDR_SERVICE_LOCATION_CLIENT_NAME,
+ AUDIO_PDR_ADSP_SERVICE_NAME, ADSP_AUDIOPD_NAME, domain_id, 0);
+ if (err)
+ return err;
+
+ err = fastrpc_setup_service_locator(data, SENSORS_PDR_ADSP_SERVICE_LOCATION_CLIENT_NAME,
+ SENSORS_PDR_ADSP_SERVICE_NAME, ADSP_SENSORPD_NAME, domain_id, 1);
+ if (err)
+ return err;
+ } else if (domain_id == SDSP_DOMAIN_ID) {
+ err = fastrpc_setup_service_locator(data, SENSORS_PDR_SLPI_SERVICE_LOCATION_CLIENT_NAME,
+ SENSORS_PDR_SLPI_SERVICE_NAME, SLPI_SENSORPD_NAME, domain_id, 0);
+ if (err)
+ return err;
+ }
+
kref_init(&data->refcount);
dev_set_drvdata(&rpdev->dev, data);
@@ -2464,18 +2661,6 @@ static int fastrpc_rpmsg_probe(struct rpmsg_device *rpdev)
return err;
}
-static void fastrpc_notify_users(struct fastrpc_user *user)
-{
- struct fastrpc_invoke_ctx *ctx;
-
- spin_lock(&user->lock);
- list_for_each_entry(ctx, &user->pending, node) {
- ctx->retval = -EPIPE;
- complete(&ctx->work);
- }
- spin_unlock(&user->lock);
-}
-
static void fastrpc_rpmsg_remove(struct rpmsg_device *rpdev)
{
struct fastrpc_channel_ctx *cctx = dev_get_drvdata(&rpdev->dev);