drivers/remoteproc/qcom_sysmon.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2017, Linaro Ltd.
   4  */
   5 #include <linux/firmware.h>
   6 #include <linux/module.h>
   7 #include <linux/notifier.h>
   8 #include <linux/slab.h>
   9 #include <linux/interrupt.h>
  10 #include <linux/io.h>
  11 #include <linux/of_irq.h>
  12 #include <linux/of_platform.h>
  13 #include <linux/platform_device.h>
  14 #include <linux/remoteproc/qcom_rproc.h>
  15 #include <linux/rpmsg.h>
  16
  17 #include "qcom_common.h"
  18
  19 static BLOCKING_NOTIFIER_HEAD(sysmon_notifiers);
  20
  21 struct qcom_sysmon {
  22         struct rproc_subdev subdev;
  23         struct rproc *rproc;
  24
  25         struct list_head node;
  26
  27         const char *name;
  28
  29         int shutdown_irq;
  30         int ssctl_version;
  31         int ssctl_instance;
  32
  33         struct notifier_block nb;
  34
  35         struct device *dev;
  36
  37         struct rpmsg_endpoint *ept;
  38         struct completion comp;
  39         struct completion ind_comp;
  40         struct completion shutdown_comp;
  41         struct mutex lock;
  42
  43         bool ssr_ack;
  44
  45         struct qmi_handle qmi;
  46         struct sockaddr_qrtr ssctl;
  47 };
  48
  49 enum {
  50         SSCTL_SSR_EVENT_BEFORE_POWERUP,
  51         SSCTL_SSR_EVENT_AFTER_POWERUP,
  52         SSCTL_SSR_EVENT_BEFORE_SHUTDOWN,
  53         SSCTL_SSR_EVENT_AFTER_SHUTDOWN,
  54 };
  55
  56 static const char * const sysmon_state_string[] = {
  57         [SSCTL_SSR_EVENT_BEFORE_POWERUP]        = "before_powerup",
  58         [SSCTL_SSR_EVENT_AFTER_POWERUP]         = "after_powerup",
  59         [SSCTL_SSR_EVENT_BEFORE_SHUTDOWN]       = "before_shutdown",
  60         [SSCTL_SSR_EVENT_AFTER_SHUTDOWN]        = "after_shutdown",
  61 };
  62
  63 struct sysmon_event {
  64         const char *subsys_name;
  65         u32 ssr_event;
  66 };
  67
  68 static DEFINE_MUTEX(sysmon_lock);
  69 static LIST_HEAD(sysmon_list);
  70
  71 /**
  72  * sysmon_send_event() - send notification of other remote's SSR event
  73  * @sysmon:     sysmon context
  74  * @event:      sysmon event context
  75  */
  76 static void sysmon_send_event(struct qcom_sysmon *sysmon,
  77                               const struct sysmon_event *event)
  78 {
  79         char req[50];
  80         int len;
  81         int ret;
  82
  83         len = snprintf(req, sizeof(req), "ssr:%s:%s", event->subsys_name,
  84                        sysmon_state_string[event->ssr_event]);
  85         if (len >= sizeof(req))
  86                 return;
  87
  88         mutex_lock(&sysmon->lock);
  89         reinit_completion(&sysmon->comp);
  90         sysmon->ssr_ack = false;
  91
  92         ret = rpmsg_send(sysmon->ept, req, len);
  93         if (ret < 0) {
  94                 dev_err(sysmon->dev, "failed to send sysmon event\n");
  95                 goto out_unlock;
  96         }
  97
  98         ret = wait_for_completion_timeout(&sysmon->comp,
  99                                           msecs_to_jiffies(5000));
 100         if (!ret) {
 101                 dev_err(sysmon->dev, "timeout waiting for sysmon ack\n");
 102                 goto out_unlock;
 103         }
 104
 105         if (!sysmon->ssr_ack)
 106                 dev_err(sysmon->dev, "unexpected response to sysmon event\n");
 107
 108 out_unlock:
 109         mutex_unlock(&sysmon->lock);
 110 }
 111
 112 /**
 113  * sysmon_request_shutdown() - request graceful shutdown of remote
 114  * @sysmon:     sysmon context
 115  */
 116 static void sysmon_request_shutdown(struct qcom_sysmon *sysmon)
 117 {
 118         char *req = "ssr:shutdown";
 119         int ret;
 120
 121         mutex_lock(&sysmon->lock);
 122         reinit_completion(&sysmon->comp);
 123         sysmon->ssr_ack = false;
 124
 125         ret = rpmsg_send(sysmon->ept, req, strlen(req) + 1);
 126         if (ret < 0) {
 127                 dev_err(sysmon->dev, "send sysmon shutdown request failed\n");
 128                 goto out_unlock;
 129         }
 130
 131         ret = wait_for_completion_timeout(&sysmon->comp,
 132                                           msecs_to_jiffies(5000));
 133         if (!ret) {
 134                 dev_err(sysmon->dev, "timeout waiting for sysmon ack\n");
 135                 goto out_unlock;
 136         }
 137
 138         if (!sysmon->ssr_ack)
 139                 dev_err(sysmon->dev,
 140                         "unexpected response to sysmon shutdown request\n");
 141
 142 out_unlock:
 143         mutex_unlock(&sysmon->lock);
 144 }
 145
 146 static int sysmon_callback(struct rpmsg_device *rpdev, void *data, int count,
 147                            void *priv, u32 addr)
 148 {
 149         struct qcom_sysmon *sysmon = priv;
 150         const char *ssr_ack = "ssr:ack";
 151         const int ssr_ack_len = strlen(ssr_ack) + 1;
 152
 153         if (!sysmon)
 154                 return -EINVAL;
 155
 156         if (count >= ssr_ack_len && !memcmp(data, ssr_ack, ssr_ack_len))
 157                 sysmon->ssr_ack = true;
 158
 159         complete(&sysmon->comp);
 160
 161         return 0;
 162 }
 163
 164 #define SSCTL_SHUTDOWN_REQ              0x21
 165 #define SSCTL_SHUTDOWN_READY_IND        0x21
 166 #define SSCTL_SUBSYS_EVENT_REQ          0x23
 167
 168 #define SSCTL_MAX_MSG_LEN               7
 169
 170 #define SSCTL_SUBSYS_NAME_LENGTH        15
 171
 172 enum {
 173         SSCTL_SSR_EVENT_FORCED,
 174         SSCTL_SSR_EVENT_GRACEFUL,
 175 };
 176
 177 struct ssctl_shutdown_resp {
 178         struct qmi_response_type_v01 resp;
 179 };
 180
 181 static struct qmi_elem_info ssctl_shutdown_resp_ei[] = {
 182         {
 183                 .data_type      = QMI_STRUCT,
 184                 .elem_len       = 1,
 185                 .elem_size      = sizeof(struct qmi_response_type_v01),
 186                 .array_type     = NO_ARRAY,
 187                 .tlv_type       = 0x02,
 188                 .offset         = offsetof(struct ssctl_shutdown_resp, resp),
 189                 .ei_array       = qmi_response_type_v01_ei,
 190         },
 191         {}
 192 };
 193
 194 struct ssctl_subsys_event_req {
 195         u8 subsys_name_len;
 196         char subsys_name[SSCTL_SUBSYS_NAME_LENGTH];
 197         u32 event;
 198         u8 evt_driven_valid;
 199         u32 evt_driven;
 200 };
 201
 202 static struct qmi_elem_info ssctl_subsys_event_req_ei[] = {
 203         {
 204                 .data_type      = QMI_DATA_LEN,
 205                 .elem_len       = 1,
 206                 .elem_size      = sizeof(uint8_t),
 207                 .array_type     = NO_ARRAY,
 208                 .tlv_type       = 0x01,
 209                 .offset         = offsetof(struct ssctl_subsys_event_req,
 210                                            subsys_name_len),
 211                 .ei_array       = NULL,
 212         },
 213         {
 214                 .data_type      = QMI_UNSIGNED_1_BYTE,
 215                 .elem_len       = SSCTL_SUBSYS_NAME_LENGTH,
 216                 .elem_size      = sizeof(char),
 217                 .array_type     = VAR_LEN_ARRAY,
 218                 .tlv_type       = 0x01,
 219                 .offset         = offsetof(struct ssctl_subsys_event_req,
 220                                            subsys_name),
 221                 .ei_array       = NULL,
 222         },
 223         {
 224                 .data_type      = QMI_SIGNED_4_BYTE_ENUM,
 225                 .elem_len       = 1,
 226                 .elem_size      = sizeof(uint32_t),
 227                 .array_type     = NO_ARRAY,
 228                 .tlv_type       = 0x02,
 229                 .offset         = offsetof(struct ssctl_subsys_event_req,
 230                                            event),
 231                 .ei_array       = NULL,
 232         },
 233         {
 234                 .data_type      = QMI_OPT_FLAG,
 235                 .elem_len       = 1,
 236                 .elem_size      = sizeof(uint8_t),
 237                 .array_type     = NO_ARRAY,
 238                 .tlv_type       = 0x10,
 239                 .offset         = offsetof(struct ssctl_subsys_event_req,
 240                                            evt_driven_valid),
 241                 .ei_array       = NULL,
 242         },
 243         {
 244                 .data_type      = QMI_SIGNED_4_BYTE_ENUM,
 245                 .elem_len       = 1,
 246                 .elem_size      = sizeof(uint32_t),
 247                 .array_type     = NO_ARRAY,
 248                 .tlv_type       = 0x10,
 249                 .offset         = offsetof(struct ssctl_subsys_event_req,
 250                                            evt_driven),
 251                 .ei_array       = NULL,
 252         },
 253         {}
 254 };
 255
 256 struct ssctl_subsys_event_resp {
 257         struct qmi_response_type_v01 resp;
 258 };
 259
 260 static struct qmi_elem_info ssctl_subsys_event_resp_ei[] = {
 261         {
 262                 .data_type      = QMI_STRUCT,
 263                 .elem_len       = 1,
 264                 .elem_size      = sizeof(struct qmi_response_type_v01),
 265                 .array_type     = NO_ARRAY,
 266                 .tlv_type       = 0x02,
 267                 .offset         = offsetof(struct ssctl_subsys_event_resp,
 268                                            resp),
 269                 .ei_array       = qmi_response_type_v01_ei,
 270         },
 271         {}
 272 };
 273
 274 static struct qmi_elem_info ssctl_shutdown_ind_ei[] = {
 275         {}
 276 };
 277
 278 static void sysmon_ind_cb(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,
 279                           struct qmi_txn *txn, const void *data)
 280 {
 281         struct qcom_sysmon *sysmon = container_of(qmi, struct qcom_sysmon, qmi);
 282
 283         complete(&sysmon->ind_comp);
 284 }
 285
 286 static struct qmi_msg_handler qmi_indication_handler[] = {
 287         {
 288                 .type = QMI_INDICATION,
 289                 .msg_id = SSCTL_SHUTDOWN_READY_IND,
 290                 .ei = ssctl_shutdown_ind_ei,
 291                 .decoded_size = 0,
 292                 .fn = sysmon_ind_cb
 293         },
 294         {}
 295 };
 296
 297 /**
 298  * ssctl_request_shutdown() - request shutdown via SSCTL QMI service
 299  * @sysmon:     sysmon context
 300  */
 301 static void ssctl_request_shutdown(struct qcom_sysmon *sysmon)
 302 {
 303         struct ssctl_shutdown_resp resp;
 304         struct qmi_txn txn;
 305         int ret;
 306
 307         reinit_completion(&sysmon->ind_comp);
 308         reinit_completion(&sysmon->shutdown_comp);
 309         ret = qmi_txn_init(&sysmon->qmi, &txn, ssctl_shutdown_resp_ei, &resp);
 310         if (ret < 0) {
 311                 dev_err(sysmon->dev, "failed to allocate QMI txn\n");
 312                 return;
 313         }
 314
 315         ret = qmi_send_request(&sysmon->qmi, &sysmon->ssctl, &txn,
 316                                SSCTL_SHUTDOWN_REQ, 0, NULL, NULL);
 317         if (ret < 0) {
 318                 dev_err(sysmon->dev, "failed to send shutdown request\n");
 319                 qmi_txn_cancel(&txn);
 320                 return;
 321         }
 322
 323         ret = qmi_txn_wait(&txn, 5 * HZ);
 324         if (ret < 0)
 325                 dev_err(sysmon->dev, "failed receiving QMI response\n");
 326         else if (resp.resp.result)
 327                 dev_err(sysmon->dev, "shutdown request failed\n");
 328         else
 329                 dev_dbg(sysmon->dev, "shutdown request completed\n");
 330
 331         if (sysmon->shutdown_irq > 0) {
 332                 ret = wait_for_completion_timeout(&sysmon->shutdown_comp,
 333                                                   10 * HZ);
 334                 if (!ret) {
 335                         ret = try_wait_for_completion(&sysmon->ind_comp);
 336                         if (!ret)
 337                                 dev_err(sysmon->dev,
 338                                         "timeout waiting for shutdown ack\n");
 339                 }
 340         }
 341 }
 342
 343 /**
 344  * ssctl_send_event() - send notification of other remote's SSR event
 345  * @sysmon:     sysmon context
 346  * @event:      sysmon event context
 347  */
 348 static void ssctl_send_event(struct qcom_sysmon *sysmon,
 349                              const struct sysmon_event *event)
 350 {
 351         struct ssctl_subsys_event_resp resp;
 352         struct ssctl_subsys_event_req req;
 353         struct qmi_txn txn;
 354         int ret;
 355
 356         memset(&resp, 0, sizeof(resp));
 357         ret = qmi_txn_init(&sysmon->qmi, &txn, ssctl_subsys_event_resp_ei, &resp);
 358         if (ret < 0) {
 359                 dev_err(sysmon->dev, "failed to allocate QMI txn\n");
 360                 return;
 361         }
 362
 363         memset(&req, 0, sizeof(req));
 364         strlcpy(req.subsys_name, event->subsys_name, sizeof(req.subsys_name));
 365         req.subsys_name_len = strlen(req.subsys_name);
 366         req.event = event->ssr_event;
 367         req.evt_driven_valid = true;
 368         req.evt_driven = SSCTL_SSR_EVENT_FORCED;
 369
 370         ret = qmi_send_request(&sysmon->qmi, &sysmon->ssctl, &txn,
 371                                SSCTL_SUBSYS_EVENT_REQ, 40,
 372                                ssctl_subsys_event_req_ei, &req);
 373         if (ret < 0) {
 374                 dev_err(sysmon->dev, "failed to send shutdown request\n");
 375                 qmi_txn_cancel(&txn);
 376                 return;
 377         }
 378
 379         ret = qmi_txn_wait(&txn, 5 * HZ);
 380         if (ret < 0)
 381                 dev_err(sysmon->dev, "failed receiving QMI response\n");
 382         else if (resp.resp.result)
 383                 dev_err(sysmon->dev, "ssr event send failed\n");
 384         else
 385                 dev_dbg(sysmon->dev, "ssr event send completed\n");
 386 }
 387
 388 /**
 389  * ssctl_new_server() - QMI callback indicating a new service
 390  * @qmi:        QMI handle
 391  * @svc:        service information
 392  *
 393  * Return: 0 if we're interested in this service, -EINVAL otherwise.
 394  */
 395 static int ssctl_new_server(struct qmi_handle *qmi, struct qmi_service *svc)
 396 {
 397         struct qcom_sysmon *sysmon = container_of(qmi, struct qcom_sysmon, qmi);
 398
 399         switch (svc->version) {
 400         case 1:
 401                 if (svc->instance != 0)
 402                         return -EINVAL;
 403                 if (strcmp(sysmon->name, "modem"))
 404                         return -EINVAL;
 405                 break;
 406         case 2:
 407                 if (svc->instance != sysmon->ssctl_instance)
 408                         return -EINVAL;
 409                 break;
 410         default:
 411                 return -EINVAL;
 412         }
 413
 414         sysmon->ssctl_version = svc->version;
 415
 416         sysmon->ssctl.sq_family = AF_QIPCRTR;
 417         sysmon->ssctl.sq_node = svc->node;
 418         sysmon->ssctl.sq_port = svc->port;
 419
 420         svc->priv = sysmon;
 421
 422         return 0;
 423 }
 424
 425 /**
 426  * ssctl_del_server() - QMI callback indicating that @svc is removed
 427  * @qmi:        QMI handle
 428  * @svc:        service information
 429  */
 430 static void ssctl_del_server(struct qmi_handle *qmi, struct qmi_service *svc)
 431 {
 432         struct qcom_sysmon *sysmon = svc->priv;
 433
 434         sysmon->ssctl_version = 0;
 435 }
 436
 437 static const struct qmi_ops ssctl_ops = {
 438         .new_server = ssctl_new_server,
 439         .del_server = ssctl_del_server,
 440 };
 441
 442 static int sysmon_prepare(struct rproc_subdev *subdev)
 443 {
 444         struct qcom_sysmon *sysmon = container_of(subdev, struct qcom_sysmon,
 445                                                   subdev);
 446         struct sysmon_event event = {
 447                 .subsys_name = sysmon->name,
 448                 .ssr_event = SSCTL_SSR_EVENT_BEFORE_POWERUP
 449         };
 450
 451         blocking_notifier_call_chain(&sysmon_notifiers, 0, (void *)&event);
 452
 453         return 0;
 454 }
 455
 456 /**
 457  * sysmon_start() - start callback for the sysmon remoteproc subdevice
 458  * @subdev:     instance of the sysmon subdevice
 459  *
 460  * Inform all the listners of sysmon notifications that the rproc associated
 461  * to @subdev has booted up. The rproc that booted up also needs to know
 462  * which rprocs are already up and running, so send start notifications
 463  * on behalf of all the online rprocs.
 464  */
 465 static int sysmon_start(struct rproc_subdev *subdev)
 466 {
 467         struct qcom_sysmon *sysmon = container_of(subdev, struct qcom_sysmon,
 468                                                   subdev);
 469         struct qcom_sysmon *target;
 470         struct sysmon_event event = {
 471                 .subsys_name = sysmon->name,
 472                 .ssr_event = SSCTL_SSR_EVENT_AFTER_POWERUP
 473         };
 474
 475         blocking_notifier_call_chain(&sysmon_notifiers, 0, (void *)&event);
 476
 477         mutex_lock(&sysmon_lock);
 478         list_for_each_entry(target, &sysmon_list, node) {
 479                 if (target == sysmon ||
 480                     target->rproc->state != RPROC_RUNNING)
 481                         continue;
 482
 483                 event.subsys_name = target->name;
 484
 485                 if (sysmon->ssctl_version == 2)
 486                         ssctl_send_event(sysmon, &event);
 487                 else if (sysmon->ept)
 488                         sysmon_send_event(sysmon, &event);
 489         }
 490         mutex_unlock(&sysmon_lock);
 491
 492         return 0;
 493 }
 494
 495 static void sysmon_stop(struct rproc_subdev *subdev, bool crashed)
 496 {
 497         struct qcom_sysmon *sysmon = container_of(subdev, struct qcom_sysmon, subdev);
 498         struct sysmon_event event = {
 499                 .subsys_name = sysmon->name,
 500                 .ssr_event = SSCTL_SSR_EVENT_BEFORE_SHUTDOWN
 501         };
 502
 503         blocking_notifier_call_chain(&sysmon_notifiers, 0, (void *)&event);
 504
 505         /* Don't request graceful shutdown if we've crashed */
 506         if (crashed)
 507                 return;
 508
 509         if (sysmon->ssctl_version)
 510                 ssctl_request_shutdown(sysmon);
 511         else if (sysmon->ept)
 512                 sysmon_request_shutdown(sysmon);
 513 }
 514
 515 static void sysmon_unprepare(struct rproc_subdev *subdev)
 516 {
 517         struct qcom_sysmon *sysmon = container_of(subdev, struct qcom_sysmon,
 518                                                   subdev);
 519         struct sysmon_event event = {
 520                 .subsys_name = sysmon->name,
 521                 .ssr_event = SSCTL_SSR_EVENT_AFTER_SHUTDOWN
 522         };
 523
 524         blocking_notifier_call_chain(&sysmon_notifiers, 0, (void *)&event);
 525 }
 526
 527 /**
 528  * sysmon_notify() - notify sysmon target of another's SSR
 529  * @nb:         notifier_block associated with sysmon instance
 530  * @event:      unused
 531  * @data:       SSR identifier of the remote that is going down
 532  */
 533 static int sysmon_notify(struct notifier_block *nb, unsigned long event,
 534                          void *data)
 535 {
 536         struct qcom_sysmon *sysmon = container_of(nb, struct qcom_sysmon, nb);
 537         struct rproc *rproc = sysmon->rproc;
 538         struct sysmon_event *sysmon_event = data;
 539
 540         /* Skip non-running rprocs and the originating instance */
 541         if (rproc->state != RPROC_RUNNING ||
 542             !strcmp(sysmon_event->subsys_name, sysmon->name)) {
 543                 dev_dbg(sysmon->dev, "not notifying %s\n", sysmon->name);
 544                 return NOTIFY_DONE;
 545         }
 546
 547         /* Only SSCTL version 2 supports SSR events */
 548         if (sysmon->ssctl_version == 2)
 549                 ssctl_send_event(sysmon, sysmon_event);
 550         else if (sysmon->ept)
 551                 sysmon_send_event(sysmon, sysmon_event);
 552
 553         return NOTIFY_DONE;
 554 }
 555
 556 static irqreturn_t sysmon_shutdown_interrupt(int irq, void *data)
 557 {
 558         struct qcom_sysmon *sysmon = data;
 559
 560         complete(&sysmon->shutdown_comp);
 561
 562         return IRQ_HANDLED;
 563 }
 564
 565 /**
 566  * qcom_add_sysmon_subdev() - create a sysmon subdev for the given remoteproc
 567  * @rproc:      rproc context to associate the subdev with
 568  * @name:       name of this subdev, to use in SSR
 569  * @ssctl_instance: instance id of the ssctl QMI service
 570  *
 571  * Return: A new qcom_sysmon object, or NULL on failure
 572  */
 573 struct qcom_sysmon *qcom_add_sysmon_subdev(struct rproc *rproc,
 574                                            const char *name,
 575                                            int ssctl_instance)
 576 {
 577         struct qcom_sysmon *sysmon;
 578         int ret;
 579
 580         sysmon = kzalloc(sizeof(*sysmon), GFP_KERNEL);
 581         if (!sysmon)
 582                 return ERR_PTR(-ENOMEM);
 583
 584         sysmon->dev = rproc->dev.parent;
 585         sysmon->rproc = rproc;
 586
 587         sysmon->name = name;
 588         sysmon->ssctl_instance = ssctl_instance;
 589
 590         init_completion(&sysmon->comp);
 591         init_completion(&sysmon->ind_comp);
 592         init_completion(&sysmon->shutdown_comp);
 593         mutex_init(&sysmon->lock);
 594
 595         sysmon->shutdown_irq = of_irq_get_byname(sysmon->dev->of_node,
 596                                                  "shutdown-ack");
 597         if (sysmon->shutdown_irq < 0) {
 598                 if (sysmon->shutdown_irq != -ENODATA) {
 599                         dev_err(sysmon->dev,
 600                                 "failed to retrieve shutdown-ack IRQ\n");
 601                         return ERR_PTR(sysmon->shutdown_irq);
 602                 }
 603         } else {
 604                 ret = devm_request_threaded_irq(sysmon->dev,
 605                                                 sysmon->shutdown_irq,
 606                                                 NULL, sysmon_shutdown_interrupt,
 607                                                 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 608                                                 "q6v5 shutdown-ack", sysmon);
 609                 if (ret) {
 610                         dev_err(sysmon->dev,
 611                                 "failed to acquire shutdown-ack IRQ\n");
 612                         return ERR_PTR(ret);
 613                 }
 614         }
 615
 616         ret = qmi_handle_init(&sysmon->qmi, SSCTL_MAX_MSG_LEN, &ssctl_ops,
 617                               qmi_indication_handler);
 618         if (ret < 0) {
 619                 dev_err(sysmon->dev, "failed to initialize qmi handle\n");
 620                 kfree(sysmon);
 621                 return ERR_PTR(ret);
 622         }
 623
 624         qmi_add_lookup(&sysmon->qmi, 43, 0, 0);
 625
 626         sysmon->subdev.prepare = sysmon_prepare;
 627         sysmon->subdev.start = sysmon_start;
 628         sysmon->subdev.stop = sysmon_stop;
 629         sysmon->subdev.unprepare = sysmon_unprepare;
 630
 631         rproc_add_subdev(rproc, &sysmon->subdev);
 632
 633         sysmon->nb.notifier_call = sysmon_notify;
 634         blocking_notifier_chain_register(&sysmon_notifiers, &sysmon->nb);
 635
 636         mutex_lock(&sysmon_lock);
 637         list_add(&sysmon->node, &sysmon_list);
 638         mutex_unlock(&sysmon_lock);
 639
 640         return sysmon;
 641 }
 642 EXPORT_SYMBOL_GPL(qcom_add_sysmon_subdev);
 643
 644 /**
 645  * qcom_remove_sysmon_subdev() - release a qcom_sysmon
 646  * @sysmon:     sysmon context, as retrieved by qcom_add_sysmon_subdev()
 647  */
 648 void qcom_remove_sysmon_subdev(struct qcom_sysmon *sysmon)
 649 {
 650         if (!sysmon)
 651                 return;
 652
 653         mutex_lock(&sysmon_lock);
 654         list_del(&sysmon->node);
 655         mutex_unlock(&sysmon_lock);
 656
 657         blocking_notifier_chain_unregister(&sysmon_notifiers, &sysmon->nb);
 658
 659         rproc_remove_subdev(sysmon->rproc, &sysmon->subdev);
 660
 661         qmi_handle_release(&sysmon->qmi);
 662
 663         kfree(sysmon);
 664 }
 665 EXPORT_SYMBOL_GPL(qcom_remove_sysmon_subdev);
 666
 667 /**
 668  * sysmon_probe() - probe sys_mon channel
 669  * @rpdev:      rpmsg device handle
 670  *
 671  * Find the sysmon context associated with the ancestor remoteproc and assign
 672  * this rpmsg device with said sysmon context.
 673  *
 674  * Return: 0 on success, negative errno on failure.
 675  */
 676 static int sysmon_probe(struct rpmsg_device *rpdev)
 677 {
 678         struct qcom_sysmon *sysmon;
 679         struct rproc *rproc;
 680
 681         rproc = rproc_get_by_child(&rpdev->dev);
 682         if (!rproc) {
 683                 dev_err(&rpdev->dev, "sysmon device not child of rproc\n");
 684                 return -EINVAL;
 685         }
 686
 687         mutex_lock(&sysmon_lock);
 688         list_for_each_entry(sysmon, &sysmon_list, node) {
 689                 if (sysmon->rproc == rproc)
 690                         goto found;
 691         }
 692         mutex_unlock(&sysmon_lock);
 693
 694         dev_err(&rpdev->dev, "no sysmon associated with parent rproc\n");
 695
 696         return -EINVAL;
 697
 698 found:
 699         mutex_unlock(&sysmon_lock);
 700
 701         rpdev->ept->priv = sysmon;
 702         sysmon->ept = rpdev->ept;
 703
 704         return 0;
 705 }
 706
 707 /**
 708  * sysmon_remove() - sys_mon channel remove handler
 709  * @rpdev:      rpmsg device handle
 710  *
 711  * Disassociate the rpmsg device with the sysmon instance.
 712  */
 713 static void sysmon_remove(struct rpmsg_device *rpdev)
 714 {
 715         struct qcom_sysmon *sysmon = rpdev->ept->priv;
 716
 717         sysmon->ept = NULL;
 718 }
 719
 720 static const struct rpmsg_device_id sysmon_match[] = {
 721         { "sys_mon" },
 722         {}
 723 };
 724
 725 static struct rpmsg_driver sysmon_driver = {
 726         .probe = sysmon_probe,
 727         .remove = sysmon_remove,
 728         .callback = sysmon_callback,
 729         .id_table = sysmon_match,
 730         .drv = {
 731                 .name = "qcom_sysmon",
 732         },
 733 };
 734
 735 module_rpmsg_driver(sysmon_driver);
 736
 737 MODULE_DESCRIPTION("Qualcomm sysmon driver");
 738 MODULE_LICENSE("GPL v2");