dfc_qmap.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (c) 2019-2021, The Linux Foundation. All rights reserved.
  4. * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
  5. */
  6. #include <net/pkt_sched.h>
  7. #include <linux/module.h>
  8. #include "rmnet_qmap.h"
  9. #include "dfc_defs.h"
  10. #include "rmnet_qmi.h"
  11. #include "qmi_rmnet.h"
  12. #include "dfc.h"
  13. #include "rmnet_map.h"
  14. #define QMAP_DFC_VER 1
  15. struct qmap_dfc_config {
  16. struct qmap_cmd_hdr hdr;
  17. u8 cmd_ver;
  18. u8 cmd_id;
  19. u8 reserved;
  20. u8 tx_info:1;
  21. u8 reserved2:7;
  22. __be32 ep_type;
  23. __be32 iface_id;
  24. u32 reserved3;
  25. } __aligned(1);
  26. struct qmap_dfc_ind {
  27. struct qmap_cmd_hdr hdr;
  28. u8 cmd_ver;
  29. u8 reserved;
  30. __be16 seq_num;
  31. u8 reserved2;
  32. u8 tx_info_valid:1;
  33. u8 tx_info:1;
  34. u8 rx_bytes_valid:1;
  35. u8 reserved3:5;
  36. u8 bearer_id;
  37. u8 tcp_bidir:1;
  38. u8 bearer_status:3;
  39. u8 ll_status:1;
  40. u8 reserved4:3;
  41. __be32 grant;
  42. __be32 rx_bytes;
  43. u32 reserved6;
  44. } __aligned(1);
  45. struct qmap_dfc_query {
  46. struct qmap_cmd_hdr hdr;
  47. u8 cmd_ver;
  48. u8 reserved;
  49. u8 bearer_id;
  50. u8 reserved2;
  51. u32 reserved3;
  52. } __aligned(1);
  53. struct qmap_dfc_query_resp {
  54. struct qmap_cmd_hdr hdr;
  55. u8 cmd_ver;
  56. u8 bearer_id;
  57. u8 tcp_bidir:1;
  58. u8 rx_bytes_valid:1;
  59. u8 reserved:6;
  60. u8 invalid:1;
  61. u8 reserved2:7;
  62. __be32 grant;
  63. __be32 rx_bytes;
  64. u32 reserved4;
  65. } __aligned(1);
  66. struct qmap_dfc_end_marker_req {
  67. struct qmap_cmd_hdr hdr;
  68. u8 cmd_ver;
  69. u8 reserved;
  70. u8 bearer_id;
  71. u8 reserved2;
  72. u16 reserved3;
  73. __be16 seq_num;
  74. u32 reserved4;
  75. } __aligned(1);
  76. struct qmap_dfc_end_marker_cnf {
  77. struct qmap_cmd_hdr hdr;
  78. u8 cmd_ver;
  79. u8 reserved;
  80. u8 bearer_id;
  81. u8 reserved2;
  82. u16 reserved3;
  83. __be16 seq_num;
  84. u32 reserved4;
  85. } __aligned(1);
  86. struct qmapv5_cmd_hdr {
  87. u8 pad_len:6;
  88. u8 next_hdr:1;
  89. u8 cd_bit:1;
  90. u8 mux_id;
  91. __be16 pkt_len;
  92. struct rmnet_map_v5_csum_header csum_hdr;
  93. u8 cmd_name;
  94. u8 cmd_type:2;
  95. u8 reserved:6;
  96. u16 reserved2;
  97. __be32 tx_id;
  98. } __aligned(1);
  99. struct qmapv5_dfc_end_marker_cnf {
  100. struct qmapv5_cmd_hdr hdr;
  101. u8 cmd_ver;
  102. u8 reserved;
  103. u8 bearer_id;
  104. u8 reserved2;
  105. u16 reserved3;
  106. __be16 seq_num;
  107. u32 reserved4;
  108. } __aligned(1);
  109. struct qmap_dfc_powersave_req {
  110. struct qmap_cmd_hdr hdr;
  111. u8 cmd_ver;
  112. u8 allow:1;
  113. u8 autoshut:1;
  114. u8 reserved:6;
  115. u8 reserved2;
  116. u8 mode:1;
  117. u8 reserved3:7;
  118. __be32 ep_type;
  119. __be32 iface_id;
  120. u8 num_bearers;
  121. u8 bearer_id[PS_MAX_BEARERS];
  122. u8 reserved4[3];
  123. } __aligned(1);
  124. static struct dfc_flow_status_ind_msg_v01 qmap_flow_ind;
  125. static struct dfc_tx_link_status_ind_msg_v01 qmap_tx_ind;
  126. static struct dfc_qmi_data __rcu *qmap_dfc_data;
  127. static bool dfc_config_acked;
  128. static void dfc_qmap_send_config(struct dfc_qmi_data *data);
  129. static void dfc_qmap_send_end_marker_cnf(struct qos_info *qos,
  130. struct rmnet_bearer_map *bearer,
  131. u16 seq, u32 tx_id);
  132. static int dfc_qmap_handle_ind(struct dfc_qmi_data *dfc,
  133. struct sk_buff *skb)
  134. {
  135. struct qmap_dfc_ind *cmd;
  136. if (skb->len < sizeof(struct qmap_dfc_ind))
  137. return QMAP_CMD_INVALID;
  138. cmd = (struct qmap_dfc_ind *)skb->data;
  139. if (cmd->tx_info_valid) {
  140. memset(&qmap_tx_ind, 0, sizeof(qmap_tx_ind));
  141. qmap_tx_ind.tx_status = cmd->tx_info;
  142. qmap_tx_ind.bearer_info_valid = 1;
  143. qmap_tx_ind.bearer_info_len = 1;
  144. qmap_tx_ind.bearer_info[0].mux_id = cmd->hdr.mux_id;
  145. qmap_tx_ind.bearer_info[0].bearer_id = cmd->bearer_id;
  146. dfc_handle_tx_link_status_ind(dfc, &qmap_tx_ind);
  147. /* Ignore grant since it is always 0 */
  148. goto done;
  149. }
  150. memset(&qmap_flow_ind, 0, sizeof(qmap_flow_ind));
  151. qmap_flow_ind.flow_status_valid = 1;
  152. qmap_flow_ind.flow_status_len = 1;
  153. qmap_flow_ind.flow_status[0].mux_id = cmd->hdr.mux_id;
  154. qmap_flow_ind.flow_status[0].bearer_id = cmd->bearer_id;
  155. qmap_flow_ind.flow_status[0].num_bytes = ntohl(cmd->grant);
  156. qmap_flow_ind.flow_status[0].seq_num = ntohs(cmd->seq_num);
  157. qmap_flow_ind.flow_status[0].ll_status = cmd->ll_status;
  158. if (cmd->rx_bytes_valid) {
  159. qmap_flow_ind.flow_status[0].rx_bytes_valid = 1;
  160. qmap_flow_ind.flow_status[0].rx_bytes = ntohl(cmd->rx_bytes);
  161. }
  162. if (cmd->tcp_bidir) {
  163. qmap_flow_ind.ancillary_info_valid = 1;
  164. qmap_flow_ind.ancillary_info_len = 1;
  165. qmap_flow_ind.ancillary_info[0].mux_id = cmd->hdr.mux_id;
  166. qmap_flow_ind.ancillary_info[0].bearer_id = cmd->bearer_id;
  167. qmap_flow_ind.ancillary_info[0].reserved = DFC_MASK_TCP_BIDIR;
  168. }
  169. dfc_do_burst_flow_control(dfc, &qmap_flow_ind, false);
  170. done:
  171. return QMAP_CMD_ACK;
  172. }
  173. static int dfc_qmap_handle_query_resp(struct dfc_qmi_data *dfc,
  174. struct sk_buff *skb)
  175. {
  176. struct qmap_dfc_query_resp *cmd;
  177. if (skb->len < sizeof(struct qmap_dfc_query_resp))
  178. return QMAP_CMD_DONE;
  179. cmd = (struct qmap_dfc_query_resp *)skb->data;
  180. if (cmd->invalid)
  181. return QMAP_CMD_DONE;
  182. memset(&qmap_flow_ind, 0, sizeof(qmap_flow_ind));
  183. qmap_flow_ind.flow_status_valid = 1;
  184. qmap_flow_ind.flow_status_len = 1;
  185. qmap_flow_ind.flow_status[0].mux_id = cmd->hdr.mux_id;
  186. qmap_flow_ind.flow_status[0].bearer_id = cmd->bearer_id;
  187. qmap_flow_ind.flow_status[0].num_bytes = ntohl(cmd->grant);
  188. qmap_flow_ind.flow_status[0].seq_num = 0xFFFF;
  189. if (cmd->rx_bytes_valid) {
  190. qmap_flow_ind.flow_status[0].rx_bytes_valid = 1;
  191. qmap_flow_ind.flow_status[0].rx_bytes = ntohl(cmd->rx_bytes);
  192. }
  193. if (cmd->tcp_bidir) {
  194. qmap_flow_ind.ancillary_info_valid = 1;
  195. qmap_flow_ind.ancillary_info_len = 1;
  196. qmap_flow_ind.ancillary_info[0].mux_id = cmd->hdr.mux_id;
  197. qmap_flow_ind.ancillary_info[0].bearer_id = cmd->bearer_id;
  198. qmap_flow_ind.ancillary_info[0].reserved = DFC_MASK_TCP_BIDIR;
  199. }
  200. dfc_do_burst_flow_control(dfc, &qmap_flow_ind, true);
  201. return QMAP_CMD_DONE;
  202. }
  203. static int dfc_qmap_set_end_marker(struct dfc_qmi_data *dfc, u8 mux_id,
  204. u8 bearer_id, u16 seq_num, u32 tx_id)
  205. {
  206. struct net_device *dev;
  207. struct qos_info *qos;
  208. struct rmnet_bearer_map *bearer;
  209. int rc = QMAP_CMD_ACK;
  210. dev = rmnet_get_rmnet_dev(dfc->rmnet_port, mux_id);
  211. if (!dev)
  212. return rc;
  213. qos = (struct qos_info *)rmnet_get_qos_pt(dev);
  214. if (!qos)
  215. return rc;
  216. spin_lock_bh(&qos->qos_lock);
  217. bearer = qmi_rmnet_get_bearer_map(qos, bearer_id);
  218. if (!bearer) {
  219. spin_unlock_bh(&qos->qos_lock);
  220. return rc;
  221. }
  222. if (bearer->last_seq == seq_num && bearer->grant_size) {
  223. bearer->ack_req = 1;
  224. bearer->ack_txid = tx_id;
  225. } else {
  226. dfc_qmap_send_end_marker_cnf(qos, bearer, seq_num, tx_id);
  227. }
  228. spin_unlock_bh(&qos->qos_lock);
  229. return QMAP_CMD_DONE;
  230. }
  231. static int dfc_qmap_handle_end_marker_req(struct dfc_qmi_data *dfc,
  232. struct sk_buff *skb)
  233. {
  234. struct qmap_dfc_end_marker_req *cmd;
  235. if (skb->len < sizeof(struct qmap_dfc_end_marker_req))
  236. return QMAP_CMD_INVALID;
  237. cmd = (struct qmap_dfc_end_marker_req *)skb->data;
  238. return dfc_qmap_set_end_marker(dfc, cmd->hdr.mux_id, cmd->bearer_id,
  239. ntohs(cmd->seq_num),
  240. ntohl(cmd->hdr.tx_id));
  241. }
  242. int dfc_qmap_cmd_handler(struct sk_buff *skb)
  243. {
  244. struct qmap_cmd_hdr *cmd;
  245. struct dfc_qmi_data *dfc;
  246. int rc = QMAP_CMD_DONE;
  247. cmd = (struct qmap_cmd_hdr *)skb->data;
  248. if (cmd->cmd_name == QMAP_DFC_QUERY) {
  249. if (cmd->cmd_type != QMAP_CMD_ACK)
  250. return rc;
  251. } else if (cmd->cmd_type != QMAP_CMD_REQUEST) {
  252. if (cmd->cmd_type == QMAP_CMD_ACK &&
  253. cmd->cmd_name == QMAP_DFC_CONFIG)
  254. dfc_config_acked = true;
  255. return rc;
  256. }
  257. dfc = rcu_dereference(qmap_dfc_data);
  258. if (!dfc || READ_ONCE(dfc->restart_state))
  259. return rc;
  260. /* Re-send DFC config once if needed */
  261. if (unlikely(!dfc_config_acked)) {
  262. dfc_qmap_send_config(dfc);
  263. dfc_config_acked = true;
  264. }
  265. switch (cmd->cmd_name) {
  266. case QMAP_DFC_IND:
  267. rc = dfc_qmap_handle_ind(dfc, skb);
  268. qmi_rmnet_set_dl_msg_active(dfc->rmnet_port);
  269. break;
  270. case QMAP_DFC_QUERY:
  271. rc = dfc_qmap_handle_query_resp(dfc, skb);
  272. break;
  273. case QMAP_DFC_END_MARKER:
  274. rc = dfc_qmap_handle_end_marker_req(dfc, skb);
  275. break;
  276. default:
  277. if (cmd->cmd_type == QMAP_CMD_REQUEST)
  278. rc = QMAP_CMD_UNSUPPORTED;
  279. }
  280. return rc;
  281. }
  282. static void dfc_qmap_send_config(struct dfc_qmi_data *data)
  283. {
  284. struct sk_buff *skb;
  285. struct qmap_dfc_config *dfc_config;
  286. unsigned int len = sizeof(struct qmap_dfc_config);
  287. skb = alloc_skb(len, GFP_ATOMIC);
  288. if (!skb)
  289. return;
  290. skb->protocol = htons(ETH_P_MAP);
  291. dfc_config = (struct qmap_dfc_config *)skb_put(skb, len);
  292. memset(dfc_config, 0, len);
  293. dfc_config->hdr.cd_bit = 1;
  294. dfc_config->hdr.mux_id = 0;
  295. dfc_config->hdr.pkt_len = htons(len - QMAP_HDR_LEN);
  296. dfc_config->hdr.cmd_name = QMAP_DFC_CONFIG;
  297. dfc_config->hdr.cmd_type = QMAP_CMD_REQUEST;
  298. dfc_config->hdr.tx_id = htonl(rmnet_qmap_next_txid());
  299. dfc_config->cmd_ver = QMAP_DFC_VER;
  300. dfc_config->cmd_id = QMAP_DFC_IND;
  301. dfc_config->tx_info = 1;
  302. dfc_config->ep_type = htonl(data->svc.ep_type);
  303. dfc_config->iface_id = htonl(data->svc.iface_id);
  304. rmnet_qmap_send(skb, RMNET_CH_CTL, false);
  305. }
  306. static void dfc_qmap_send_query(u8 mux_id, u8 bearer_id)
  307. {
  308. struct sk_buff *skb;
  309. struct qmap_dfc_query *dfc_query;
  310. unsigned int len = sizeof(struct qmap_dfc_query);
  311. skb = alloc_skb(len, GFP_ATOMIC);
  312. if (!skb)
  313. return;
  314. skb->protocol = htons(ETH_P_MAP);
  315. dfc_query = (struct qmap_dfc_query *)skb_put(skb, len);
  316. memset(dfc_query, 0, len);
  317. dfc_query->hdr.cd_bit = 1;
  318. dfc_query->hdr.mux_id = mux_id;
  319. dfc_query->hdr.pkt_len = htons(len - QMAP_HDR_LEN);
  320. dfc_query->hdr.cmd_name = QMAP_DFC_QUERY;
  321. dfc_query->hdr.cmd_type = QMAP_CMD_REQUEST;
  322. dfc_query->hdr.tx_id = htonl(rmnet_qmap_next_txid());
  323. dfc_query->cmd_ver = QMAP_DFC_VER;
  324. dfc_query->bearer_id = bearer_id;
  325. rmnet_qmap_send(skb, RMNET_CH_CTL, false);
  326. }
  327. static void dfc_qmap_send_end_marker_cnf(struct qos_info *qos,
  328. struct rmnet_bearer_map *bearer,
  329. u16 seq, u32 tx_id)
  330. {
  331. struct sk_buff *skb;
  332. struct qmapv5_dfc_end_marker_cnf *em_cnf;
  333. unsigned int len = sizeof(struct qmapv5_dfc_end_marker_cnf);
  334. skb = alloc_skb(len, GFP_ATOMIC);
  335. if (!skb)
  336. return;
  337. em_cnf = (struct qmapv5_dfc_end_marker_cnf *)skb_put(skb, len);
  338. memset(em_cnf, 0, len);
  339. em_cnf->hdr.cd_bit = 1;
  340. em_cnf->hdr.next_hdr = 1;
  341. em_cnf->hdr.mux_id = qos->mux_id;
  342. em_cnf->hdr.pkt_len = htons(len -
  343. (QMAP_HDR_LEN +
  344. sizeof(struct rmnet_map_v5_csum_header)));
  345. em_cnf->hdr.csum_hdr.header_type = RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD;
  346. em_cnf->hdr.cmd_name = QMAP_DFC_END_MARKER;
  347. em_cnf->hdr.cmd_type = QMAP_CMD_ACK;
  348. em_cnf->hdr.tx_id = htonl(tx_id);
  349. em_cnf->cmd_ver = QMAP_DFC_VER;
  350. em_cnf->bearer_id = bearer->bearer_id;
  351. em_cnf->seq_num = htons(seq);
  352. rmnet_qmap_send(skb, bearer->ch_switch.current_ch, true);
  353. }
  354. static int dfc_qmap_send_powersave(u8 enable, u8 num_bearers, u8 *bearer_id)
  355. {
  356. struct sk_buff *skb;
  357. struct qmap_dfc_powersave_req *dfc_powersave;
  358. unsigned int len = sizeof(struct qmap_dfc_powersave_req);
  359. struct dfc_qmi_data *dfc;
  360. u32 ep_type = 0;
  361. u32 iface_id = 0;
  362. rcu_read_lock();
  363. dfc = rcu_dereference(qmap_dfc_data);
  364. if (dfc) {
  365. ep_type = dfc->svc.ep_type;
  366. iface_id = dfc->svc.iface_id;
  367. } else {
  368. rcu_read_unlock();
  369. return -EINVAL;
  370. }
  371. rcu_read_unlock();
  372. skb = alloc_skb(len, GFP_ATOMIC);
  373. if (!skb)
  374. return -ENOMEM;
  375. skb->protocol = htons(ETH_P_MAP);
  376. dfc_powersave = (struct qmap_dfc_powersave_req *)skb_put(skb, len);
  377. memset(dfc_powersave, 0, len);
  378. dfc_powersave->hdr.cd_bit = 1;
  379. dfc_powersave->hdr.mux_id = 0;
  380. dfc_powersave->hdr.pkt_len = htons(len - QMAP_HDR_LEN);
  381. dfc_powersave->hdr.cmd_name = QMAP_DFC_POWERSAVE;
  382. dfc_powersave->hdr.cmd_type = QMAP_CMD_REQUEST;
  383. dfc_powersave->hdr.tx_id = htonl(rmnet_qmap_next_txid());
  384. dfc_powersave->cmd_ver = 3;
  385. dfc_powersave->mode = enable ? 1 : 0;
  386. if (enable && num_bearers) {
  387. if (unlikely(num_bearers > PS_MAX_BEARERS))
  388. num_bearers = PS_MAX_BEARERS;
  389. dfc_powersave->allow = 1;
  390. dfc_powersave->autoshut = 1;
  391. dfc_powersave->num_bearers = num_bearers;
  392. memcpy(dfc_powersave->bearer_id, bearer_id, num_bearers);
  393. }
  394. dfc_powersave->ep_type = htonl(ep_type);
  395. dfc_powersave->iface_id = htonl(iface_id);
  396. return rmnet_qmap_send(skb, RMNET_CH_CTL, false);
  397. }
  398. int dfc_qmap_set_powersave(u8 enable, u8 num_bearers, u8 *bearer_id)
  399. {
  400. trace_dfc_set_powersave_mode(enable);
  401. return dfc_qmap_send_powersave(enable, num_bearers, bearer_id);
  402. }
  403. void dfc_qmap_send_ack(struct qos_info *qos, u8 bearer_id, u16 seq, u8 type)
  404. {
  405. struct rmnet_bearer_map *bearer;
  406. if (type == DFC_ACK_TYPE_DISABLE) {
  407. bearer = qmi_rmnet_get_bearer_map(qos, bearer_id);
  408. if (bearer)
  409. dfc_qmap_send_end_marker_cnf(qos, bearer,
  410. seq, bearer->ack_txid);
  411. } else if (type == DFC_ACK_TYPE_THRESHOLD) {
  412. dfc_qmap_send_query(qos->mux_id, bearer_id);
  413. }
  414. }
  415. int dfc_qmap_client_init(void *port, int index, struct svc_info *psvc,
  416. struct qmi_info *qmi)
  417. {
  418. struct dfc_qmi_data *data;
  419. if (!port || !qmi)
  420. return -EINVAL;
  421. /* Prevent double init */
  422. data = rcu_dereference(qmap_dfc_data);
  423. if (data)
  424. return -EINVAL;
  425. data = kzalloc(sizeof(struct dfc_qmi_data), GFP_KERNEL);
  426. if (!data)
  427. return -ENOMEM;
  428. data->rmnet_port = port;
  429. data->index = index;
  430. memcpy(&data->svc, psvc, sizeof(data->svc));
  431. qmi->dfc_clients[index] = (void *)data;
  432. rcu_assign_pointer(qmap_dfc_data, data);
  433. rmnet_qmap_init(port);
  434. trace_dfc_client_state_up(data->index, data->svc.instance,
  435. data->svc.ep_type, data->svc.iface_id);
  436. pr_info("DFC QMAP init\n");
  437. /* Currently if powersave ext is enabled, no need to do dfc config
  438. * which only enables tx_info */
  439. if (qmi->ps_ext) {
  440. dfc_config_acked = true;
  441. } else {
  442. dfc_config_acked = false;
  443. dfc_qmap_send_config(data);
  444. }
  445. return 0;
  446. }
  447. void dfc_qmap_client_exit(void *dfc_data)
  448. {
  449. struct dfc_qmi_data *data = (struct dfc_qmi_data *)dfc_data;
  450. if (!data) {
  451. pr_err("%s() data is null\n", __func__);
  452. return;
  453. }
  454. trace_dfc_client_state_down(data->index, 0);
  455. rmnet_qmap_exit();
  456. WRITE_ONCE(data->restart_state, 1);
  457. RCU_INIT_POINTER(qmap_dfc_data, NULL);
  458. synchronize_rcu();
  459. kfree(data);
  460. pr_info("DFC QMAP exit\n");
  461. }