• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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