wda_qmi.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582
  1. /* Copyright (c) 2018-2021, The Linux Foundation. All rights reserved.
  2. * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
  3. *
  4. * This program is free software; you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License version 2 and
  6. * only version 2 as published by the Free Software Foundation.
  7. *
  8. * This program is distributed in the hope that it will be useful,
  9. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. * GNU General Public License for more details.
  12. *
  13. */
  14. #include <linux/rtnetlink.h>
  15. #include <linux/soc/qcom/qmi.h>
  16. #include "rmnet_qmi.h"
  17. #define CREATE_TRACE_POINTS
  18. #include "wda.h"
  19. #undef CREATE_TRACE_POINTS
  20. #include "qmi_rmnet_i.h"
  21. struct wda_qmi_data {
  22. void *rmnet_port;
  23. struct workqueue_struct *wda_wq;
  24. struct work_struct svc_arrive;
  25. struct qmi_handle handle;
  26. struct sockaddr_qrtr ssctl;
  27. struct svc_info svc;
  28. int restart_state;
  29. };
  30. static void wda_svc_config(struct work_struct *work);
  31. /* **************************************************** */
  32. #define WDA_SERVICE_ID_V01 0x1A
  33. #define WDA_SERVICE_VERS_V01 0x01
  34. #define WDA_TIMEOUT_JF msecs_to_jiffies(1000)
  35. #define QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01 0x002D
  36. #define QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01 0x002D
  37. #define QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01_MAX_MSG_LEN 18
  38. #define QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01_MAX_MSG_LEN 14
  39. #define QMI_WDA_SET_POWERSAVE_MODE_REQ_V01 0x002E
  40. #define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01 0x002E
  41. #define QMI_WDA_SET_POWERSAVE_MODE_REQ_V01_MAX_MSG_LEN 48
  42. #define QMI_WDA_SET_POWERSAVE_MODE_RESP_V01_MAX_MSG_LEN 7
  43. enum wda_powersave_config_mask_enum_v01 {
  44. WDA_DATA_POWERSAVE_CONFIG_MASK_ENUM_MIN_ENUM_VAL_V01 = -2147483647,
  45. WDA_DATA_POWERSAVE_CONFIG_NOT_SUPPORTED = 0x00,
  46. WDA_DATA_POWERSAVE_CONFIG_DL_MARKER_V01 = 0x01,
  47. WDA_DATA_POWERSAVE_CONFIG_FLOW_CTL_V01 = 0x02,
  48. WDA_DATA_POWERSAVE_CONFIG_ALL_MASK_V01 = 0x7FFFFFFF,
  49. WDA_DATA_POWERSAVE_CONFIG_MASK_ENUM_MAX_ENUM_VAL_V01 = 2147483647
  50. };
  51. struct wda_set_powersave_config_req_msg_v01 {
  52. /* Mandatory */
  53. struct data_ep_id_type_v01 ep_id;
  54. /* Optional */
  55. uint8_t req_data_cfg_valid;
  56. enum wda_powersave_config_mask_enum_v01 req_data_cfg;
  57. };
  58. struct wda_set_powersave_config_resp_msg_v01 {
  59. /* Mandatory */
  60. struct qmi_response_type_v01 resp;
  61. /* Optional */
  62. uint8_t data_cfg_valid;
  63. enum wda_powersave_config_mask_enum_v01 data_cfg;
  64. };
  65. struct wda_set_powersave_mode_req_msg_v01 {
  66. /* Mandatory */
  67. uint8_t powersave_control_flag;
  68. /* Optional */
  69. uint8_t allow_dfc_notify_valid;
  70. uint8_t allow_dfc_notify;
  71. uint8_t allow_bearer_id_list_valid;
  72. uint8_t allow_bearer_id_list_len;
  73. uint8_t allow_bearer_id_list[PS_MAX_BEARERS];
  74. uint8_t auto_shut_allow_bearer_valid;
  75. uint8_t auto_shut_allow_bearer;
  76. };
  77. struct wda_set_powersave_mode_resp_msg_v01 {
  78. /* Mandatory */
  79. struct qmi_response_type_v01 resp;
  80. };
  81. static struct qmi_elem_info wda_set_powersave_config_req_msg_v01_ei[] = {
  82. {
  83. .data_type = QMI_STRUCT,
  84. .elem_len = 1,
  85. .elem_size = sizeof(struct data_ep_id_type_v01),
  86. .array_type = NO_ARRAY,
  87. .tlv_type = 0x01,
  88. .offset = offsetof(struct
  89. wda_set_powersave_config_req_msg_v01,
  90. ep_id),
  91. .ei_array = data_ep_id_type_v01_ei,
  92. },
  93. {
  94. .data_type = QMI_OPT_FLAG,
  95. .elem_len = 1,
  96. .elem_size = sizeof(uint8_t),
  97. .array_type = NO_ARRAY,
  98. .tlv_type = 0x10,
  99. .offset = offsetof(struct
  100. wda_set_powersave_config_req_msg_v01,
  101. req_data_cfg_valid),
  102. .ei_array = NULL,
  103. },
  104. {
  105. .data_type = QMI_SIGNED_4_BYTE_ENUM,
  106. .elem_len = 1,
  107. .elem_size = sizeof(enum
  108. wda_powersave_config_mask_enum_v01),
  109. .array_type = NO_ARRAY,
  110. .tlv_type = 0x10,
  111. .offset = offsetof(struct
  112. wda_set_powersave_config_req_msg_v01,
  113. req_data_cfg),
  114. .ei_array = NULL,
  115. },
  116. {
  117. .data_type = QMI_EOTI,
  118. .array_type = NO_ARRAY,
  119. .tlv_type = QMI_COMMON_TLV_TYPE,
  120. },
  121. };
  122. static struct qmi_elem_info wda_set_powersave_config_resp_msg_v01_ei[] = {
  123. {
  124. .data_type = QMI_STRUCT,
  125. .elem_len = 1,
  126. .elem_size = sizeof(struct qmi_response_type_v01),
  127. .array_type = NO_ARRAY,
  128. .tlv_type = 0x02,
  129. .offset = offsetof(struct
  130. wda_set_powersave_config_resp_msg_v01,
  131. resp),
  132. .ei_array = qmi_response_type_v01_ei,
  133. },
  134. {
  135. .data_type = QMI_OPT_FLAG,
  136. .elem_len = 1,
  137. .elem_size = sizeof(uint8_t),
  138. .array_type = NO_ARRAY,
  139. .tlv_type = 0x10,
  140. .offset = offsetof(struct
  141. wda_set_powersave_config_resp_msg_v01,
  142. data_cfg_valid),
  143. .ei_array = NULL,
  144. },
  145. {
  146. .data_type = QMI_SIGNED_4_BYTE_ENUM,
  147. .elem_len = 1,
  148. .elem_size = sizeof(enum
  149. wda_powersave_config_mask_enum_v01),
  150. .array_type = NO_ARRAY,
  151. .tlv_type = 0x10,
  152. .offset = offsetof(struct
  153. wda_set_powersave_config_resp_msg_v01,
  154. data_cfg),
  155. .ei_array = NULL,
  156. },
  157. {
  158. .data_type = QMI_EOTI,
  159. .array_type = NO_ARRAY,
  160. .tlv_type = QMI_COMMON_TLV_TYPE,
  161. },
  162. };
  163. static struct qmi_elem_info wda_set_powersave_mode_req_msg_v01_ei[] = {
  164. {
  165. .data_type = QMI_UNSIGNED_1_BYTE,
  166. .elem_len = 1,
  167. .elem_size = sizeof(uint8_t),
  168. .array_type = NO_ARRAY,
  169. .tlv_type = 0x01,
  170. .offset = offsetof(struct
  171. wda_set_powersave_mode_req_msg_v01,
  172. powersave_control_flag),
  173. .ei_array = NULL,
  174. },
  175. {
  176. .data_type = QMI_OPT_FLAG,
  177. .elem_len = 1,
  178. .elem_size = sizeof(u8),
  179. .array_type = NO_ARRAY,
  180. .tlv_type = 0x10,
  181. .offset = offsetof(struct
  182. wda_set_powersave_mode_req_msg_v01,
  183. allow_dfc_notify_valid),
  184. .ei_array = NULL,
  185. },
  186. {
  187. .data_type = QMI_UNSIGNED_1_BYTE,
  188. .elem_len = 1,
  189. .elem_size = sizeof(u8),
  190. .array_type = NO_ARRAY,
  191. .tlv_type = 0x10,
  192. .offset = offsetof(struct
  193. wda_set_powersave_mode_req_msg_v01,
  194. allow_dfc_notify),
  195. .ei_array = NULL,
  196. },
  197. {
  198. .data_type = QMI_OPT_FLAG,
  199. .elem_len = 1,
  200. .elem_size = sizeof(u8),
  201. .array_type = NO_ARRAY,
  202. .tlv_type = 0x11,
  203. .offset = offsetof(struct
  204. wda_set_powersave_mode_req_msg_v01,
  205. allow_bearer_id_list_valid),
  206. .ei_array = NULL,
  207. },
  208. {
  209. .data_type = QMI_DATA_LEN,
  210. .elem_len = 1,
  211. .elem_size = sizeof(u8),
  212. .array_type = NO_ARRAY,
  213. .tlv_type = 0x11,
  214. .offset = offsetof(struct
  215. wda_set_powersave_mode_req_msg_v01,
  216. allow_bearer_id_list_len),
  217. .ei_array = NULL,
  218. },
  219. {
  220. .data_type = QMI_UNSIGNED_1_BYTE,
  221. .elem_len = PS_MAX_BEARERS,
  222. .elem_size = sizeof(u8),
  223. .array_type = VAR_LEN_ARRAY,
  224. .tlv_type = 0x11,
  225. .offset = offsetof(struct
  226. wda_set_powersave_mode_req_msg_v01,
  227. allow_bearer_id_list),
  228. .ei_array = NULL,
  229. },
  230. {
  231. .data_type = QMI_OPT_FLAG,
  232. .elem_len = 1,
  233. .elem_size = sizeof(u8),
  234. .array_type = NO_ARRAY,
  235. .tlv_type = 0x12,
  236. .offset = offsetof(struct
  237. wda_set_powersave_mode_req_msg_v01,
  238. auto_shut_allow_bearer_valid),
  239. .ei_array = NULL,
  240. },
  241. {
  242. .data_type = QMI_UNSIGNED_1_BYTE,
  243. .elem_len = 1,
  244. .elem_size = sizeof(u8),
  245. .array_type = NO_ARRAY,
  246. .tlv_type = 0x12,
  247. .offset = offsetof(struct
  248. wda_set_powersave_mode_req_msg_v01,
  249. auto_shut_allow_bearer),
  250. .ei_array = NULL,
  251. },
  252. {
  253. .data_type = QMI_EOTI,
  254. .array_type = NO_ARRAY,
  255. .tlv_type = QMI_COMMON_TLV_TYPE,
  256. },
  257. };
  258. static struct qmi_elem_info wda_set_powersave_mode_resp_msg_v01_ei[] = {
  259. {
  260. .data_type = QMI_STRUCT,
  261. .elem_len = 1,
  262. .elem_size = sizeof(struct qmi_response_type_v01),
  263. .array_type = NO_ARRAY,
  264. .tlv_type = 0x02,
  265. .offset = offsetof(struct
  266. wda_set_powersave_mode_resp_msg_v01,
  267. resp),
  268. .ei_array = qmi_response_type_v01_ei,
  269. },
  270. {
  271. .data_type = QMI_EOTI,
  272. .array_type = NO_ARRAY,
  273. .tlv_type = QMI_COMMON_TLV_TYPE,
  274. },
  275. };
  276. static int wda_set_powersave_mode_req(void *wda_data, uint8_t enable,
  277. u8 num_bearers, u8 *bearer_id)
  278. {
  279. struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data;
  280. struct wda_set_powersave_mode_resp_msg_v01 *resp;
  281. struct wda_set_powersave_mode_req_msg_v01 *req;
  282. struct qmi_txn txn;
  283. int ret;
  284. if (!data || !data->rmnet_port)
  285. return -EINVAL;
  286. req = kzalloc(sizeof(*req), GFP_ATOMIC);
  287. if (!req)
  288. return -ENOMEM;
  289. resp = kzalloc(sizeof(*resp), GFP_ATOMIC);
  290. if (!resp) {
  291. kfree(req);
  292. return -ENOMEM;
  293. }
  294. ret = qmi_txn_init(&data->handle, &txn,
  295. wda_set_powersave_mode_resp_msg_v01_ei, resp);
  296. if (ret < 0) {
  297. pr_err("%s() Failed init for response, err: %d\n",
  298. __func__, ret);
  299. goto out;
  300. }
  301. req->powersave_control_flag = enable;
  302. if (enable && num_bearers && bearer_id &&
  303. num_bearers <= PS_MAX_BEARERS) {
  304. req->allow_dfc_notify_valid = 1;
  305. req->allow_dfc_notify = 1;
  306. req->allow_bearer_id_list_valid = 1;
  307. req->allow_bearer_id_list_len = num_bearers;
  308. memcpy(req->allow_bearer_id_list, bearer_id, num_bearers);
  309. req->auto_shut_allow_bearer_valid = 1;
  310. req->auto_shut_allow_bearer = 1;
  311. }
  312. ret = qmi_send_request(&data->handle, &data->ssctl, &txn,
  313. QMI_WDA_SET_POWERSAVE_MODE_REQ_V01,
  314. QMI_WDA_SET_POWERSAVE_MODE_REQ_V01_MAX_MSG_LEN,
  315. wda_set_powersave_mode_req_msg_v01_ei, req);
  316. if (ret < 0) {
  317. qmi_txn_cancel(&txn);
  318. pr_err("%s() Failed sending request, err: %d\n",
  319. __func__, ret);
  320. goto out;
  321. }
  322. ret = qmi_txn_wait(&txn, WDA_TIMEOUT_JF);
  323. if (ret < 0) {
  324. pr_err("%s() Response waiting failed, err: %d\n",
  325. __func__, ret);
  326. } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) {
  327. pr_err("%s() Request rejected, result: %d, err: %d\n",
  328. __func__, resp->resp.result, resp->resp.error);
  329. ret = -resp->resp.result;
  330. }
  331. out:
  332. kfree(resp);
  333. kfree(req);
  334. return ret;
  335. }
  336. static int wda_set_powersave_config_req(struct qmi_handle *wda_handle,
  337. int dl_marker)
  338. {
  339. struct wda_qmi_data *data = container_of(wda_handle,
  340. struct wda_qmi_data, handle);
  341. struct wda_set_powersave_config_resp_msg_v01 *resp;
  342. struct wda_set_powersave_config_req_msg_v01 *req;
  343. struct qmi_txn txn;
  344. int ret;
  345. req = kzalloc(sizeof(*req), GFP_ATOMIC);
  346. if (!req)
  347. return -ENOMEM;
  348. resp = kzalloc(sizeof(*resp), GFP_ATOMIC);
  349. if (!resp) {
  350. kfree(req);
  351. return -ENOMEM;
  352. }
  353. ret = qmi_txn_init(wda_handle, &txn,
  354. wda_set_powersave_config_resp_msg_v01_ei, resp);
  355. if (ret < 0) {
  356. pr_err("%s() Failed init for response, err: %d\n",
  357. __func__, ret);
  358. goto out;
  359. }
  360. req->ep_id.ep_type = data->svc.ep_type;
  361. req->ep_id.iface_id = data->svc.iface_id;
  362. req->req_data_cfg_valid = 1;
  363. req->req_data_cfg = dl_marker ? WDA_DATA_POWERSAVE_CONFIG_ALL_MASK_V01 :
  364. WDA_DATA_POWERSAVE_CONFIG_FLOW_CTL_V01;
  365. ret = qmi_send_request(wda_handle, &data->ssctl, &txn,
  366. QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01,
  367. QMI_WDA_SET_POWERSAVE_CONFIG_REQ_V01_MAX_MSG_LEN,
  368. wda_set_powersave_config_req_msg_v01_ei, req);
  369. if (ret < 0) {
  370. qmi_txn_cancel(&txn);
  371. pr_err("%s() Failed sending request, err: %d\n", __func__, ret);
  372. goto out;
  373. }
  374. ret = qmi_txn_wait(&txn, WDA_TIMEOUT_JF);
  375. if (ret < 0) {
  376. pr_err("%s() Response waiting failed, err: %d\n",
  377. __func__, ret);
  378. } else if (resp->resp.result != QMI_RESULT_SUCCESS_V01) {
  379. pr_err("%s() Request rejected, result: %d, error: %d\n",
  380. __func__, resp->resp.result, resp->resp.error);
  381. ret = -resp->resp.result;
  382. }
  383. out:
  384. kfree(resp);
  385. kfree(req);
  386. return ret;
  387. }
  388. static void wda_svc_config(struct work_struct *work)
  389. {
  390. struct wda_qmi_data *data = container_of(work, struct wda_qmi_data,
  391. svc_arrive);
  392. struct qmi_info *qmi;
  393. int rc, dl_marker = 0;
  394. while (!rtnl_trylock()) {
  395. if (!data->restart_state)
  396. cond_resched();
  397. else
  398. return;
  399. }
  400. dl_marker = rmnet_get_dlmarker_info(data->rmnet_port);
  401. rtnl_unlock();
  402. if (data->restart_state == 1)
  403. return;
  404. rc = wda_set_powersave_config_req(&data->handle, dl_marker);
  405. if (rc < 0) {
  406. pr_err("%s Failed to init service, err[%d]\n", __func__, rc);
  407. return;
  408. }
  409. if (data->restart_state == 1)
  410. return;
  411. while (!rtnl_trylock()) {
  412. if (!data->restart_state)
  413. cond_resched();
  414. else
  415. return;
  416. }
  417. qmi = (struct qmi_info *)rmnet_get_qmi_pt(data->rmnet_port);
  418. if (!qmi) {
  419. rtnl_unlock();
  420. return;
  421. }
  422. qmi->wda_pending = NULL;
  423. qmi->wda_client = (void *)data;
  424. trace_wda_client_state_up(data->svc.instance,
  425. data->svc.ep_type,
  426. data->svc.iface_id);
  427. rtnl_unlock();
  428. pr_info("Connection established with the WDA Service, DL Marker %s\n",
  429. dl_marker ? "enabled" : "disabled");
  430. }
  431. static int wda_svc_arrive(struct qmi_handle *qmi, struct qmi_service *svc)
  432. {
  433. struct wda_qmi_data *data = container_of(qmi, struct wda_qmi_data,
  434. handle);
  435. data->ssctl.sq_family = AF_QIPCRTR;
  436. data->ssctl.sq_node = svc->node;
  437. data->ssctl.sq_port = svc->port;
  438. queue_work(data->wda_wq, &data->svc_arrive);
  439. return 0;
  440. }
  441. static void wda_svc_exit(struct qmi_handle *qmi, struct qmi_service *svc)
  442. {
  443. struct wda_qmi_data *data = container_of(qmi, struct wda_qmi_data,
  444. handle);
  445. if (!data)
  446. pr_info("%s() data is null\n", __func__);
  447. }
  448. static struct qmi_ops server_ops = {
  449. .new_server = wda_svc_arrive,
  450. .del_server = wda_svc_exit,
  451. };
  452. int
  453. wda_qmi_client_init(void *port, struct svc_info *psvc, struct qmi_info *qmi)
  454. {
  455. struct wda_qmi_data *data;
  456. int rc = -ENOMEM;
  457. if (!port || !qmi)
  458. return -EINVAL;
  459. data = kzalloc(sizeof(*data), GFP_KERNEL);
  460. if (!data)
  461. return -ENOMEM;
  462. data->wda_wq = create_singlethread_workqueue("wda_wq");
  463. if (!data->wda_wq) {
  464. pr_err("%s Could not create workqueue\n", __func__);
  465. goto err0;
  466. }
  467. data->rmnet_port = port;
  468. data->restart_state = 0;
  469. memcpy(&data->svc, psvc, sizeof(data->svc));
  470. INIT_WORK(&data->svc_arrive, wda_svc_config);
  471. rc = qmi_handle_init(&data->handle,
  472. QMI_WDA_SET_POWERSAVE_CONFIG_RESP_V01_MAX_MSG_LEN,
  473. &server_ops, NULL);
  474. if (rc < 0) {
  475. pr_err("%s: Failed qmi_handle_init, err: %d\n", __func__, rc);
  476. goto err1;
  477. }
  478. rc = qmi_add_lookup(&data->handle, WDA_SERVICE_ID_V01,
  479. WDA_SERVICE_VERS_V01, psvc->instance);
  480. if (rc < 0) {
  481. pr_err("%s(): Failed qmi_add_lookup, err: %d\n", __func__, rc);
  482. goto err2;
  483. }
  484. qmi->wda_pending = (void *)data;
  485. return 0;
  486. err2:
  487. qmi_handle_release(&data->handle);
  488. err1:
  489. destroy_workqueue(data->wda_wq);
  490. err0:
  491. kfree(data);
  492. return rc;
  493. }
  494. void wda_qmi_client_exit(void *wda_data)
  495. {
  496. struct wda_qmi_data *data = (struct wda_qmi_data *)wda_data;
  497. if (!data) {
  498. pr_info("%s() data is null\n", __func__);
  499. return;
  500. }
  501. data->restart_state = 1;
  502. trace_wda_client_state_down(0);
  503. destroy_workqueue(data->wda_wq);
  504. kfree(data);
  505. }
  506. int wda_set_powersave_mode(void *wda_data, uint8_t enable, u8 num_bearers,
  507. u8 *bearer_id)
  508. {
  509. trace_wda_set_powersave_mode(enable);
  510. return wda_set_powersave_mode_req(wda_data, enable, num_bearers,
  511. bearer_id);
  512. }
  513. void wda_qmi_client_release(void *wda_data)
  514. {
  515. if (!wda_data)
  516. return;
  517. qmi_handle_release(&((struct wda_qmi_data *)wda_data)->handle);
  518. }