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