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");
739