123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582 |
- /* Copyright (c) 2018-2021, The Linux Foundation. All rights reserved.
- * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- */
- #include <linux/rtnetlink.h>
- #include <linux/soc/qcom/qmi.h>
- #include "rmnet_qmi.h"
- #define CREATE_TRACE_POINTS
- #include "wda.h"
- #undef CREATE_TRACE_POINTS
- #include "qmi_rmnet_i.h"
- struct wda_qmi_data {
- void *rmnet_port;
- struct workqueue_struct *wda_wq;
- struct work_struct svc_arrive;
- struct qmi_handle handle;
- struct sockaddr_qrtr ssctl;
- struct svc_info svc;
- int restart_state;
- };
- static void wda_svc_config(struct work_struct *work);
- /* **************************************************** */
- #define WDA_SERVICE_ID_V01 0x1A
- #define WDA_SERVICE_VERS_V01 0x01
- #define WDA_TIMEOUT_JF msecs_to_jiffies(1000)
- #define QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01 0x002D
- #define QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01 0x002D
- #define QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01_MAX_MSG_LEN 18
- #define QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01_MAX_MSG_LEN 14
- #define QMI_WDA_SET_POWERSAVE_MODE_REQ_V01 0x002E
- #define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01 0x002E
- #define QMI_WDA_SET_POWERSAVE_MODE_REQ_V01_MAX_MSG_LEN 48
- #define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01_MAX_MSG_LEN 7
- enum wda_powersave_config_mask_enum_v01 {
- WDA_DATA_POWERSAVE_CONFIG_MASK_ENUM_MIN_ENUM_VAL_V01 = -2147483647,
- WDA_DATA_POWERSAVE_CONFIG_NOT_SUPPORTED = 0x00,
- WDA_DATA_POWERSAVE_CONFIG_DL_MARKER_V01 = 0x01,
- WDA_DATA_POWERSAVE_CONFIG_FLOW_CTL_V01 = 0x02,
- WDA_DATA_POWERSAVE_CONFIG_ALL_MASK_V01 = 0x7FFFFFFF,
- WDA_DATA_POWERSAVE_CONFIG_MASK_ENUM_MAX_ENUM_VAL_V01 = 2147483647
- };
- struct wda_set_powersave_config_req_msg_v01 {
- /* Mandatory */
- struct data_ep_id_type_v01 ep_id;
- /* Optional */
- uint8_t req_data_cfg_valid;
- enum wda_powersave_config_mask_enum_v01 req_data_cfg;
- };
- struct wda_set_powersave_config_resp_msg_v01 {
- /* Mandatory */
- struct qmi_response_type_v01 resp;
- /* Optional */
- uint8_t data_cfg_valid;
- enum wda_powersave_config_mask_enum_v01 data_cfg;
- };
- struct wda_set_powersave_mode_req_msg_v01 {
- /* Mandatory */
- uint8_t powersave_control_flag;
- /* Optional */
- uint8_t allow_dfc_notify_valid;
- uint8_t allow_dfc_notify;
- uint8_t allow_bearer_id_list_valid;
- uint8_t allow_bearer_id_list_len;
- uint8_t allow_bearer_id_list[PS_MAX_BEARERS];
- uint8_t auto_shut_allow_bearer_valid;
- uint8_t auto_shut_allow_bearer;
- };
- struct wda_set_powersave_mode_resp_msg_v01 {
- /* Mandatory */
- struct qmi_response_type_v01 resp;
- };
- static struct qmi_elem_info wda_set_powersave_config_req_msg_v01_ei[] = {
- {
- .data_type = QMI_STRUCT,
- .elem_len = 1,
- .elem_size = sizeof(struct data_ep_id_type_v01),
- .array_type = NO_ARRAY,
- .tlv_type = 0x01,
- .offset = offsetof(struct
- wda_set_powersave_config_req_msg_v01,
- ep_id),
- .ei_array = data_ep_id_type_v01_ei,
- },
- {
- .data_type = QMI_OPT_FLAG,
- .elem_len = 1,
- .elem_size = sizeof(uint8_t),
- .array_type = NO_ARRAY,
- .tlv_type = 0x10,
- .offset = offsetof(struct
- wda_set_powersave_config_req_msg_v01,
- req_data_cfg_valid),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_SIGNED_4_BYTE_ENUM,
- .elem_len = 1,
- .elem_size = sizeof(enum
- wda_powersave_config_mask_enum_v01),
- .array_type = NO_ARRAY,
- .tlv_type = 0x10,
- .offset = offsetof(struct
- wda_set_powersave_config_req_msg_v01,
- req_data_cfg),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_EOTI,
- .array_type = NO_ARRAY,
- .tlv_type = QMI_COMMON_TLV_TYPE,
- },
- };
- static struct qmi_elem_info wda_set_powersave_config_resp_msg_v01_ei[] = {
- {
- .data_type = QMI_STRUCT,
- .elem_len = 1,
- .elem_size = sizeof(struct qmi_response_type_v01),
- .array_type = NO_ARRAY,
- .tlv_type = 0x02,
- .offset = offsetof(struct
- wda_set_powersave_config_resp_msg_v01,
- resp),
- .ei_array = qmi_response_type_v01_ei,
- },
- {
- .data_type = QMI_OPT_FLAG,
- .elem_len = 1,
- .elem_size = sizeof(uint8_t),
- .array_type = NO_ARRAY,
- .tlv_type = 0x10,
- .offset = offsetof(struct
- wda_set_powersave_config_resp_msg_v01,
- data_cfg_valid),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_SIGNED_4_BYTE_ENUM,
- .elem_len = 1,
- .elem_size = sizeof(enum
- wda_powersave_config_mask_enum_v01),
- .array_type = NO_ARRAY,
- .tlv_type = 0x10,
- .offset = offsetof(struct
- wda_set_powersave_config_resp_msg_v01,
- data_cfg),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_EOTI,
- .array_type = NO_ARRAY,
- .tlv_type = QMI_COMMON_TLV_TYPE,
- },
- };
- static struct qmi_elem_info wda_set_powersave_mode_req_msg_v01_ei[] = {
- {
- .data_type = QMI_UNSIGNED_1_BYTE,
- .elem_len = 1,
- .elem_size = sizeof(uint8_t),
- .array_type = NO_ARRAY,
- .tlv_type = 0x01,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- powersave_control_flag),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_OPT_FLAG,
- .elem_len = 1,
- .elem_size = sizeof(u8),
- .array_type = NO_ARRAY,
- .tlv_type = 0x10,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- allow_dfc_notify_valid),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_UNSIGNED_1_BYTE,
- .elem_len = 1,
- .elem_size = sizeof(u8),
- .array_type = NO_ARRAY,
- .tlv_type = 0x10,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- allow_dfc_notify),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_OPT_FLAG,
- .elem_len = 1,
- .elem_size = sizeof(u8),
- .array_type = NO_ARRAY,
- .tlv_type = 0x11,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- allow_bearer_id_list_valid),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_DATA_LEN,
- .elem_len = 1,
- .elem_size = sizeof(u8),
- .array_type = NO_ARRAY,
- .tlv_type = 0x11,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- allow_bearer_id_list_len),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_UNSIGNED_1_BYTE,
- .elem_len = PS_MAX_BEARERS,
- .elem_size = sizeof(u8),
- .array_type = VAR_LEN_ARRAY,
- .tlv_type = 0x11,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- allow_bearer_id_list),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_OPT_FLAG,
- .elem_len = 1,
- .elem_size = sizeof(u8),
- .array_type = NO_ARRAY,
- .tlv_type = 0x12,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- auto_shut_allow_bearer_valid),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_UNSIGNED_1_BYTE,
- .elem_len = 1,
- .elem_size = sizeof(u8),
- .array_type = NO_ARRAY,
- .tlv_type = 0x12,
- .offset = offsetof(struct
- wda_set_powersave_mode_req_msg_v01,
- auto_shut_allow_bearer),
- .ei_array = NULL,
- },
- {
- .data_type = QMI_EOTI,
- .array_type = NO_ARRAY,
- .tlv_type = QMI_COMMON_TLV_TYPE,
- },
- };
- static struct qmi_elem_info wda_set_powersave_mode_resp_msg_v01_ei[] = {
- {
- .data_type = QMI_STRUCT,
- .elem_len = 1,
- .elem_size = sizeof(struct qmi_response_type_v01),
- .array_type = NO_ARRAY,
- .tlv_type = 0x02,
- .offset = offsetof(struct
- wda_set_powersave_mode_resp_msg_v01,
- resp),
- .ei_array = qmi_response_type_v01_ei,
- },
- {
- .data_type = QMI_EOTI,
- .array_type = NO_ARRAY,
- .tlv_type = QMI_COMMON_TLV_TYPE,
- },
- };
- static int wda_set_powersave_mode_req(void *wda_data, uint8_t enable,
- u8 num_bearers, u8 *bearer_id)
- {
- struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data;
- struct wda_set_powersave_mode_resp_msg_v01 *resp;
- struct wda_set_powersave_mode_req_msg_v01 *req;
- struct qmi_txn txn;
- int ret;
- if (!data || !data->rmnet_port)
- return -EINVAL;
- req = kzalloc(sizeof(*req), GFP_ATOMIC);
- if (!req)
- return -ENOMEM;
- resp = kzalloc(sizeof(*resp), GFP_ATOMIC);
- if (!resp) {
- kfree(req);
- return -ENOMEM;
- }
- ret = qmi_txn_init(&data->handle, &txn,
- wda_set_powersave_mode_resp_msg_v01_ei, resp);
- if (ret < 0) {
- pr_err("%s() Failed init for response, err: %d\n",
- __func__, ret);
- goto out;
- }
- req->powersave_control_flag = enable;
- if (enable && num_bearers && bearer_id &&
- num_bearers <= PS_MAX_BEARERS) {
- req->allow_dfc_notify_valid = 1;
- req->allow_dfc_notify = 1;
- req->allow_bearer_id_list_valid = 1;
- req->allow_bearer_id_list_len = num_bearers;
- memcpy(req->allow_bearer_id_list, bearer_id, num_bearers);
- req->auto_shut_allow_bearer_valid = 1;
- req->auto_shut_allow_bearer = 1;
- }
- ret = qmi_send_request(&data->handle, &data->ssctl, &txn,
- QMI_WDA_SET_POWERSAVE_MODE_REQ_V01,
- QMI_WDA_SET_POWERSAVE_MODE_REQ_V01_MAX_MSG_LEN,
- wda_set_powersave_mode_req_msg_v01_ei, req);
- if (ret < 0) {
- qmi_txn_cancel(&txn);
- pr_err("%s() Failed sending request, err: %d\n",
- __func__, ret);
- goto out;
- }
- ret = qmi_txn_wait(&txn, WDA_TIMEOUT_JF);
- if (ret < 0) {
- pr_err("%s() Response waiting failed, err: %d\n",
- __func__, ret);
- } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) {
- pr_err("%s() Request rejected, result: %d, err: %d\n",
- __func__, resp->resp.result, resp->resp.error);
- ret = -resp->resp.result;
- }
- out:
- kfree(resp);
- kfree(req);
- return ret;
- }
- static int wda_set_powersave_config_req(struct qmi_handle *wda_handle,
- int dl_marker)
- {
- struct wda_qmi_data *data = container_of(wda_handle,
- struct wda_qmi_data, handle);
- struct wda_set_powersave_config_resp_msg_v01 *resp;
- struct wda_set_powersave_config_req_msg_v01 *req;
- struct qmi_txn txn;
- int ret;
- req = kzalloc(sizeof(*req), GFP_ATOMIC);
- if (!req)
- return -ENOMEM;
- resp = kzalloc(sizeof(*resp), GFP_ATOMIC);
- if (!resp) {
- kfree(req);
- return -ENOMEM;
- }
- ret = qmi_txn_init(wda_handle, &txn,
- wda_set_powersave_config_resp_msg_v01_ei, resp);
- if (ret < 0) {
- pr_err("%s() Failed init for response, err: %d\n",
- __func__, ret);
- goto out;
- }
- req->ep_id.ep_type = data->svc.ep_type;
- req->ep_id.iface_id = data->svc.iface_id;
- req->req_data_cfg_valid = 1;
- req->req_data_cfg = dl_marker ? WDA_DATA_POWERSAVE_CONFIG_ALL_MASK_V01 :
- WDA_DATA_POWERSAVE_CONFIG_FLOW_CTL_V01;
- ret = qmi_send_request(wda_handle, &data->ssctl, &txn,
- QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01,
- QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01_MAX_MSG_LEN,
- wda_set_powersave_config_req_msg_v01_ei, req);
- if (ret < 0) {
- qmi_txn_cancel(&txn);
- pr_err("%s() Failed sending request, err: %d\n", __func__, ret);
- goto out;
- }
- ret = qmi_txn_wait(&txn, WDA_TIMEOUT_JF);
- if (ret < 0) {
- pr_err("%s() Response waiting failed, err: %d\n",
- __func__, ret);
- } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) {
- pr_err("%s() Request rejected, result: %d, error: %d\n",
- __func__, resp->resp.result, resp->resp.error);
- ret = -resp->resp.result;
- }
- out:
- kfree(resp);
- kfree(req);
- return ret;
- }
- static void wda_svc_config(struct work_struct *work)
- {
- struct wda_qmi_data *data = container_of(work, struct wda_qmi_data,
- svc_arrive);
- struct qmi_info *qmi;
- int rc, dl_marker = 0;
- while (!rtnl_trylock()) {
- if (!data->restart_state)
- cond_resched();
- else
- return;
- }
- dl_marker = rmnet_get_dlmarker_info(data->rmnet_port);
- rtnl_unlock();
- if (data->restart_state == 1)
- return;
- rc = wda_set_powersave_config_req(&data->handle, dl_marker);
- if (rc < 0) {
- pr_err("%s Failed to init service, err[%d]\n", __func__, rc);
- return;
- }
- if (data->restart_state == 1)
- return;
- while (!rtnl_trylock()) {
- if (!data->restart_state)
- cond_resched();
- else
- return;
- }
- qmi = (struct qmi_info *)rmnet_get_qmi_pt(data->rmnet_port);
- if (!qmi) {
- rtnl_unlock();
- return;
- }
- qmi->wda_pending = NULL;
- qmi->wda_client = (void *)data;
- trace_wda_client_state_up(data->svc.instance,
- data->svc.ep_type,
- data->svc.iface_id);
- rtnl_unlock();
- pr_info("Connection established with the WDA Service, DL Marker %s\n",
- dl_marker ? "enabled" : "disabled");
- }
- static int wda_svc_arrive(struct qmi_handle *qmi, struct qmi_service *svc)
- {
- struct wda_qmi_data *data = container_of(qmi, struct wda_qmi_data,
- handle);
- data->ssctl.sq_family = AF_QIPCRTR;
- data->ssctl.sq_node = svc->node;
- data->ssctl.sq_port = svc->port;
- queue_work(data->wda_wq, &data->svc_arrive);
- return 0;
- }
- static void wda_svc_exit(struct qmi_handle *qmi, struct qmi_service *svc)
- {
- struct wda_qmi_data *data = container_of(qmi, struct wda_qmi_data,
- handle);
- if (!data)
- pr_info("%s() data is null\n", __func__);
- }
- static struct qmi_ops server_ops = {
- .new_server = wda_svc_arrive,
- .del_server = wda_svc_exit,
- };
- int
- wda_qmi_client_init(void *port, struct svc_info *psvc, struct qmi_info *qmi)
- {
- struct wda_qmi_data *data;
- int rc = -ENOMEM;
- if (!port || !qmi)
- return -EINVAL;
- data = kzalloc(sizeof(*data), GFP_KERNEL);
- if (!data)
- return -ENOMEM;
- data->wda_wq = create_singlethread_workqueue("wda_wq");
- if (!data->wda_wq) {
- pr_err("%s Could not create workqueue\n", __func__);
- goto err0;
- }
- data->rmnet_port = port;
- data->restart_state = 0;
- memcpy(&data->svc, psvc, sizeof(data->svc));
- INIT_WORK(&data->svc_arrive, wda_svc_config);
- rc = qmi_handle_init(&data->handle,
- QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01_MAX_MSG_LEN,
- &server_ops, NULL);
- if (rc < 0) {
- pr_err("%s: Failed qmi_handle_init, err: %d\n", __func__, rc);
- goto err1;
- }
- rc = qmi_add_lookup(&data->handle, WDA_SERVICE_ID_V01,
- WDA_SERVICE_VERS_V01, psvc->instance);
- if (rc < 0) {
- pr_err("%s(): Failed qmi_add_lookup, err: %d\n", __func__, rc);
- goto err2;
- }
- qmi->wda_pending = (void *)data;
- return 0;
- err2:
- qmi_handle_release(&data->handle);
- err1:
- destroy_workqueue(data->wda_wq);
- err0:
- kfree(data);
- return rc;
- }
- void wda_qmi_client_exit(void *wda_data)
- {
- struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data;
- if (!data) {
- pr_info("%s() data is null\n", __func__);
- return;
- }
- data->restart_state = 1;
- trace_wda_client_state_down(0);
- destroy_workqueue(data->wda_wq);
- kfree(data);
- }
- int wda_set_powersave_mode(void *wda_data, uint8_t enable, u8 num_bearers,
- u8 *bearer_id)
- {
- trace_wda_set_powersave_mode(enable);
- return wda_set_powersave_mode_req(wda_data, enable, num_bearers,
- bearer_id);
- }
- void wda_qmi_client_release(void *wda_data)
- {
- if (!wda_data)
- return;
- qmi_handle_release(&((struct wda_qmi_data *)wda_data)->handle);
- }
|