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 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 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 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 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 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 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 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 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 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 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