1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Qualcomm Protection Domain mapper
4 *
5 * Copyright (c) 2023 Linaro Ltd.
6 */
7
8 #include <linux/auxiliary_bus.h>
9 #include <linux/kernel.h>
10 #include <linux/module.h>
11 #include <linux/of.h>
12 #include <linux/refcount.h>
13 #include <linux/slab.h>
14 #include <linux/soc/qcom/qmi.h>
15
16 #include "pdr_internal.h"
17
18 #define SERVREG_QMI_VERSION 0x101
19 #define SERVREG_QMI_INSTANCE 0
20
21 #define TMS_SERVREG_SERVICE "tms/servreg"
22
23 struct qcom_pdm_domain_data {
24 const char *domain;
25 u32 instance_id;
26 /* NULL-terminated array */
27 const char * services[];
28 };
29
30 struct qcom_pdm_domain {
31 struct list_head list;
32 const char *name;
33 u32 instance_id;
34 };
35
36 struct qcom_pdm_service {
37 struct list_head list;
38 struct list_head domains;
39 const char *name;
40 };
41
42 struct qcom_pdm_data {
43 refcount_t refcnt;
44 struct qmi_handle handle;
45 struct list_head services;
46 };
47
48 static DEFINE_MUTEX(qcom_pdm_mutex); /* protects __qcom_pdm_data */
49 static struct qcom_pdm_data *__qcom_pdm_data;
50
qcom_pdm_find(struct qcom_pdm_data * data,const char * name)51 static struct qcom_pdm_service *qcom_pdm_find(struct qcom_pdm_data *data,
52 const char *name)
53 {
54 struct qcom_pdm_service *service;
55
56 list_for_each_entry(service, &data->services, list) {
57 if (!strcmp(service->name, name))
58 return service;
59 }
60
61 return NULL;
62 }
63
qcom_pdm_add_service_domain(struct qcom_pdm_data * data,const char * service_name,const char * domain_name,u32 instance_id)64 static int qcom_pdm_add_service_domain(struct qcom_pdm_data *data,
65 const char *service_name,
66 const char *domain_name,
67 u32 instance_id)
68 {
69 struct qcom_pdm_service *service;
70 struct qcom_pdm_domain *domain;
71
72 service = qcom_pdm_find(data, service_name);
73 if (service) {
74 list_for_each_entry(domain, &service->domains, list) {
75 if (!strcmp(domain->name, domain_name))
76 return -EBUSY;
77 }
78 } else {
79 service = kzalloc_obj(*service);
80 if (!service)
81 return -ENOMEM;
82
83 INIT_LIST_HEAD(&service->domains);
84 service->name = service_name;
85
86 list_add_tail(&service->list, &data->services);
87 }
88
89 domain = kzalloc_obj(*domain);
90 if (!domain) {
91 if (list_empty(&service->domains)) {
92 list_del(&service->list);
93 kfree(service);
94 }
95
96 return -ENOMEM;
97 }
98
99 domain->name = domain_name;
100 domain->instance_id = instance_id;
101 list_add_tail(&domain->list, &service->domains);
102
103 return 0;
104 }
105
qcom_pdm_add_domain(struct qcom_pdm_data * data,const struct qcom_pdm_domain_data * domain)106 static int qcom_pdm_add_domain(struct qcom_pdm_data *data,
107 const struct qcom_pdm_domain_data *domain)
108 {
109 int ret;
110 int i;
111
112 ret = qcom_pdm_add_service_domain(data,
113 TMS_SERVREG_SERVICE,
114 domain->domain,
115 domain->instance_id);
116 if (ret)
117 return ret;
118
119 for (i = 0; domain->services[i]; i++) {
120 ret = qcom_pdm_add_service_domain(data,
121 domain->services[i],
122 domain->domain,
123 domain->instance_id);
124 if (ret)
125 return ret;
126 }
127
128 return 0;
129
130 }
131
qcom_pdm_free_domains(struct qcom_pdm_data * data)132 static void qcom_pdm_free_domains(struct qcom_pdm_data *data)
133 {
134 struct qcom_pdm_service *service, *tservice;
135 struct qcom_pdm_domain *domain, *tdomain;
136
137 list_for_each_entry_safe(service, tservice, &data->services, list) {
138 list_for_each_entry_safe(domain, tdomain, &service->domains, list) {
139 list_del(&domain->list);
140 kfree(domain);
141 }
142
143 list_del(&service->list);
144 kfree(service);
145 }
146 }
147
qcom_pdm_get_domain_list(struct qmi_handle * qmi,struct sockaddr_qrtr * sq,struct qmi_txn * txn,const void * decoded)148 static void qcom_pdm_get_domain_list(struct qmi_handle *qmi,
149 struct sockaddr_qrtr *sq,
150 struct qmi_txn *txn,
151 const void *decoded)
152 {
153 struct qcom_pdm_data *data = container_of(qmi, struct qcom_pdm_data, handle);
154 const struct servreg_get_domain_list_req *req = decoded;
155 struct servreg_get_domain_list_resp *rsp;
156 struct qcom_pdm_service *service;
157 u32 offset;
158 int ret;
159
160 rsp = kzalloc_obj(*rsp);
161 if (!rsp)
162 return;
163
164 offset = req->domain_offset_valid ? req->domain_offset : 0;
165
166 rsp->resp.result = QMI_RESULT_SUCCESS_V01;
167 rsp->resp.error = QMI_ERR_NONE_V01;
168
169 rsp->db_rev_count_valid = true;
170 rsp->db_rev_count = 1;
171
172 rsp->total_domains_valid = true;
173 rsp->total_domains = 0;
174
175 mutex_lock(&qcom_pdm_mutex);
176
177 service = qcom_pdm_find(data, req->service_name);
178 if (service) {
179 struct qcom_pdm_domain *domain;
180
181 rsp->domain_list_valid = true;
182 rsp->domain_list_len = 0;
183
184 list_for_each_entry(domain, &service->domains, list) {
185 u32 i = rsp->total_domains++;
186
187 if (i >= offset && i < SERVREG_DOMAIN_LIST_LENGTH) {
188 u32 j = rsp->domain_list_len++;
189
190 strscpy(rsp->domain_list[j].name, domain->name,
191 sizeof(rsp->domain_list[i].name));
192 rsp->domain_list[j].instance = domain->instance_id;
193
194 pr_debug("PDM: found %s / %d\n", domain->name,
195 domain->instance_id);
196 }
197 }
198 }
199
200 pr_debug("PDM: service '%s' offset %d returning %d domains (of %d)\n", req->service_name,
201 req->domain_offset_valid ? req->domain_offset : -1, rsp->domain_list_len, rsp->total_domains);
202
203 ret = qmi_send_response(qmi, sq, txn, SERVREG_GET_DOMAIN_LIST_REQ,
204 SERVREG_GET_DOMAIN_LIST_RESP_MAX_LEN,
205 servreg_get_domain_list_resp_ei, rsp);
206 if (ret)
207 pr_err("Error sending servreg response: %d\n", ret);
208
209 mutex_unlock(&qcom_pdm_mutex);
210
211 kfree(rsp);
212 }
213
qcom_pdm_pfr(struct qmi_handle * qmi,struct sockaddr_qrtr * sq,struct qmi_txn * txn,const void * decoded)214 static void qcom_pdm_pfr(struct qmi_handle *qmi,
215 struct sockaddr_qrtr *sq,
216 struct qmi_txn *txn,
217 const void *decoded)
218 {
219 const struct servreg_loc_pfr_req *req = decoded;
220 struct servreg_loc_pfr_resp rsp = {};
221 int ret;
222
223 pr_warn_ratelimited("PDM: service '%s' crash: '%s'\n", req->service, req->reason);
224
225 rsp.rsp.result = QMI_RESULT_SUCCESS_V01;
226 rsp.rsp.error = QMI_ERR_NONE_V01;
227
228 ret = qmi_send_response(qmi, sq, txn, SERVREG_LOC_PFR_REQ,
229 SERVREG_LOC_PFR_RESP_MAX_LEN,
230 servreg_loc_pfr_resp_ei, &rsp);
231 if (ret)
232 pr_err("Error sending servreg response: %d\n", ret);
233 }
234
235 static const struct qmi_msg_handler qcom_pdm_msg_handlers[] = {
236 {
237 .type = QMI_REQUEST,
238 .msg_id = SERVREG_GET_DOMAIN_LIST_REQ,
239 .ei = servreg_get_domain_list_req_ei,
240 .decoded_size = sizeof(struct servreg_get_domain_list_req),
241 .fn = qcom_pdm_get_domain_list,
242 },
243 {
244 .type = QMI_REQUEST,
245 .msg_id = SERVREG_LOC_PFR_REQ,
246 .ei = servreg_loc_pfr_req_ei,
247 .decoded_size = sizeof(struct servreg_loc_pfr_req),
248 .fn = qcom_pdm_pfr,
249 },
250 { },
251 };
252
253 static const struct qcom_pdm_domain_data adsp_audio_pd = {
254 .domain = "msm/adsp/audio_pd",
255 .instance_id = 74,
256 .services = {
257 "avs/audio",
258 NULL,
259 },
260 };
261
262 static const struct qcom_pdm_domain_data adsp_charger_pd = {
263 .domain = "msm/adsp/charger_pd",
264 .instance_id = 74,
265 .services = { NULL },
266 };
267
268 static const struct qcom_pdm_domain_data adsp_ois_pd = {
269 .domain = "msm/adsp/ois_pd",
270 .instance_id = 74,
271 .services = { NULL, },
272 };
273
274 static const struct qcom_pdm_domain_data adsp_root_pd = {
275 .domain = "msm/adsp/root_pd",
276 .instance_id = 74,
277 .services = { NULL },
278 };
279
280 static const struct qcom_pdm_domain_data adsp_root_pd_pdr = {
281 .domain = "msm/adsp/root_pd",
282 .instance_id = 74,
283 .services = {
284 "tms/pdr_enabled",
285 NULL,
286 },
287 };
288
289 static const struct qcom_pdm_domain_data adsp_sensor_pd = {
290 .domain = "msm/adsp/sensor_pd",
291 .instance_id = 74,
292 .services = { NULL },
293 };
294
295 static const struct qcom_pdm_domain_data msm8996_adsp_audio_pd = {
296 .domain = "msm/adsp/audio_pd",
297 .instance_id = 4,
298 .services = { NULL },
299 };
300
301 static const struct qcom_pdm_domain_data msm8996_adsp_root_pd = {
302 .domain = "msm/adsp/root_pd",
303 .instance_id = 4,
304 .services = { NULL },
305 };
306
307 static const struct qcom_pdm_domain_data cdsp_root_pd = {
308 .domain = "msm/cdsp/root_pd",
309 .instance_id = 76,
310 .services = { NULL },
311 };
312
313 static const struct qcom_pdm_domain_data cdsp1_root_pd = {
314 .domain = "msm/cdsp1/root_pd",
315 .instance_id = 125,
316 .services = { NULL },
317 };
318
319 static const struct qcom_pdm_domain_data gpdsp_root_pd = {
320 .domain = "msm/gpdsp/root_pd",
321 .instance_id = 192,
322 .services = { NULL },
323 };
324
325 static const struct qcom_pdm_domain_data gpdsp1_root_pd = {
326 .domain = "msm/gpdsp1/root_pd",
327 .instance_id = 241,
328 .services = { NULL },
329 };
330
331 static const struct qcom_pdm_domain_data slpi_root_pd = {
332 .domain = "msm/slpi/root_pd",
333 .instance_id = 90,
334 .services = { NULL },
335 };
336
337 static const struct qcom_pdm_domain_data slpi_sensor_pd = {
338 .domain = "msm/slpi/sensor_pd",
339 .instance_id = 90,
340 .services = { NULL },
341 };
342
343 static const struct qcom_pdm_domain_data mpss_root_pd = {
344 .domain = "msm/modem/root_pd",
345 .instance_id = 180,
346 .services = {
347 NULL,
348 },
349 };
350
351 static const struct qcom_pdm_domain_data mpss_root_pd_gps = {
352 .domain = "msm/modem/root_pd",
353 .instance_id = 180,
354 .services = {
355 "gps/gps_service",
356 NULL,
357 },
358 };
359
360 static const struct qcom_pdm_domain_data mpss_root_pd_gps_pdr = {
361 .domain = "msm/modem/root_pd",
362 .instance_id = 180,
363 .services = {
364 "gps/gps_service",
365 "tms/pdr_enabled",
366 NULL,
367 },
368 };
369
370 static const struct qcom_pdm_domain_data msm8996_mpss_root_pd = {
371 .domain = "msm/modem/root_pd",
372 .instance_id = 100,
373 .services = { NULL },
374 };
375
376 static const struct qcom_pdm_domain_data mpss_wlan_pd = {
377 .domain = "msm/modem/wlan_pd",
378 .instance_id = 180,
379 .services = {
380 "kernel/elf_loader",
381 "wlan/fw",
382 NULL,
383 },
384 };
385
386 static const struct qcom_pdm_domain_data *glymur_domains[] = {
387 &adsp_audio_pd,
388 &adsp_root_pd,
389 &adsp_sensor_pd,
390 &cdsp_root_pd,
391 NULL,
392 };
393
394 static const struct qcom_pdm_domain_data *kaanapali_domains[] = {
395 &adsp_audio_pd,
396 &adsp_ois_pd,
397 &adsp_root_pd,
398 &adsp_sensor_pd,
399 &cdsp_root_pd,
400 &mpss_root_pd_gps,
401 NULL,
402 };
403
404 static const struct qcom_pdm_domain_data *msm8996_domains[] = {
405 &msm8996_adsp_audio_pd,
406 &msm8996_adsp_root_pd,
407 &msm8996_mpss_root_pd,
408 NULL,
409 };
410
411 static const struct qcom_pdm_domain_data *msm8998_domains[] = {
412 &mpss_root_pd,
413 &mpss_wlan_pd,
414 NULL,
415 };
416
417 static const struct qcom_pdm_domain_data *qcm2290_domains[] = {
418 &adsp_audio_pd,
419 &adsp_root_pd,
420 &adsp_sensor_pd,
421 &mpss_root_pd_gps,
422 &mpss_wlan_pd,
423 NULL,
424 };
425
426 static const struct qcom_pdm_domain_data *qcs404_domains[] = {
427 &adsp_audio_pd,
428 &adsp_root_pd,
429 &adsp_sensor_pd,
430 &cdsp_root_pd,
431 &mpss_root_pd,
432 &mpss_wlan_pd,
433 NULL,
434 };
435
436 static const struct qcom_pdm_domain_data *qcs615_domains[] = {
437 &adsp_audio_pd,
438 &adsp_root_pd,
439 &adsp_sensor_pd,
440 &cdsp_root_pd,
441 &mpss_root_pd,
442 &mpss_wlan_pd,
443 NULL,
444 };
445
446 static const struct qcom_pdm_domain_data *qcs8300_domains[] = {
447 &adsp_audio_pd,
448 &adsp_root_pd,
449 &cdsp_root_pd,
450 &gpdsp_root_pd,
451 NULL,
452 };
453
454 static const struct qcom_pdm_domain_data *sa8775p_domains[] = {
455 &adsp_audio_pd,
456 &adsp_root_pd,
457 &cdsp_root_pd,
458 &cdsp1_root_pd,
459 &gpdsp_root_pd,
460 &gpdsp1_root_pd,
461 NULL,
462 };
463
464 static const struct qcom_pdm_domain_data *sc7180_domains[] = {
465 &adsp_audio_pd,
466 &adsp_root_pd_pdr,
467 &adsp_sensor_pd,
468 &mpss_root_pd_gps_pdr,
469 &mpss_wlan_pd,
470 NULL,
471 };
472
473 static const struct qcom_pdm_domain_data *sc7280_domains[] = {
474 &adsp_audio_pd,
475 &adsp_root_pd_pdr,
476 &adsp_charger_pd,
477 &adsp_sensor_pd,
478 &cdsp_root_pd,
479 &mpss_root_pd_gps_pdr,
480 NULL,
481 };
482
483 static const struct qcom_pdm_domain_data *sc8180x_domains[] = {
484 &adsp_audio_pd,
485 &adsp_root_pd,
486 &adsp_charger_pd,
487 &cdsp_root_pd,
488 &mpss_root_pd_gps,
489 &mpss_wlan_pd,
490 NULL,
491 };
492
493 static const struct qcom_pdm_domain_data *sc8280xp_domains[] = {
494 &adsp_audio_pd,
495 &adsp_root_pd_pdr,
496 &adsp_charger_pd,
497 &cdsp_root_pd,
498 NULL,
499 };
500
501 /* Unlike SDM660, SDM630/636 lack CDSP */
502 static const struct qcom_pdm_domain_data *sdm630_domains[] = {
503 &adsp_audio_pd,
504 &adsp_root_pd,
505 &adsp_sensor_pd,
506 &mpss_root_pd,
507 &mpss_wlan_pd,
508 NULL,
509 };
510
511 static const struct qcom_pdm_domain_data *sdm660_domains[] = {
512 &adsp_audio_pd,
513 &adsp_root_pd,
514 &adsp_sensor_pd,
515 &cdsp_root_pd,
516 &mpss_root_pd,
517 &mpss_wlan_pd,
518 NULL,
519 };
520
521 static const struct qcom_pdm_domain_data *sdm670_domains[] = {
522 &adsp_audio_pd,
523 &adsp_root_pd,
524 &cdsp_root_pd,
525 &mpss_root_pd,
526 &mpss_wlan_pd,
527 NULL,
528 };
529
530 static const struct qcom_pdm_domain_data *sdm845_domains[] = {
531 &adsp_audio_pd,
532 &adsp_root_pd,
533 &cdsp_root_pd,
534 &mpss_root_pd,
535 &mpss_wlan_pd,
536 &slpi_root_pd,
537 &slpi_sensor_pd,
538 NULL,
539 };
540
541 static const struct qcom_pdm_domain_data *sm6115_domains[] = {
542 &adsp_audio_pd,
543 &adsp_root_pd,
544 &adsp_sensor_pd,
545 &cdsp_root_pd,
546 &mpss_root_pd_gps,
547 &mpss_wlan_pd,
548 NULL,
549 };
550
551 static const struct qcom_pdm_domain_data *sm6350_domains[] = {
552 &adsp_audio_pd,
553 &adsp_root_pd,
554 &adsp_sensor_pd,
555 &cdsp_root_pd,
556 &mpss_wlan_pd,
557 NULL,
558 };
559
560 static const struct qcom_pdm_domain_data *sm7150_domains[] = {
561 &adsp_audio_pd,
562 &adsp_root_pd,
563 &adsp_sensor_pd,
564 &cdsp_root_pd,
565 &mpss_root_pd_gps,
566 &mpss_wlan_pd,
567 NULL,
568 };
569
570 static const struct qcom_pdm_domain_data *sm8150_domains[] = {
571 &adsp_audio_pd,
572 &adsp_root_pd,
573 &cdsp_root_pd,
574 &mpss_root_pd_gps,
575 &mpss_wlan_pd,
576 NULL,
577 };
578
579 static const struct qcom_pdm_domain_data *sm8250_domains[] = {
580 &adsp_audio_pd,
581 &adsp_root_pd,
582 &cdsp_root_pd,
583 &slpi_root_pd,
584 &slpi_sensor_pd,
585 NULL,
586 };
587
588 static const struct qcom_pdm_domain_data *sm8350_domains[] = {
589 &adsp_audio_pd,
590 &adsp_root_pd_pdr,
591 &adsp_charger_pd,
592 &cdsp_root_pd,
593 &mpss_root_pd_gps,
594 &slpi_root_pd,
595 &slpi_sensor_pd,
596 NULL,
597 };
598
599 static const struct qcom_pdm_domain_data *sm8550_domains[] = {
600 &adsp_audio_pd,
601 &adsp_root_pd,
602 &adsp_charger_pd,
603 &adsp_sensor_pd,
604 &cdsp_root_pd,
605 &mpss_root_pd_gps,
606 NULL,
607 };
608
609 static const struct qcom_pdm_domain_data *x1e80100_domains[] = {
610 &adsp_audio_pd,
611 &adsp_root_pd,
612 &adsp_charger_pd,
613 &adsp_sensor_pd,
614 &cdsp_root_pd,
615 NULL,
616 };
617
618 static const struct of_device_id qcom_pdm_domains[] __maybe_unused = {
619 { .compatible = "qcom,apq8016", .data = NULL, },
620 { .compatible = "qcom,apq8064", .data = NULL, },
621 { .compatible = "qcom,apq8074", .data = NULL, },
622 { .compatible = "qcom,apq8084", .data = NULL, },
623 { .compatible = "qcom,eliza", .data = sm8550_domains, },
624 { .compatible = "qcom,apq8096", .data = msm8996_domains, },
625 { .compatible = "qcom,glymur", .data = glymur_domains, },
626 { .compatible = "qcom,hawi", .data = kaanapali_domains, },
627 { .compatible = "qcom,kaanapali", .data = kaanapali_domains, },
628 { .compatible = "qcom,mahua", .data = glymur_domains, },
629 { .compatible = "qcom,milos", .data = sm8550_domains, },
630 { .compatible = "qcom,msm8226", .data = NULL, },
631 { .compatible = "qcom,msm8909", .data = NULL, },
632 { .compatible = "qcom,msm8916", .data = NULL, },
633 { .compatible = "qcom,msm8939", .data = NULL, },
634 { .compatible = "qcom,msm8974", .data = NULL, },
635 { .compatible = "qcom,msm8996", .data = msm8996_domains, },
636 { .compatible = "qcom,msm8998", .data = msm8998_domains, },
637 { .compatible = "qcom,qcm2290", .data = qcm2290_domains, },
638 { .compatible = "qcom,qcm6490", .data = sc7280_domains, },
639 { .compatible = "qcom,qcs404", .data = qcs404_domains, },
640 { .compatible = "qcom,qcs615", .data = qcs615_domains, },
641 { .compatible = "qcom,qcs8300", .data = qcs8300_domains, },
642 { .compatible = "qcom,sa8775p", .data = sa8775p_domains, },
643 { .compatible = "qcom,sc7180", .data = sc7180_domains, },
644 { .compatible = "qcom,sc7280", .data = sc7280_domains, },
645 { .compatible = "qcom,sc8180x", .data = sc8180x_domains, },
646 { .compatible = "qcom,sc8280xp", .data = sc8280xp_domains, },
647 { .compatible = "qcom,sdm630", .data = sdm630_domains, },
648 { .compatible = "qcom,sdm636", .data = sdm630_domains, },
649 { .compatible = "qcom,sda660", .data = sdm660_domains, },
650 { .compatible = "qcom,sdm660", .data = sdm660_domains, },
651 { .compatible = "qcom,sdm670", .data = sdm670_domains, },
652 { .compatible = "qcom,sdm845", .data = sdm845_domains, },
653 { .compatible = "qcom,sm4250", .data = sm6115_domains, },
654 { .compatible = "qcom,sm6115", .data = sm6115_domains, },
655 { .compatible = "qcom,sm6350", .data = sm6350_domains, },
656 { .compatible = "qcom,sm7150", .data = sm7150_domains, },
657 { .compatible = "qcom,sm7225", .data = sm6350_domains, },
658 { .compatible = "qcom,sm7325", .data = sc7280_domains, },
659 { .compatible = "qcom,sm8150", .data = sm8150_domains, },
660 { .compatible = "qcom,sm8250", .data = sm8250_domains, },
661 { .compatible = "qcom,sm8350", .data = sm8350_domains, },
662 { .compatible = "qcom,sm8450", .data = sm8350_domains, },
663 { .compatible = "qcom,sm8550", .data = sm8550_domains, },
664 { .compatible = "qcom,sm8650", .data = sm8550_domains, },
665 { .compatible = "qcom,sm8750", .data = sm8550_domains, },
666 { .compatible = "qcom,x1e80100", .data = x1e80100_domains, },
667 { .compatible = "qcom,x1p42100", .data = x1e80100_domains, },
668 {},
669 };
670
qcom_pdm_stop(struct qcom_pdm_data * data)671 static void qcom_pdm_stop(struct qcom_pdm_data *data)
672 {
673 qcom_pdm_free_domains(data);
674
675 /* The server is removed automatically */
676 qmi_handle_release(&data->handle);
677
678 kfree(data);
679 }
680
qcom_pdm_start(void)681 static struct qcom_pdm_data *qcom_pdm_start(void)
682 {
683 const struct qcom_pdm_domain_data * const *domains;
684 const struct of_device_id *match;
685 struct qcom_pdm_data *data;
686 int ret, i;
687
688 match = of_machine_get_match(qcom_pdm_domains);
689 if (!match) {
690 pr_notice("PDM: no support for the platform, userspace daemon might be required.\n");
691 return ERR_PTR(-ENODEV);
692 }
693
694 domains = match->data;
695 if (!domains) {
696 pr_debug("PDM: no domains\n");
697 return ERR_PTR(-ENODEV);
698 }
699
700 data = kzalloc_obj(*data);
701 if (!data)
702 return ERR_PTR(-ENOMEM);
703
704 INIT_LIST_HEAD(&data->services);
705
706 ret = qmi_handle_init(&data->handle, SERVREG_GET_DOMAIN_LIST_REQ_MAX_LEN,
707 NULL, qcom_pdm_msg_handlers);
708 if (ret) {
709 kfree(data);
710 return ERR_PTR(ret);
711 }
712
713 refcount_set(&data->refcnt, 1);
714
715 for (i = 0; domains[i]; i++) {
716 ret = qcom_pdm_add_domain(data, domains[i]);
717 if (ret)
718 goto err_stop;
719 }
720
721 ret = qmi_add_server(&data->handle, QMI_SERVICE_ID_SERVREG_LOC,
722 SERVREG_QMI_VERSION, SERVREG_QMI_INSTANCE);
723 if (ret) {
724 pr_err("PDM: error adding server %d\n", ret);
725 goto err_stop;
726 }
727
728 return data;
729
730 err_stop:
731 qcom_pdm_stop(data);
732
733 return ERR_PTR(ret);
734 }
735
qcom_pdm_probe(struct auxiliary_device * auxdev,const struct auxiliary_device_id * id)736 static int qcom_pdm_probe(struct auxiliary_device *auxdev,
737 const struct auxiliary_device_id *id)
738
739 {
740 struct qcom_pdm_data *data;
741 int ret = 0;
742
743 mutex_lock(&qcom_pdm_mutex);
744
745 if (!__qcom_pdm_data) {
746 data = qcom_pdm_start();
747
748 if (IS_ERR(data))
749 ret = PTR_ERR(data);
750 else
751 __qcom_pdm_data = data;
752 } else {
753 refcount_inc(&__qcom_pdm_data->refcnt);
754 }
755
756 auxiliary_set_drvdata(auxdev, __qcom_pdm_data);
757
758 mutex_unlock(&qcom_pdm_mutex);
759
760 return ret;
761 }
762
qcom_pdm_remove(struct auxiliary_device * auxdev)763 static void qcom_pdm_remove(struct auxiliary_device *auxdev)
764 {
765 struct qcom_pdm_data *data;
766
767 data = auxiliary_get_drvdata(auxdev);
768 if (!data)
769 return;
770
771 if (refcount_dec_and_mutex_lock(&data->refcnt, &qcom_pdm_mutex)) {
772 __qcom_pdm_data = NULL;
773 qcom_pdm_stop(data);
774 mutex_unlock(&qcom_pdm_mutex);
775 }
776 }
777
778 static const struct auxiliary_device_id qcom_pdm_table[] = {
779 { .name = "qcom_common.pd-mapper" },
780 {},
781 };
782 MODULE_DEVICE_TABLE(auxiliary, qcom_pdm_table);
783
784 static struct auxiliary_driver qcom_pdm_drv = {
785 .name = "qcom-pdm-mapper",
786 .id_table = qcom_pdm_table,
787 .probe = qcom_pdm_probe,
788 .remove = qcom_pdm_remove,
789 };
790 module_auxiliary_driver(qcom_pdm_drv);
791
792 MODULE_DESCRIPTION("Qualcomm Protection Domain Mapper");
793 MODULE_LICENSE("GPL");
794