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