748 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (C) 2020 The Linux Foundation. All rights reserved.4 */5 6#include <linux/cleanup.h>7#include <linux/kernel.h>8#include <linux/module.h>9#include <linux/slab.h>10#include <linux/string.h>11#include <linux/workqueue.h>12 13#include "pdr_internal.h"14 15struct pdr_service {16 char service_name[SERVREG_NAME_LENGTH + 1];17 char service_path[SERVREG_NAME_LENGTH + 1];18 19 struct sockaddr_qrtr addr;20 21 unsigned int instance;22 unsigned int service;23 u8 service_data_valid;24 u32 service_data;25 int state;26 27 bool need_notifier_register;28 bool need_notifier_remove;29 bool need_locator_lookup;30 bool service_connected;31 32 struct list_head node;33};34 35struct pdr_handle {36 struct qmi_handle locator_hdl;37 struct qmi_handle notifier_hdl;38 39 struct sockaddr_qrtr locator_addr;40 41 struct list_head lookups;42 struct list_head indack_list;43 44 /* control access to pdr lookup/indack lists */45 struct mutex list_lock;46 47 /* serialize pd status invocation */48 struct mutex status_lock;49 50 /* control access to the locator state */51 struct mutex lock;52 53 bool locator_init_complete;54 55 struct work_struct locator_work;56 struct work_struct notifier_work;57 struct work_struct indack_work;58 59 struct workqueue_struct *notifier_wq;60 struct workqueue_struct *indack_wq;61 62 void (*status)(int state, char *service_path, void *priv);63 void *priv;64};65 66struct pdr_list_node {67 enum servreg_service_state curr_state;68 u16 transaction_id;69 struct pdr_service *pds;70 struct list_head node;71};72 73static int pdr_locator_new_server(struct qmi_handle *qmi,74 struct qmi_service *svc)75{76 struct pdr_handle *pdr = container_of(qmi, struct pdr_handle,77 locator_hdl);78 struct pdr_service *pds;79 80 mutex_lock(&pdr->lock);81 /* Create a local client port for QMI communication */82 pdr->locator_addr.sq_family = AF_QIPCRTR;83 pdr->locator_addr.sq_node = svc->node;84 pdr->locator_addr.sq_port = svc->port;85 86 pdr->locator_init_complete = true;87 mutex_unlock(&pdr->lock);88 89 /* Service pending lookup requests */90 mutex_lock(&pdr->list_lock);91 list_for_each_entry(pds, &pdr->lookups, node) {92 if (pds->need_locator_lookup)93 schedule_work(&pdr->locator_work);94 }95 mutex_unlock(&pdr->list_lock);96 97 return 0;98}99 100static void pdr_locator_del_server(struct qmi_handle *qmi,101 struct qmi_service *svc)102{103 struct pdr_handle *pdr = container_of(qmi, struct pdr_handle,104 locator_hdl);105 106 mutex_lock(&pdr->lock);107 pdr->locator_init_complete = false;108 109 pdr->locator_addr.sq_node = 0;110 pdr->locator_addr.sq_port = 0;111 mutex_unlock(&pdr->lock);112}113 114static const struct qmi_ops pdr_locator_ops = {115 .new_server = pdr_locator_new_server,116 .del_server = pdr_locator_del_server,117};118 119static int pdr_register_listener(struct pdr_handle *pdr,120 struct pdr_service *pds,121 bool enable)122{123 struct servreg_register_listener_resp resp;124 struct servreg_register_listener_req req;125 struct qmi_txn txn;126 int ret;127 128 ret = qmi_txn_init(&pdr->notifier_hdl, &txn,129 servreg_register_listener_resp_ei,130 &resp);131 if (ret < 0)132 return ret;133 134 req.enable = enable;135 strscpy(req.service_path, pds->service_path, sizeof(req.service_path));136 137 ret = qmi_send_request(&pdr->notifier_hdl, &pds->addr,138 &txn, SERVREG_REGISTER_LISTENER_REQ,139 SERVREG_REGISTER_LISTENER_REQ_LEN,140 servreg_register_listener_req_ei,141 &req);142 if (ret < 0) {143 qmi_txn_cancel(&txn);144 return ret;145 }146 147 ret = qmi_txn_wait(&txn, 5 * HZ);148 if (ret < 0) {149 pr_err("PDR: %s register listener txn wait failed: %d\n",150 pds->service_path, ret);151 return ret;152 }153 154 if (resp.resp.result != QMI_RESULT_SUCCESS_V01) {155 pr_err("PDR: %s register listener failed: 0x%x\n",156 pds->service_path, resp.resp.error);157 return -EREMOTEIO;158 }159 160 pds->state = resp.curr_state;161 162 return 0;163}164 165static void pdr_notifier_work(struct work_struct *work)166{167 struct pdr_handle *pdr = container_of(work, struct pdr_handle,168 notifier_work);169 struct pdr_service *pds;170 int ret;171 172 mutex_lock(&pdr->list_lock);173 list_for_each_entry(pds, &pdr->lookups, node) {174 if (pds->service_connected) {175 if (!pds->need_notifier_register)176 continue;177 178 pds->need_notifier_register = false;179 ret = pdr_register_listener(pdr, pds, true);180 if (ret < 0)181 pds->state = SERVREG_SERVICE_STATE_DOWN;182 } else {183 if (!pds->need_notifier_remove)184 continue;185 186 pds->need_notifier_remove = false;187 pds->state = SERVREG_SERVICE_STATE_DOWN;188 }189 190 mutex_lock(&pdr->status_lock);191 pdr->status(pds->state, pds->service_path, pdr->priv);192 mutex_unlock(&pdr->status_lock);193 }194 mutex_unlock(&pdr->list_lock);195}196 197static int pdr_notifier_new_server(struct qmi_handle *qmi,198 struct qmi_service *svc)199{200 struct pdr_handle *pdr = container_of(qmi, struct pdr_handle,201 notifier_hdl);202 struct pdr_service *pds;203 204 mutex_lock(&pdr->list_lock);205 list_for_each_entry(pds, &pdr->lookups, node) {206 if (pds->service == svc->service &&207 pds->instance == svc->instance) {208 pds->service_connected = true;209 pds->need_notifier_register = true;210 pds->addr.sq_family = AF_QIPCRTR;211 pds->addr.sq_node = svc->node;212 pds->addr.sq_port = svc->port;213 queue_work(pdr->notifier_wq, &pdr->notifier_work);214 }215 }216 mutex_unlock(&pdr->list_lock);217 218 return 0;219}220 221static void pdr_notifier_del_server(struct qmi_handle *qmi,222 struct qmi_service *svc)223{224 struct pdr_handle *pdr = container_of(qmi, struct pdr_handle,225 notifier_hdl);226 struct pdr_service *pds;227 228 mutex_lock(&pdr->list_lock);229 list_for_each_entry(pds, &pdr->lookups, node) {230 if (pds->service == svc->service &&231 pds->instance == svc->instance) {232 pds->service_connected = false;233 pds->need_notifier_remove = true;234 pds->addr.sq_node = 0;235 pds->addr.sq_port = 0;236 queue_work(pdr->notifier_wq, &pdr->notifier_work);237 }238 }239 mutex_unlock(&pdr->list_lock);240}241 242static const struct qmi_ops pdr_notifier_ops = {243 .new_server = pdr_notifier_new_server,244 .del_server = pdr_notifier_del_server,245};246 247static int pdr_send_indack_msg(struct pdr_handle *pdr, struct pdr_service *pds,248 u16 tid)249{250 struct servreg_set_ack_resp resp;251 struct servreg_set_ack_req req;252 struct qmi_txn txn;253 int ret;254 255 ret = qmi_txn_init(&pdr->notifier_hdl, &txn, servreg_set_ack_resp_ei,256 &resp);257 if (ret < 0)258 return ret;259 260 req.transaction_id = tid;261 strscpy(req.service_path, pds->service_path, sizeof(req.service_path));262 263 ret = qmi_send_request(&pdr->notifier_hdl, &pds->addr,264 &txn, SERVREG_SET_ACK_REQ,265 SERVREG_SET_ACK_REQ_LEN,266 servreg_set_ack_req_ei,267 &req);268 269 /* Skip waiting for response */270 qmi_txn_cancel(&txn);271 return ret;272}273 274static void pdr_indack_work(struct work_struct *work)275{276 struct pdr_handle *pdr = container_of(work, struct pdr_handle,277 indack_work);278 struct pdr_list_node *ind, *tmp;279 struct pdr_service *pds;280 281 list_for_each_entry_safe(ind, tmp, &pdr->indack_list, node) {282 pds = ind->pds;283 284 mutex_lock(&pdr->status_lock);285 pds->state = ind->curr_state;286 pdr->status(pds->state, pds->service_path, pdr->priv);287 mutex_unlock(&pdr->status_lock);288 289 /* Ack the indication after clients release the PD resources */290 pdr_send_indack_msg(pdr, pds, ind->transaction_id);291 292 mutex_lock(&pdr->list_lock);293 list_del(&ind->node);294 mutex_unlock(&pdr->list_lock);295 296 kfree(ind);297 }298}299 300static void pdr_indication_cb(struct qmi_handle *qmi,301 struct sockaddr_qrtr *sq,302 struct qmi_txn *txn, const void *data)303{304 struct pdr_handle *pdr = container_of(qmi, struct pdr_handle,305 notifier_hdl);306 const struct servreg_state_updated_ind *ind_msg = data;307 struct pdr_list_node *ind;308 struct pdr_service *pds = NULL, *iter;309 310 if (!ind_msg || !ind_msg->service_path[0] ||311 strlen(ind_msg->service_path) > SERVREG_NAME_LENGTH)312 return;313 314 mutex_lock(&pdr->list_lock);315 list_for_each_entry(iter, &pdr->lookups, node) {316 if (strcmp(iter->service_path, ind_msg->service_path))317 continue;318 319 pds = iter;320 break;321 }322 mutex_unlock(&pdr->list_lock);323 324 if (!pds)325 return;326 327 pr_info("PDR: Indication received from %s, state: 0x%x, trans-id: %d\n",328 ind_msg->service_path, ind_msg->curr_state,329 ind_msg->transaction_id);330 331 ind = kzalloc(sizeof(*ind), GFP_KERNEL);332 if (!ind)333 return;334 335 ind->transaction_id = ind_msg->transaction_id;336 ind->curr_state = ind_msg->curr_state;337 ind->pds = pds;338 339 mutex_lock(&pdr->list_lock);340 list_add_tail(&ind->node, &pdr->indack_list);341 mutex_unlock(&pdr->list_lock);342 343 queue_work(pdr->indack_wq, &pdr->indack_work);344}345 346static const struct qmi_msg_handler qmi_indication_handler[] = {347 {348 .type = QMI_INDICATION,349 .msg_id = SERVREG_STATE_UPDATED_IND_ID,350 .ei = servreg_state_updated_ind_ei,351 .decoded_size = sizeof(struct servreg_state_updated_ind),352 .fn = pdr_indication_cb,353 },354 {}355};356 357static int pdr_get_domain_list(struct servreg_get_domain_list_req *req,358 struct servreg_get_domain_list_resp *resp,359 struct pdr_handle *pdr)360{361 struct qmi_txn txn;362 int ret;363 364 ret = qmi_txn_init(&pdr->locator_hdl, &txn,365 servreg_get_domain_list_resp_ei, resp);366 if (ret < 0)367 return ret;368 369 mutex_lock(&pdr->lock);370 ret = qmi_send_request(&pdr->locator_hdl,371 &pdr->locator_addr,372 &txn, SERVREG_GET_DOMAIN_LIST_REQ,373 SERVREG_GET_DOMAIN_LIST_REQ_MAX_LEN,374 servreg_get_domain_list_req_ei,375 req);376 mutex_unlock(&pdr->lock);377 if (ret < 0) {378 qmi_txn_cancel(&txn);379 return ret;380 }381 382 ret = qmi_txn_wait(&txn, 5 * HZ);383 if (ret < 0) {384 pr_err("PDR: %s get domain list txn wait failed: %d\n",385 req->service_name, ret);386 return ret;387 }388 389 if (resp->resp.result != QMI_RESULT_SUCCESS_V01) {390 pr_err("PDR: %s get domain list failed: 0x%x\n",391 req->service_name, resp->resp.error);392 return -EREMOTEIO;393 }394 395 return 0;396}397 398static int pdr_locate_service(struct pdr_handle *pdr, struct pdr_service *pds)399{400 struct servreg_get_domain_list_req req;401 struct servreg_location_entry *entry;402 int domains_read = 0;403 int ret, i;404 405 struct servreg_get_domain_list_resp *resp __free(kfree) = kzalloc(sizeof(*resp),406 GFP_KERNEL);407 if (!resp)408 return -ENOMEM;409 410 /* Prepare req message */411 strscpy(req.service_name, pds->service_name, sizeof(req.service_name));412 req.domain_offset_valid = true;413 req.domain_offset = 0;414 415 do {416 req.domain_offset = domains_read;417 ret = pdr_get_domain_list(&req, resp, pdr);418 if (ret < 0)419 return ret;420 421 for (i = 0; i < resp->domain_list_len; i++) {422 entry = &resp->domain_list[i];423 424 if (strnlen(entry->name, sizeof(entry->name)) == sizeof(entry->name))425 continue;426 427 if (!strcmp(entry->name, pds->service_path)) {428 pds->service_data_valid = entry->service_data_valid;429 pds->service_data = entry->service_data;430 pds->instance = entry->instance;431 return 0;432 }433 }434 435 /* Update ret to indicate that the service is not yet found */436 ret = -ENXIO;437 438 /* Always read total_domains from the response msg */439 if (resp->domain_list_len > resp->total_domains)440 resp->domain_list_len = resp->total_domains;441 442 domains_read += resp->domain_list_len;443 } while (domains_read < resp->total_domains);444 445 return ret;446}447 448static void pdr_notify_lookup_failure(struct pdr_handle *pdr,449 struct pdr_service *pds,450 int err)451{452 pr_err("PDR: service lookup for %s failed: %d\n",453 pds->service_name, err);454 455 if (err == -ENXIO)456 return;457 458 list_del(&pds->node);459 pds->state = SERVREG_LOCATOR_ERR;460 mutex_lock(&pdr->status_lock);461 pdr->status(pds->state, pds->service_path, pdr->priv);462 mutex_unlock(&pdr->status_lock);463 kfree(pds);464}465 466static void pdr_locator_work(struct work_struct *work)467{468 struct pdr_handle *pdr = container_of(work, struct pdr_handle,469 locator_work);470 struct pdr_service *pds, *tmp;471 int ret = 0;472 473 /* Bail out early if the SERVREG LOCATOR QMI service is not up */474 mutex_lock(&pdr->lock);475 if (!pdr->locator_init_complete) {476 mutex_unlock(&pdr->lock);477 pr_debug("PDR: SERVICE LOCATOR service not available\n");478 return;479 }480 mutex_unlock(&pdr->lock);481 482 mutex_lock(&pdr->list_lock);483 list_for_each_entry_safe(pds, tmp, &pdr->lookups, node) {484 if (!pds->need_locator_lookup)485 continue;486 487 ret = pdr_locate_service(pdr, pds);488 if (ret < 0) {489 pdr_notify_lookup_failure(pdr, pds, ret);490 continue;491 }492 493 ret = qmi_add_lookup(&pdr->notifier_hdl, pds->service, 1,494 pds->instance);495 if (ret < 0) {496 pdr_notify_lookup_failure(pdr, pds, ret);497 continue;498 }499 500 pds->need_locator_lookup = false;501 }502 mutex_unlock(&pdr->list_lock);503}504 505/**506 * pdr_add_lookup() - register a tracking request for a PD507 * @pdr: PDR client handle508 * @service_name: service name of the tracking request509 * @service_path: service path of the tracking request510 *511 * Registering a pdr lookup allows for tracking the life cycle of the PD.512 *513 * Return: pdr_service object on success, ERR_PTR on failure. -EALREADY is514 * returned if a lookup is already in progress for the given service path.515 */516struct pdr_service *pdr_add_lookup(struct pdr_handle *pdr,517 const char *service_name,518 const char *service_path)519{520 struct pdr_service *tmp;521 522 if (IS_ERR_OR_NULL(pdr))523 return ERR_PTR(-EINVAL);524 525 if (!service_name || strlen(service_name) > SERVREG_NAME_LENGTH ||526 !service_path || strlen(service_path) > SERVREG_NAME_LENGTH)527 return ERR_PTR(-EINVAL);528 529 struct pdr_service *pds __free(kfree) = kzalloc(sizeof(*pds), GFP_KERNEL);530 if (!pds)531 return ERR_PTR(-ENOMEM);532 533 pds->service = SERVREG_NOTIFIER_SERVICE;534 strscpy(pds->service_name, service_name, sizeof(pds->service_name));535 strscpy(pds->service_path, service_path, sizeof(pds->service_path));536 pds->need_locator_lookup = true;537 538 mutex_lock(&pdr->list_lock);539 list_for_each_entry(tmp, &pdr->lookups, node) {540 if (strcmp(tmp->service_path, service_path))541 continue;542 543 mutex_unlock(&pdr->list_lock);544 return ERR_PTR(-EALREADY);545 }546 547 list_add(&pds->node, &pdr->lookups);548 mutex_unlock(&pdr->list_lock);549 550 schedule_work(&pdr->locator_work);551 552 return_ptr(pds);553}554EXPORT_SYMBOL_GPL(pdr_add_lookup);555 556/**557 * pdr_restart_pd() - restart PD558 * @pdr: PDR client handle559 * @pds: PD service handle560 *561 * Restarts the PD tracked by the PDR client handle for a given service path.562 *563 * Return: 0 on success, negative errno on failure.564 */565int pdr_restart_pd(struct pdr_handle *pdr, struct pdr_service *pds)566{567 struct servreg_restart_pd_resp resp;568 struct servreg_restart_pd_req req = { 0 };569 struct sockaddr_qrtr addr;570 struct pdr_service *tmp;571 struct qmi_txn txn;572 int ret;573 574 if (IS_ERR_OR_NULL(pdr) || IS_ERR_OR_NULL(pds))575 return -EINVAL;576 577 mutex_lock(&pdr->list_lock);578 list_for_each_entry(tmp, &pdr->lookups, node) {579 if (tmp != pds)580 continue;581 582 if (!pds->service_connected)583 break;584 585 /* Prepare req message */586 strscpy(req.service_path, pds->service_path, sizeof(req.service_path));587 addr = pds->addr;588 break;589 }590 mutex_unlock(&pdr->list_lock);591 592 if (!req.service_path[0])593 return -EINVAL;594 595 ret = qmi_txn_init(&pdr->notifier_hdl, &txn,596 servreg_restart_pd_resp_ei,597 &resp);598 if (ret < 0)599 return ret;600 601 ret = qmi_send_request(&pdr->notifier_hdl, &addr,602 &txn, SERVREG_RESTART_PD_REQ,603 SERVREG_RESTART_PD_REQ_MAX_LEN,604 servreg_restart_pd_req_ei, &req);605 if (ret < 0) {606 qmi_txn_cancel(&txn);607 return ret;608 }609 610 ret = qmi_txn_wait(&txn, 5 * HZ);611 if (ret < 0) {612 pr_err("PDR: %s PD restart txn wait failed: %d\n",613 req.service_path, ret);614 return ret;615 }616 617 /* Check response if PDR is disabled */618 if (resp.resp.result == QMI_RESULT_FAILURE_V01 &&619 resp.resp.error == QMI_ERR_DISABLED_V01) {620 pr_err("PDR: %s PD restart is disabled: 0x%x\n",621 req.service_path, resp.resp.error);622 return -EOPNOTSUPP;623 }624 625 /* Check the response for other error case*/626 if (resp.resp.result != QMI_RESULT_SUCCESS_V01) {627 pr_err("PDR: %s request for PD restart failed: 0x%x\n",628 req.service_path, resp.resp.error);629 return -EREMOTEIO;630 }631 632 return 0;633}634EXPORT_SYMBOL_GPL(pdr_restart_pd);635 636/**637 * pdr_handle_alloc() - initialize the PDR client handle638 * @status: function to be called on PD state change639 * @priv: handle for client's use640 *641 * Initializes the PDR client handle to allow for tracking/restart of PDs.642 *643 * Return: pdr_handle object on success, ERR_PTR on failure.644 */645struct pdr_handle *pdr_handle_alloc(void (*status)(int state,646 char *service_path,647 void *priv), void *priv)648{649 int ret;650 651 if (!status)652 return ERR_PTR(-EINVAL);653 654 struct pdr_handle *pdr __free(kfree) = kzalloc(sizeof(*pdr), GFP_KERNEL);655 if (!pdr)656 return ERR_PTR(-ENOMEM);657 658 pdr->status = status;659 pdr->priv = priv;660 661 mutex_init(&pdr->status_lock);662 mutex_init(&pdr->list_lock);663 mutex_init(&pdr->lock);664 665 INIT_LIST_HEAD(&pdr->lookups);666 INIT_LIST_HEAD(&pdr->indack_list);667 668 INIT_WORK(&pdr->locator_work, pdr_locator_work);669 INIT_WORK(&pdr->notifier_work, pdr_notifier_work);670 INIT_WORK(&pdr->indack_work, pdr_indack_work);671 672 pdr->notifier_wq = create_singlethread_workqueue("pdr_notifier_wq");673 if (!pdr->notifier_wq)674 return ERR_PTR(-ENOMEM);675 676 pdr->indack_wq = alloc_ordered_workqueue("pdr_indack_wq", WQ_HIGHPRI);677 if (!pdr->indack_wq) {678 ret = -ENOMEM;679 goto destroy_notifier;680 }681 682 ret = qmi_handle_init(&pdr->locator_hdl,683 SERVREG_GET_DOMAIN_LIST_RESP_MAX_LEN,684 &pdr_locator_ops, NULL);685 if (ret < 0)686 goto destroy_indack;687 688 ret = qmi_add_lookup(&pdr->locator_hdl, SERVREG_LOCATOR_SERVICE, 1, 1);689 if (ret < 0)690 goto release_qmi_handle;691 692 ret = qmi_handle_init(&pdr->notifier_hdl,693 SERVREG_STATE_UPDATED_IND_MAX_LEN,694 &pdr_notifier_ops,695 qmi_indication_handler);696 if (ret < 0)697 goto release_qmi_handle;698 699 return_ptr(pdr);700 701release_qmi_handle:702 qmi_handle_release(&pdr->locator_hdl);703destroy_indack:704 destroy_workqueue(pdr->indack_wq);705destroy_notifier:706 destroy_workqueue(pdr->notifier_wq);707 708 return ERR_PTR(ret);709}710EXPORT_SYMBOL_GPL(pdr_handle_alloc);711 712/**713 * pdr_handle_release() - release the PDR client handle714 * @pdr: PDR client handle715 *716 * Cleans up pending tracking requests and releases the underlying qmi handles.717 */718void pdr_handle_release(struct pdr_handle *pdr)719{720 struct pdr_service *pds, *tmp;721 722 if (IS_ERR_OR_NULL(pdr))723 return;724 725 mutex_lock(&pdr->list_lock);726 list_for_each_entry_safe(pds, tmp, &pdr->lookups, node) {727 list_del(&pds->node);728 kfree(pds);729 }730 mutex_unlock(&pdr->list_lock);731 732 cancel_work_sync(&pdr->locator_work);733 cancel_work_sync(&pdr->notifier_work);734 cancel_work_sync(&pdr->indack_work);735 736 destroy_workqueue(pdr->notifier_wq);737 destroy_workqueue(pdr->indack_wq);738 739 qmi_handle_release(&pdr->locator_hdl);740 qmi_handle_release(&pdr->notifier_hdl);741 742 kfree(pdr);743}744EXPORT_SYMBOL_GPL(pdr_handle_release);745 746MODULE_LICENSE("GPL v2");747MODULE_DESCRIPTION("Qualcomm Protection Domain Restart helpers");748