qmi_rmnet.c 33 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505
  1. /*
  2. * Copyright (c) 2018-2021, The Linux Foundation. All rights reserved.
  3. * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
  4. *
  5. * This program is free software; you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License version 2 and
  7. * only version 2 as published by the Free Software Foundation.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. */
  14. #include <linux/soc/qcom/qmi.h>
  15. #include "qmi_rmnet_i.h"
  16. #include "qmi_rmnet.h"
  17. #include "rmnet_qmi.h"
  18. #include "dfc.h"
  19. #include <linux/rtnetlink.h>
  20. #include <uapi/linux/rtnetlink.h>
  21. #include <net/pkt_sched.h>
  22. #include <net/tcp.h>
  23. #include <linux/module.h>
  24. #include <linux/moduleparam.h>
  25. #include <linux/ip.h>
  26. #include <linux/ipv6.h>
  27. #define NLMSG_FLOW_ACTIVATE 1
  28. #define NLMSG_FLOW_DEACTIVATE 2
  29. #define NLMSG_CLIENT_SETUP 4
  30. #define NLMSG_CLIENT_DELETE 5
  31. #define NLMSG_SCALE_FACTOR 6
  32. #define NLMSG_WQ_FREQUENCY 7
  33. #define NLMSG_CHANNEL_SWITCH 8
  34. #define FLAG_DFC_MASK 0x000F
  35. #define FLAG_POWERSAVE_MASK 0x0010
  36. #define FLAG_QMAP_MASK 0x0020
  37. #define FLAG_PS_EXT_MASK 0x0040
  38. #define FLAG_TO_MODE(f) ((f) & FLAG_DFC_MASK)
  39. #define DFC_SUPPORTED_MODE(m) \
  40. ((m) == DFC_MODE_SA)
  41. #define FLAG_TO_QMAP(f) ((f) & FLAG_QMAP_MASK)
  42. #define FLAG_TO_PS_EXT(f) ((f) & FLAG_PS_EXT_MASK)
  43. int dfc_mode;
  44. int dfc_qmap;
  45. int dfc_ps_ext;
  46. unsigned int rmnet_wq_frequency __read_mostly = 1000;
  47. #define PS_WORK_ACTIVE_BIT 0
  48. #define NO_DELAY (0x0000 * HZ)
  49. #define PS_INTERVAL_MS (rmnet_wq_frequency)
  50. #define PS_INTERVAL_KT (ms_to_ktime(PS_INTERVAL_MS))
  51. #define PS_INTERVAL_JF (msecs_to_jiffies(PS_INTERVAL_MS))
  52. #define WATCHDOG_EXPIRE_JF (msecs_to_jiffies(50))
  53. #ifdef CONFIG_QTI_QMI_DFC
  54. static unsigned int qmi_rmnet_scale_factor = 5;
  55. static LIST_HEAD(qos_cleanup_list);
  56. #endif
  57. static int
  58. qmi_rmnet_del_flow(struct net_device *dev, struct tcmsg *tcm,
  59. struct qmi_info *qmi);
  60. struct qmi_elem_info data_ep_id_type_v01_ei[] = {
  61. {
  62. .data_type = QMI_SIGNED_4_BYTE_ENUM,
  63. .elem_len = 1,
  64. .elem_size = sizeof(enum data_ep_type_enum_v01),
  65. .array_type = NO_ARRAY,
  66. .tlv_type = QMI_COMMON_TLV_TYPE,
  67. .offset = offsetof(struct data_ep_id_type_v01,
  68. ep_type),
  69. .ei_array = NULL,
  70. },
  71. {
  72. .data_type = QMI_UNSIGNED_4_BYTE,
  73. .elem_len = 1,
  74. .elem_size = sizeof(u32),
  75. .array_type = NO_ARRAY,
  76. .tlv_type = QMI_COMMON_TLV_TYPE,
  77. .offset = offsetof(struct data_ep_id_type_v01,
  78. iface_id),
  79. .ei_array = NULL,
  80. },
  81. {
  82. .data_type = QMI_EOTI,
  83. .elem_len = 0,
  84. .elem_size = 0,
  85. .array_type = NO_ARRAY,
  86. .tlv_type = QMI_COMMON_TLV_TYPE,
  87. .offset = 0,
  88. .ei_array = NULL,
  89. },
  90. };
  91. EXPORT_SYMBOL(data_ep_id_type_v01_ei);
  92. void *qmi_rmnet_has_dfc_client(struct qmi_info *qmi)
  93. {
  94. int i;
  95. if (!qmi)
  96. return NULL;
  97. for (i = 0; i < MAX_CLIENT_NUM; i++) {
  98. if (qmi->dfc_clients[i])
  99. return qmi->dfc_clients[i];
  100. }
  101. return NULL;
  102. }
  103. static inline int
  104. qmi_rmnet_has_client(struct qmi_info *qmi)
  105. {
  106. if (qmi->wda_client)
  107. return 1;
  108. return qmi_rmnet_has_dfc_client(qmi) ? 1 : 0;
  109. }
  110. static int
  111. qmi_rmnet_has_pending(struct qmi_info *qmi)
  112. {
  113. int i;
  114. if (qmi->wda_pending)
  115. return 1;
  116. for (i = 0; i < MAX_CLIENT_NUM; i++) {
  117. if (qmi->dfc_pending[i])
  118. return 1;
  119. }
  120. return 0;
  121. }
  122. #ifdef CONFIG_QTI_QMI_DFC
  123. static void
  124. qmi_rmnet_clean_flow_list(struct qos_info *qos)
  125. {
  126. struct rmnet_bearer_map *bearer, *br_tmp;
  127. struct rmnet_flow_map *itm, *fl_tmp;
  128. ASSERT_RTNL();
  129. list_for_each_entry_safe(itm, fl_tmp, &qos->flow_head, list) {
  130. list_del(&itm->list);
  131. kfree(itm);
  132. }
  133. list_for_each_entry_safe(bearer, br_tmp, &qos->bearer_head, list) {
  134. list_del(&bearer->list);
  135. kfree(bearer);
  136. }
  137. memset(qos->mq, 0, sizeof(qos->mq));
  138. }
  139. struct rmnet_flow_map *
  140. qmi_rmnet_get_flow_map(struct qos_info *qos, u32 flow_id, int ip_type)
  141. {
  142. struct rmnet_flow_map *itm;
  143. if (!qos)
  144. return NULL;
  145. list_for_each_entry(itm, &qos->flow_head, list) {
  146. if ((itm->flow_id == flow_id) && (itm->ip_type == ip_type))
  147. return itm;
  148. }
  149. return NULL;
  150. }
  151. struct rmnet_bearer_map *
  152. qmi_rmnet_get_bearer_map(struct qos_info *qos, uint8_t bearer_id)
  153. {
  154. struct rmnet_bearer_map *itm;
  155. if (!qos)
  156. return NULL;
  157. list_for_each_entry(itm, &qos->bearer_head, list) {
  158. if (itm->bearer_id == bearer_id)
  159. return itm;
  160. }
  161. return NULL;
  162. }
  163. static void qmi_rmnet_update_flow_map(struct rmnet_flow_map *itm,
  164. struct rmnet_flow_map *new_map)
  165. {
  166. itm->bearer_id = new_map->bearer_id;
  167. itm->flow_id = new_map->flow_id;
  168. itm->ip_type = new_map->ip_type;
  169. itm->mq_idx = new_map->mq_idx;
  170. }
  171. int qmi_rmnet_flow_control(struct net_device *dev, u32 mq_idx, int enable)
  172. {
  173. struct netdev_queue *q;
  174. if (unlikely(mq_idx >= dev->num_tx_queues))
  175. return 0;
  176. q = netdev_get_tx_queue(dev, mq_idx);
  177. if (unlikely(!q))
  178. return 0;
  179. if (enable)
  180. netif_tx_wake_queue(q);
  181. else
  182. netif_tx_stop_queue(q);
  183. trace_dfc_qmi_tc(dev->name, mq_idx, enable);
  184. return 0;
  185. }
  186. /**
  187. * qmi_rmnet_watchdog_fn - watchdog timer func
  188. */
  189. static void qmi_rmnet_watchdog_fn(struct timer_list *t)
  190. {
  191. struct rmnet_bearer_map *bearer;
  192. bearer = container_of(t, struct rmnet_bearer_map, watchdog);
  193. trace_dfc_watchdog(bearer->qos->mux_id, bearer->bearer_id, 2);
  194. spin_lock_bh(&bearer->qos->qos_lock);
  195. if (bearer->watchdog_quit)
  196. goto done;
  197. /*
  198. * Possible stall, try to recover. Enable 80% query and jumpstart
  199. * the bearer if disabled.
  200. */
  201. bearer->watchdog_expire_cnt++;
  202. bearer->bytes_in_flight = 0;
  203. if (!bearer->grant_size) {
  204. bearer->grant_size = DEFAULT_CALL_GRANT;
  205. bearer->grant_thresh = qmi_rmnet_grant_per(bearer->grant_size);
  206. dfc_bearer_flow_ctl(bearer->qos->vnd_dev, bearer, bearer->qos);
  207. } else {
  208. bearer->grant_thresh = qmi_rmnet_grant_per(bearer->grant_size);
  209. }
  210. done:
  211. bearer->watchdog_started = false;
  212. spin_unlock_bh(&bearer->qos->qos_lock);
  213. }
  214. /**
  215. * qmi_rmnet_watchdog_add - add the bearer to watch
  216. * Needs to be called with qos_lock
  217. */
  218. void qmi_rmnet_watchdog_add(struct rmnet_bearer_map *bearer)
  219. {
  220. bearer->watchdog_quit = false;
  221. if (bearer->watchdog_started)
  222. return;
  223. bearer->watchdog_started = true;
  224. mod_timer(&bearer->watchdog, jiffies + WATCHDOG_EXPIRE_JF);
  225. trace_dfc_watchdog(bearer->qos->mux_id, bearer->bearer_id, 1);
  226. }
  227. /**
  228. * qmi_rmnet_watchdog_remove - remove the bearer from watch
  229. * Needs to be called with qos_lock
  230. */
  231. void qmi_rmnet_watchdog_remove(struct rmnet_bearer_map *bearer)
  232. {
  233. bearer->watchdog_quit = true;
  234. if (!bearer->watchdog_started)
  235. return;
  236. del_timer(&bearer->watchdog);
  237. bearer->watchdog_started = false;
  238. trace_dfc_watchdog(bearer->qos->mux_id, bearer->bearer_id, 0);
  239. }
  240. /**
  241. * qmi_rmnet_bearer_clean - clean the removed bearer
  242. * Needs to be called with rtn_lock but not qos_lock
  243. */
  244. static void qmi_rmnet_bearer_clean(struct qos_info *qos)
  245. {
  246. if (qos->removed_bearer) {
  247. qos->removed_bearer->watchdog_quit = true;
  248. del_timer_sync(&qos->removed_bearer->watchdog);
  249. qos->removed_bearer->ch_switch.timer_quit = true;
  250. del_timer_sync(&qos->removed_bearer->ch_switch.guard_timer);
  251. kfree(qos->removed_bearer);
  252. qos->removed_bearer = NULL;
  253. }
  254. }
  255. static struct rmnet_bearer_map *__qmi_rmnet_bearer_get(
  256. struct qos_info *qos_info, u8 bearer_id)
  257. {
  258. struct rmnet_bearer_map *bearer;
  259. bearer = qmi_rmnet_get_bearer_map(qos_info, bearer_id);
  260. if (bearer) {
  261. bearer->flow_ref++;
  262. } else {
  263. bearer = kzalloc(sizeof(*bearer), GFP_ATOMIC);
  264. if (!bearer)
  265. return NULL;
  266. bearer->bearer_id = bearer_id;
  267. bearer->flow_ref = 1;
  268. bearer->grant_size = DEFAULT_CALL_GRANT;
  269. bearer->grant_thresh = qmi_rmnet_grant_per(bearer->grant_size);
  270. bearer->mq_idx = INVALID_MQ;
  271. bearer->ack_mq_idx = INVALID_MQ;
  272. bearer->qos = qos_info;
  273. timer_setup(&bearer->watchdog, qmi_rmnet_watchdog_fn, 0);
  274. timer_setup(&bearer->ch_switch.guard_timer,
  275. rmnet_ll_guard_fn, 0);
  276. list_add(&bearer->list, &qos_info->bearer_head);
  277. }
  278. return bearer;
  279. }
  280. static void __qmi_rmnet_bearer_put(struct net_device *dev,
  281. struct qos_info *qos_info,
  282. struct rmnet_bearer_map *bearer,
  283. bool reset)
  284. {
  285. struct mq_map *mq;
  286. int i, j;
  287. if (bearer && --bearer->flow_ref == 0) {
  288. for (i = 0; i < MAX_MQ_NUM; i++) {
  289. mq = &qos_info->mq[i];
  290. if (mq->bearer != bearer)
  291. continue;
  292. mq->bearer = NULL;
  293. mq->is_ll_ch = false;
  294. mq->drop_on_remove = reset;
  295. smp_mb();
  296. qmi_rmnet_flow_control(dev, i, 1);
  297. if (dfc_mode == DFC_MODE_SA) {
  298. j = i + ACK_MQ_OFFSET;
  299. qmi_rmnet_flow_control(dev, j, 1);
  300. }
  301. }
  302. /* Remove from bearer map */
  303. list_del(&bearer->list);
  304. qos_info->removed_bearer = bearer;
  305. }
  306. }
  307. static void __qmi_rmnet_update_mq(struct net_device *dev,
  308. struct qos_info *qos_info,
  309. struct rmnet_bearer_map *bearer,
  310. struct rmnet_flow_map *itm)
  311. {
  312. struct mq_map *mq;
  313. /* In SA mode default mq is not associated with any bearer */
  314. if (dfc_mode == DFC_MODE_SA && itm->mq_idx == DEFAULT_MQ_NUM)
  315. return;
  316. mq = &qos_info->mq[itm->mq_idx];
  317. if (!mq->bearer) {
  318. mq->bearer = bearer;
  319. mq->is_ll_ch = bearer->ch_switch.current_ch;
  320. mq->drop_on_remove = false;
  321. smp_mb();
  322. if (dfc_mode == DFC_MODE_SA) {
  323. bearer->mq_idx = itm->mq_idx;
  324. bearer->ack_mq_idx = itm->mq_idx + ACK_MQ_OFFSET;
  325. } else {
  326. bearer->mq_idx = itm->mq_idx;
  327. }
  328. /* Always enable flow for the newly associated bearer */
  329. if (!bearer->grant_size) {
  330. bearer->grant_size = DEFAULT_GRANT;
  331. bearer->grant_thresh =
  332. qmi_rmnet_grant_per(DEFAULT_GRANT);
  333. }
  334. qmi_rmnet_flow_control(dev, itm->mq_idx, 1);
  335. if (dfc_mode == DFC_MODE_SA)
  336. qmi_rmnet_flow_control(dev, bearer->ack_mq_idx, 1);
  337. }
  338. }
  339. static int __qmi_rmnet_rebind_flow(struct net_device *dev,
  340. struct qos_info *qos_info,
  341. struct rmnet_flow_map *itm,
  342. struct rmnet_flow_map *new_map)
  343. {
  344. struct rmnet_bearer_map *bearer;
  345. __qmi_rmnet_bearer_put(dev, qos_info, itm->bearer, false);
  346. bearer = __qmi_rmnet_bearer_get(qos_info, new_map->bearer_id);
  347. if (!bearer)
  348. return -ENOMEM;
  349. qmi_rmnet_update_flow_map(itm, new_map);
  350. itm->bearer = bearer;
  351. __qmi_rmnet_update_mq(dev, qos_info, bearer, itm);
  352. return 0;
  353. }
  354. static int qmi_rmnet_add_flow(struct net_device *dev, struct tcmsg *tcm,
  355. struct qmi_info *qmi)
  356. {
  357. struct qos_info *qos_info = (struct qos_info *)rmnet_get_qos_pt(dev);
  358. struct rmnet_flow_map new_map, *itm;
  359. struct rmnet_bearer_map *bearer;
  360. struct tcmsg tmp_tcm;
  361. int rc = 0;
  362. if (!qos_info || !tcm || tcm->tcm_handle >= MAX_MQ_NUM)
  363. return -EINVAL;
  364. ASSERT_RTNL();
  365. /* flow activate
  366. * tcm->tcm__pad1 - bearer_id, tcm->tcm_parent - flow_id,
  367. * tcm->tcm_ifindex - ip_type, tcm->tcm_handle - mq_idx
  368. */
  369. new_map.bearer_id = tcm->tcm__pad1;
  370. new_map.flow_id = tcm->tcm_parent;
  371. new_map.ip_type = tcm->tcm_ifindex;
  372. new_map.mq_idx = tcm->tcm_handle;
  373. trace_dfc_flow_info(dev->name, new_map.bearer_id, new_map.flow_id,
  374. new_map.ip_type, new_map.mq_idx, 1);
  375. again:
  376. spin_lock_bh(&qos_info->qos_lock);
  377. itm = qmi_rmnet_get_flow_map(qos_info, new_map.flow_id,
  378. new_map.ip_type);
  379. if (itm) {
  380. if (itm->bearer_id != new_map.bearer_id) {
  381. rc = __qmi_rmnet_rebind_flow(
  382. dev, qos_info, itm, &new_map);
  383. goto done;
  384. } else if (itm->mq_idx != new_map.mq_idx) {
  385. tmp_tcm.tcm__pad1 = itm->bearer_id;
  386. tmp_tcm.tcm_parent = itm->flow_id;
  387. tmp_tcm.tcm_ifindex = itm->ip_type;
  388. tmp_tcm.tcm_handle = itm->mq_idx;
  389. spin_unlock_bh(&qos_info->qos_lock);
  390. qmi_rmnet_del_flow(dev, &tmp_tcm, qmi);
  391. goto again;
  392. } else {
  393. goto done;
  394. }
  395. }
  396. /* Create flow map */
  397. itm = kzalloc(sizeof(*itm), GFP_ATOMIC);
  398. if (!itm) {
  399. spin_unlock_bh(&qos_info->qos_lock);
  400. return -ENOMEM;
  401. }
  402. qmi_rmnet_update_flow_map(itm, &new_map);
  403. list_add(&itm->list, &qos_info->flow_head);
  404. /* Create or update bearer map */
  405. bearer = __qmi_rmnet_bearer_get(qos_info, new_map.bearer_id);
  406. if (!bearer) {
  407. rc = -ENOMEM;
  408. goto done;
  409. }
  410. itm->bearer = bearer;
  411. __qmi_rmnet_update_mq(dev, qos_info, bearer, itm);
  412. done:
  413. spin_unlock_bh(&qos_info->qos_lock);
  414. qmi_rmnet_bearer_clean(qos_info);
  415. return rc;
  416. }
  417. static int
  418. qmi_rmnet_del_flow(struct net_device *dev, struct tcmsg *tcm,
  419. struct qmi_info *qmi)
  420. {
  421. struct qos_info *qos_info = (struct qos_info *)rmnet_get_qos_pt(dev);
  422. struct rmnet_flow_map new_map, *itm;
  423. if (!qos_info)
  424. return -EINVAL;
  425. ASSERT_RTNL();
  426. /* flow deactivate
  427. * tcm->tcm__pad1 - bearer_id, tcm->tcm_parent - flow_id,
  428. * tcm->tcm_ifindex - ip_type
  429. */
  430. spin_lock_bh(&qos_info->qos_lock);
  431. new_map.bearer_id = tcm->tcm__pad1;
  432. new_map.flow_id = tcm->tcm_parent;
  433. new_map.ip_type = tcm->tcm_ifindex;
  434. itm = qmi_rmnet_get_flow_map(qos_info, new_map.flow_id,
  435. new_map.ip_type);
  436. if (itm) {
  437. trace_dfc_flow_info(dev->name, new_map.bearer_id,
  438. new_map.flow_id, new_map.ip_type,
  439. itm->mq_idx, 0);
  440. __qmi_rmnet_bearer_put(dev, qos_info, itm->bearer, true);
  441. /* Remove from flow map */
  442. list_del(&itm->list);
  443. kfree(itm);
  444. }
  445. if (list_empty(&qos_info->flow_head))
  446. netif_tx_wake_all_queues(dev);
  447. spin_unlock_bh(&qos_info->qos_lock);
  448. qmi_rmnet_bearer_clean(qos_info);
  449. return 0;
  450. }
  451. static void qmi_rmnet_query_flows(struct qmi_info *qmi)
  452. {
  453. int i;
  454. for (i = 0; i < MAX_CLIENT_NUM; i++) {
  455. if (qmi->dfc_clients[i] && !dfc_qmap &&
  456. !qmi->dfc_client_exiting[i])
  457. dfc_qmi_query_flow(qmi->dfc_clients[i]);
  458. }
  459. }
  460. struct rmnet_bearer_map *qmi_rmnet_get_bearer_noref(struct qos_info *qos_info,
  461. u8 bearer_id)
  462. {
  463. struct rmnet_bearer_map *bearer;
  464. bearer = __qmi_rmnet_bearer_get(qos_info, bearer_id);
  465. if (bearer)
  466. bearer->flow_ref--;
  467. return bearer;
  468. }
  469. #else
  470. static inline void
  471. qmi_rmnet_update_flow_map(struct rmnet_flow_map *itm,
  472. struct rmnet_flow_map *new_map)
  473. {
  474. }
  475. static inline int
  476. qmi_rmnet_add_flow(struct net_device *dev, struct tcmsg *tcm,
  477. struct qmi_info *qmi)
  478. {
  479. return -EINVAL;
  480. }
  481. static inline int
  482. qmi_rmnet_del_flow(struct net_device *dev, struct tcmsg *tcm,
  483. struct qmi_info *qmi)
  484. {
  485. return -EINVAL;
  486. }
  487. static inline void qmi_rmnet_query_flows(struct qmi_info *qmi)
  488. {
  489. }
  490. #endif
  491. static int
  492. qmi_rmnet_setup_client(void *port, struct qmi_info *qmi, struct tcmsg *tcm)
  493. {
  494. int idx, err = 0;
  495. struct svc_info svc;
  496. ASSERT_RTNL();
  497. /* client setup
  498. * tcm->tcm_handle - instance, tcm->tcm_info - ep_type,
  499. * tcm->tcm_parent - iface_id, tcm->tcm_ifindex - flags
  500. */
  501. idx = (tcm->tcm_handle == 0) ? 0 : 1;
  502. if (!qmi) {
  503. qmi = kzalloc(sizeof(struct qmi_info), GFP_ATOMIC);
  504. if (!qmi)
  505. return -ENOMEM;
  506. rmnet_init_qmi_pt(port, qmi);
  507. }
  508. qmi->flag = tcm->tcm_ifindex;
  509. qmi->ps_ext = FLAG_TO_PS_EXT(qmi->flag);
  510. svc.instance = tcm->tcm_handle;
  511. svc.ep_type = tcm->tcm_info;
  512. svc.iface_id = tcm->tcm_parent;
  513. if (DFC_SUPPORTED_MODE(dfc_mode) &&
  514. !qmi->dfc_clients[idx] && !qmi->dfc_pending[idx]) {
  515. if (dfc_qmap)
  516. err = dfc_qmap_client_init(port, idx, &svc, qmi);
  517. else
  518. err = dfc_qmi_client_init(port, idx, &svc, qmi);
  519. qmi->dfc_client_exiting[idx] = false;
  520. }
  521. if ((tcm->tcm_ifindex & FLAG_POWERSAVE_MASK) &&
  522. (idx == 0) && !qmi->wda_client && !qmi->wda_pending) {
  523. err = wda_qmi_client_init(port, &svc, qmi);
  524. }
  525. return err;
  526. }
  527. static int
  528. __qmi_rmnet_delete_client(void *port, struct qmi_info *qmi, int idx)
  529. {
  530. void *data = NULL;
  531. ASSERT_RTNL();
  532. if (qmi->dfc_clients[idx])
  533. data = qmi->dfc_clients[idx];
  534. else if (qmi->dfc_pending[idx])
  535. data = qmi->dfc_pending[idx];
  536. if (data) {
  537. if (dfc_qmap)
  538. dfc_qmap_client_exit(data);
  539. else
  540. dfc_qmi_client_exit(data);
  541. qmi->dfc_clients[idx] = NULL;
  542. qmi->dfc_pending[idx] = NULL;
  543. }
  544. if (!qmi_rmnet_has_client(qmi) && !qmi_rmnet_has_pending(qmi)) {
  545. rmnet_reset_qmi_pt(port);
  546. kfree(qmi);
  547. return 0;
  548. }
  549. return 1;
  550. }
  551. static void
  552. qmi_rmnet_delete_client(void *port, struct qmi_info *qmi, struct tcmsg *tcm)
  553. {
  554. int idx;
  555. void *data = NULL;
  556. /* client delete: tcm->tcm_handle - instance*/
  557. idx = (tcm->tcm_handle == 0) ? 0 : 1;
  558. ASSERT_RTNL();
  559. if (qmi->wda_client)
  560. data = qmi->wda_client;
  561. else if (qmi->wda_pending)
  562. data = qmi->wda_pending;
  563. if ((idx == 0) && data) {
  564. wda_qmi_client_exit(data);
  565. qmi->wda_client = NULL;
  566. qmi->wda_pending = NULL;
  567. } else {
  568. qmi->dfc_client_exiting[idx] = true;
  569. qmi_rmnet_flush_ps_wq();
  570. }
  571. __qmi_rmnet_delete_client(port, qmi, idx);
  572. }
  573. int qmi_rmnet_change_link(struct net_device *dev, void *port, void *tcm_pt,
  574. int attr_len)
  575. {
  576. struct qmi_info *qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  577. struct tcmsg *tcm = (struct tcmsg *)tcm_pt;
  578. void *wda_data = NULL;
  579. int rc = 0;
  580. switch (tcm->tcm_family) {
  581. case NLMSG_FLOW_ACTIVATE:
  582. if (!qmi || !DFC_SUPPORTED_MODE(dfc_mode) ||
  583. !qmi_rmnet_has_dfc_client(qmi))
  584. return rc;
  585. qmi_rmnet_add_flow(dev, tcm, qmi);
  586. break;
  587. case NLMSG_FLOW_DEACTIVATE:
  588. if (!qmi || !DFC_SUPPORTED_MODE(dfc_mode))
  589. return rc;
  590. qmi_rmnet_del_flow(dev, tcm, qmi);
  591. break;
  592. case NLMSG_CLIENT_SETUP:
  593. dfc_mode = FLAG_TO_MODE(tcm->tcm_ifindex);
  594. dfc_qmap = FLAG_TO_QMAP(tcm->tcm_ifindex);
  595. dfc_ps_ext = FLAG_TO_PS_EXT(tcm->tcm_ifindex);
  596. if (!DFC_SUPPORTED_MODE(dfc_mode) &&
  597. !(tcm->tcm_ifindex & FLAG_POWERSAVE_MASK))
  598. return rc;
  599. if (qmi_rmnet_setup_client(port, qmi, tcm) < 0) {
  600. /* retrieve qmi again as it could have been changed */
  601. qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  602. if (qmi &&
  603. !qmi_rmnet_has_client(qmi) &&
  604. !qmi_rmnet_has_pending(qmi)) {
  605. rmnet_reset_qmi_pt(port);
  606. kfree(qmi);
  607. }
  608. return rc;
  609. }
  610. if (tcm->tcm_ifindex & FLAG_POWERSAVE_MASK) {
  611. qmi_rmnet_work_init(port);
  612. rmnet_set_powersave_format(port);
  613. }
  614. rmnet_ll_wq_init();
  615. break;
  616. case NLMSG_CLIENT_DELETE:
  617. if (!qmi)
  618. return rc;
  619. if (tcm->tcm_handle == 0) { /* instance 0 */
  620. rmnet_clear_powersave_format(port);
  621. if (qmi->wda_client)
  622. wda_data = qmi->wda_client;
  623. else if (qmi->wda_pending)
  624. wda_data = qmi->wda_pending;
  625. wda_qmi_client_release(wda_data);
  626. qmi_rmnet_work_exit(port);
  627. }
  628. qmi_rmnet_delete_client(port, qmi, tcm);
  629. rmnet_ll_wq_exit();
  630. break;
  631. case NLMSG_SCALE_FACTOR:
  632. if (!tcm->tcm_ifindex)
  633. return rc;
  634. qmi_rmnet_scale_factor = tcm->tcm_ifindex;
  635. break;
  636. case NLMSG_WQ_FREQUENCY:
  637. if (tcm->tcm_ifindex >= 100)
  638. rmnet_wq_frequency = tcm->tcm_ifindex;
  639. else
  640. rc = -EINVAL;
  641. break;
  642. case NLMSG_CHANNEL_SWITCH:
  643. if (!qmi || !DFC_SUPPORTED_MODE(dfc_mode) ||
  644. !qmi_rmnet_has_dfc_client(qmi))
  645. return rc;
  646. rc = rmnet_ll_switch(dev, tcm, attr_len);
  647. break;
  648. default:
  649. pr_debug("%s(): No handler\n", __func__);
  650. break;
  651. }
  652. return rc;
  653. }
  654. EXPORT_SYMBOL(qmi_rmnet_change_link);
  655. void qmi_rmnet_qmi_exit(void *qmi_pt, void *port)
  656. {
  657. struct qmi_info *qmi = (struct qmi_info *)qmi_pt;
  658. int i;
  659. void *data = NULL;
  660. if (!qmi)
  661. return;
  662. ASSERT_RTNL();
  663. if (qmi->wda_client)
  664. data = qmi->wda_client;
  665. else if (qmi->wda_pending)
  666. data = qmi->wda_pending;
  667. wda_qmi_client_release(data);
  668. qmi_rmnet_work_exit(port);
  669. rmnet_ll_wq_exit();
  670. if (data) {
  671. wda_qmi_client_exit(data);
  672. qmi->wda_client = NULL;
  673. qmi->wda_pending = NULL;
  674. }
  675. for (i = 0; i < MAX_CLIENT_NUM; i++) {
  676. if (!__qmi_rmnet_delete_client(port, qmi, i))
  677. return;
  678. }
  679. }
  680. EXPORT_SYMBOL(qmi_rmnet_qmi_exit);
  681. void qmi_rmnet_enable_all_flows(struct net_device *dev)
  682. {
  683. struct qos_info *qos;
  684. struct rmnet_bearer_map *bearer;
  685. bool do_wake;
  686. qos = (struct qos_info *)rmnet_get_qos_pt(dev);
  687. if (!qos)
  688. return;
  689. spin_lock_bh(&qos->qos_lock);
  690. list_for_each_entry(bearer, &qos->bearer_head, list) {
  691. bearer->seq = 0;
  692. bearer->ack_req = 0;
  693. bearer->bytes_in_flight = 0;
  694. bearer->tcp_bidir = false;
  695. bearer->rat_switch = false;
  696. qmi_rmnet_watchdog_remove(bearer);
  697. if (bearer->tx_off)
  698. continue;
  699. do_wake = !bearer->grant_size;
  700. bearer->grant_size = DEFAULT_GRANT;
  701. bearer->grant_thresh = qmi_rmnet_grant_per(DEFAULT_GRANT);
  702. if (do_wake)
  703. dfc_bearer_flow_ctl(dev, bearer, qos);
  704. }
  705. spin_unlock_bh(&qos->qos_lock);
  706. }
  707. EXPORT_SYMBOL(qmi_rmnet_enable_all_flows);
  708. bool qmi_rmnet_all_flows_enabled(struct net_device *dev)
  709. {
  710. struct qos_info *qos;
  711. struct rmnet_bearer_map *bearer;
  712. bool ret = true;
  713. qos = (struct qos_info *)rmnet_get_qos_pt(dev);
  714. if (!qos)
  715. return true;
  716. spin_lock_bh(&qos->qos_lock);
  717. list_for_each_entry(bearer, &qos->bearer_head, list) {
  718. if (!bearer->grant_size) {
  719. ret = false;
  720. break;
  721. }
  722. }
  723. spin_unlock_bh(&qos->qos_lock);
  724. return ret;
  725. }
  726. EXPORT_SYMBOL(qmi_rmnet_all_flows_enabled);
  727. /**
  728. * rmnet_prepare_ps_bearers - get disabled bearers and
  729. * reset enabled bearers
  730. */
  731. void qmi_rmnet_prepare_ps_bearers(struct net_device *dev, u8 *num_bearers,
  732. u8 *bearer_id)
  733. {
  734. struct qos_info *qos;
  735. struct rmnet_bearer_map *bearer;
  736. u8 current_num_bearers = 0;
  737. u8 num_bearers_left = 0;
  738. qos = (struct qos_info *)rmnet_get_qos_pt(dev);
  739. if (!qos || !num_bearers)
  740. return;
  741. spin_lock_bh(&qos->qos_lock);
  742. num_bearers_left = *num_bearers;
  743. list_for_each_entry(bearer, &qos->bearer_head, list) {
  744. if (bearer->grant_size) {
  745. bearer->seq = 0;
  746. bearer->ack_req = 0;
  747. bearer->bytes_in_flight = 0;
  748. bearer->tcp_bidir = false;
  749. bearer->rat_switch = false;
  750. qmi_rmnet_watchdog_remove(bearer);
  751. bearer->grant_size = DEFAULT_GRANT;
  752. bearer->grant_thresh =
  753. qmi_rmnet_grant_per(DEFAULT_GRANT);
  754. } else if (num_bearers_left) {
  755. if (bearer_id)
  756. bearer_id[current_num_bearers] =
  757. bearer->bearer_id;
  758. current_num_bearers++;
  759. num_bearers_left--;
  760. } else {
  761. pr_err("DFC: no bearer space\n");
  762. }
  763. }
  764. *num_bearers = current_num_bearers;
  765. spin_unlock_bh(&qos->qos_lock);
  766. }
  767. EXPORT_SYMBOL(qmi_rmnet_prepare_ps_bearers);
  768. #ifdef CONFIG_QTI_QMI_DFC
  769. bool qmi_rmnet_get_flow_state(struct net_device *dev, struct sk_buff *skb,
  770. bool *drop, bool *is_low_latency)
  771. {
  772. struct qos_info *qos = rmnet_get_qos_pt(dev);
  773. int txq = skb->queue_mapping;
  774. if (txq > ACK_MQ_OFFSET)
  775. txq -= ACK_MQ_OFFSET;
  776. if (unlikely(!qos || txq >= MAX_MQ_NUM))
  777. return false;
  778. /* If the bearer is gone, packets may need to be dropped */
  779. *drop = (txq != DEFAULT_MQ_NUM && !READ_ONCE(qos->mq[txq].bearer) &&
  780. READ_ONCE(qos->mq[txq].drop_on_remove));
  781. *is_low_latency = READ_ONCE(qos->mq[txq].is_ll_ch);
  782. return true;
  783. }
  784. EXPORT_SYMBOL(qmi_rmnet_get_flow_state);
  785. void qmi_rmnet_burst_fc_check(struct net_device *dev,
  786. int ip_type, u32 mark, unsigned int len)
  787. {
  788. struct qos_info *qos = rmnet_get_qos_pt(dev);
  789. if (!qos)
  790. return;
  791. dfc_qmi_burst_check(dev, qos, ip_type, mark, len);
  792. }
  793. EXPORT_SYMBOL(qmi_rmnet_burst_fc_check);
  794. static bool _qmi_rmnet_is_tcp_ack(struct sk_buff *skb)
  795. {
  796. struct tcphdr *th;
  797. int ip_hdr_len;
  798. int ip_payload_len;
  799. if (skb->protocol == htons(ETH_P_IP) &&
  800. ip_hdr(skb)->protocol == IPPROTO_TCP) {
  801. ip_hdr_len = ip_hdr(skb)->ihl << 2;
  802. ip_payload_len = ntohs(ip_hdr(skb)->tot_len) - ip_hdr_len;
  803. } else if (skb->protocol == htons(ETH_P_IPV6) &&
  804. ipv6_hdr(skb)->nexthdr == IPPROTO_TCP) {
  805. ip_hdr_len = sizeof(struct ipv6hdr);
  806. ip_payload_len = ntohs(ipv6_hdr(skb)->payload_len);
  807. } else {
  808. return false;
  809. }
  810. th = (struct tcphdr *)(skb->data + ip_hdr_len);
  811. /* no longer looking for ACK flag */
  812. if (ip_payload_len == th->doff << 2)
  813. return true;
  814. return false;
  815. }
  816. static inline bool qmi_rmnet_is_tcp_ack(struct sk_buff *skb)
  817. {
  818. /* Locally generated TCP acks */
  819. if (skb_is_tcp_pure_ack(skb))
  820. return true;
  821. /* Forwarded */
  822. if (unlikely(_qmi_rmnet_is_tcp_ack(skb)))
  823. return true;
  824. return false;
  825. }
  826. static int qmi_rmnet_get_queue_sa(struct qos_info *qos, struct sk_buff *skb)
  827. {
  828. struct rmnet_flow_map *itm;
  829. int ip_type;
  830. int txq = DEFAULT_MQ_NUM;
  831. /* Put NDP in default mq */
  832. if (skb->protocol == htons(ETH_P_IPV6) &&
  833. ipv6_hdr(skb)->nexthdr == IPPROTO_ICMPV6 &&
  834. icmp6_hdr(skb)->icmp6_type >= 133 &&
  835. icmp6_hdr(skb)->icmp6_type <= 137) {
  836. return DEFAULT_MQ_NUM;
  837. }
  838. ip_type = (skb->protocol == htons(ETH_P_IPV6)) ? AF_INET6 : AF_INET;
  839. spin_lock_bh(&qos->qos_lock);
  840. itm = qmi_rmnet_get_flow_map(qos, skb->mark, ip_type);
  841. if (unlikely(!itm))
  842. goto done;
  843. /* Put the packet in the assigned mq except TCP ack */
  844. if (likely(itm->bearer) && qmi_rmnet_is_tcp_ack(skb))
  845. txq = itm->bearer->ack_mq_idx;
  846. else
  847. txq = itm->mq_idx;
  848. done:
  849. spin_unlock_bh(&qos->qos_lock);
  850. return txq;
  851. }
  852. int qmi_rmnet_get_queue(struct net_device *dev, struct sk_buff *skb)
  853. {
  854. struct qos_info *qos = rmnet_get_qos_pt(dev);
  855. int txq = 0, ip_type = AF_INET;
  856. struct rmnet_flow_map *itm;
  857. u32 mark = skb->mark;
  858. if (!qos)
  859. return 0;
  860. if (likely(dfc_mode == DFC_MODE_SA))
  861. return qmi_rmnet_get_queue_sa(qos, skb);
  862. ip_type = (skb->protocol == htons(ETH_P_IPV6)) ? AF_INET6 : AF_INET;
  863. spin_lock_bh(&qos->qos_lock);
  864. itm = qmi_rmnet_get_flow_map(qos, mark, ip_type);
  865. if (itm)
  866. txq = itm->mq_idx;
  867. spin_unlock_bh(&qos->qos_lock);
  868. return txq;
  869. }
  870. EXPORT_SYMBOL(qmi_rmnet_get_queue);
  871. inline unsigned int qmi_rmnet_grant_per(unsigned int grant)
  872. {
  873. return grant / qmi_rmnet_scale_factor;
  874. }
  875. EXPORT_SYMBOL(qmi_rmnet_grant_per);
  876. void *qmi_rmnet_qos_init(struct net_device *real_dev,
  877. struct net_device *vnd_dev, u8 mux_id)
  878. {
  879. struct qos_info *qos;
  880. qos = kzalloc(sizeof(*qos), GFP_KERNEL);
  881. if (!qos)
  882. return NULL;
  883. qos->mux_id = mux_id;
  884. qos->real_dev = real_dev;
  885. qos->vnd_dev = vnd_dev;
  886. qos->tran_num = 0;
  887. INIT_LIST_HEAD(&qos->flow_head);
  888. INIT_LIST_HEAD(&qos->bearer_head);
  889. spin_lock_init(&qos->qos_lock);
  890. return qos;
  891. }
  892. EXPORT_SYMBOL(qmi_rmnet_qos_init);
  893. void qmi_rmnet_qos_exit_pre(void *qos)
  894. {
  895. struct qos_info *qosi = (struct qos_info *)qos;
  896. struct rmnet_bearer_map *bearer;
  897. if (!qos)
  898. return;
  899. list_for_each_entry(bearer, &qosi->bearer_head, list) {
  900. bearer->watchdog_quit = true;
  901. del_timer_sync(&bearer->watchdog);
  902. bearer->ch_switch.timer_quit = true;
  903. del_timer_sync(&bearer->ch_switch.guard_timer);
  904. }
  905. list_add(&qosi->list, &qos_cleanup_list);
  906. }
  907. EXPORT_SYMBOL(qmi_rmnet_qos_exit_pre);
  908. void qmi_rmnet_qos_exit_post(void)
  909. {
  910. struct qos_info *qos, *tmp;
  911. synchronize_rcu();
  912. list_for_each_entry_safe(qos, tmp, &qos_cleanup_list, list) {
  913. list_del(&qos->list);
  914. qmi_rmnet_clean_flow_list(qos);
  915. kfree(qos);
  916. }
  917. }
  918. EXPORT_SYMBOL(qmi_rmnet_qos_exit_post);
  919. #endif
  920. #ifdef CONFIG_QTI_QMI_POWER_COLLAPSE
  921. static struct workqueue_struct *rmnet_ps_wq;
  922. static struct rmnet_powersave_work *rmnet_work;
  923. static bool rmnet_work_quit;
  924. static bool rmnet_work_inited;
  925. static LIST_HEAD(ps_list);
  926. static u8 ps_bearer_id[32];
  927. struct rmnet_powersave_work {
  928. struct delayed_work work;
  929. void *port;
  930. u64 old_rx_pkts;
  931. u64 old_tx_pkts;
  932. };
  933. void qmi_rmnet_ps_on_notify(void *port)
  934. {
  935. struct qmi_rmnet_ps_ind *tmp;
  936. list_for_each_entry_rcu(tmp, &ps_list, list)
  937. tmp->ps_on_handler(port);
  938. }
  939. EXPORT_SYMBOL(qmi_rmnet_ps_on_notify);
  940. void qmi_rmnet_ps_off_notify(void *port)
  941. {
  942. struct qmi_rmnet_ps_ind *tmp;
  943. list_for_each_entry_rcu(tmp, &ps_list, list)
  944. tmp->ps_off_handler(port);
  945. }
  946. EXPORT_SYMBOL(qmi_rmnet_ps_off_notify);
  947. int qmi_rmnet_ps_ind_register(void *port,
  948. struct qmi_rmnet_ps_ind *ps_ind)
  949. {
  950. if (!port || !ps_ind || !ps_ind->ps_on_handler ||
  951. !ps_ind->ps_off_handler)
  952. return -EINVAL;
  953. list_add_rcu(&ps_ind->list, &ps_list);
  954. return 0;
  955. }
  956. EXPORT_SYMBOL(qmi_rmnet_ps_ind_register);
  957. int qmi_rmnet_ps_ind_deregister(void *port,
  958. struct qmi_rmnet_ps_ind *ps_ind)
  959. {
  960. struct qmi_rmnet_ps_ind *tmp;
  961. if (!port || !ps_ind)
  962. return -EINVAL;
  963. list_for_each_entry_rcu(tmp, &ps_list, list) {
  964. if (tmp == ps_ind) {
  965. list_del_rcu(&ps_ind->list);
  966. goto done;
  967. }
  968. }
  969. done:
  970. return 0;
  971. }
  972. EXPORT_SYMBOL(qmi_rmnet_ps_ind_deregister);
  973. int qmi_rmnet_set_powersave_mode(void *port, uint8_t enable, u8 num_bearers,
  974. u8 *bearer_id)
  975. {
  976. int rc = -EINVAL;
  977. struct qmi_info *qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  978. if (!qmi || !qmi->wda_client)
  979. return rc;
  980. rc = wda_set_powersave_mode(qmi->wda_client, enable, num_bearers,
  981. bearer_id);
  982. if (rc < 0) {
  983. pr_err("%s() failed set powersave mode[%u], err=%d\n",
  984. __func__, enable, rc);
  985. return rc;
  986. }
  987. return 0;
  988. }
  989. EXPORT_SYMBOL(qmi_rmnet_set_powersave_mode);
  990. static void qmi_rmnet_work_restart(void *port)
  991. {
  992. rcu_read_lock();
  993. if (!rmnet_work_quit)
  994. queue_delayed_work(rmnet_ps_wq, &rmnet_work->work, NO_DELAY);
  995. rcu_read_unlock();
  996. }
  997. static void qmi_rmnet_check_stats(struct work_struct *work)
  998. {
  999. struct rmnet_powersave_work *real_work;
  1000. struct qmi_info *qmi;
  1001. u64 rxd, txd;
  1002. u64 rx, tx;
  1003. bool dl_msg_active;
  1004. real_work = container_of(to_delayed_work(work),
  1005. struct rmnet_powersave_work, work);
  1006. if (unlikely(!real_work || !real_work->port))
  1007. return;
  1008. qmi = (struct qmi_info *)rmnet_get_qmi_pt(real_work->port);
  1009. if (unlikely(!qmi))
  1010. return;
  1011. rmnet_get_packets(real_work->port, &rx, &tx);
  1012. rxd = rx - real_work->old_rx_pkts;
  1013. txd = tx - real_work->old_tx_pkts;
  1014. real_work->old_rx_pkts = rx;
  1015. real_work->old_tx_pkts = tx;
  1016. dl_msg_active = qmi->dl_msg_active;
  1017. qmi->dl_msg_active = false;
  1018. if (qmi->ps_enabled) {
  1019. /* Ready to accept grant */
  1020. qmi->ps_ignore_grant = false;
  1021. /* Register to get QMI DFC and DL marker */
  1022. if (qmi_rmnet_set_powersave_mode(real_work->port, 0,
  1023. 0, NULL) < 0)
  1024. goto end;
  1025. qmi->ps_enabled = false;
  1026. /* Do a query when coming out of powersave */
  1027. qmi_rmnet_query_flows(qmi);
  1028. if (rmnet_get_powersave_notif(real_work->port))
  1029. qmi_rmnet_ps_off_notify(real_work->port);
  1030. goto end;
  1031. }
  1032. if (!rxd && !txd) {
  1033. /* If no DL msg received and there is a flow disabled,
  1034. * (likely in RLF), no need to enter powersave
  1035. */
  1036. if (!dl_msg_active &&
  1037. !rmnet_all_flows_enabled(real_work->port))
  1038. goto end;
  1039. /* Deregister to suppress QMI DFC and DL marker */
  1040. if (qmi_rmnet_set_powersave_mode(real_work->port, 1,
  1041. 0, NULL) < 0)
  1042. goto end;
  1043. qmi->ps_enabled = true;
  1044. /* Ignore grant after going into powersave */
  1045. qmi->ps_ignore_grant = true;
  1046. /* Clear the bit before enabling flow so pending packets
  1047. * can trigger the work again
  1048. */
  1049. clear_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active);
  1050. rmnet_enable_all_flows(real_work->port);
  1051. if (rmnet_get_powersave_notif(real_work->port))
  1052. qmi_rmnet_ps_on_notify(real_work->port);
  1053. return;
  1054. }
  1055. end:
  1056. rcu_read_lock();
  1057. if (!rmnet_work_quit)
  1058. queue_delayed_work(rmnet_ps_wq, &real_work->work,
  1059. PS_INTERVAL_JF);
  1060. rcu_read_unlock();
  1061. }
  1062. static void qmi_rmnet_check_stats_2(struct work_struct *work)
  1063. {
  1064. struct rmnet_powersave_work *real_work;
  1065. struct qmi_info *qmi;
  1066. u64 rxd, txd;
  1067. u64 rx, tx;
  1068. u8 num_bearers;
  1069. int rc;
  1070. real_work = container_of(to_delayed_work(work),
  1071. struct rmnet_powersave_work, work);
  1072. if (unlikely(!real_work->port))
  1073. return;
  1074. qmi = (struct qmi_info *)rmnet_get_qmi_pt(real_work->port);
  1075. if (unlikely(!qmi))
  1076. return;
  1077. rmnet_get_packets(real_work->port, &rx, &tx);
  1078. rxd = rx - real_work->old_rx_pkts;
  1079. txd = tx - real_work->old_tx_pkts;
  1080. real_work->old_rx_pkts = rx;
  1081. real_work->old_tx_pkts = tx;
  1082. if (qmi->ps_enabled) {
  1083. /* Ready to accept grant */
  1084. qmi->ps_ignore_grant = false;
  1085. /* Out of powersave */
  1086. if (dfc_qmap)
  1087. rc = dfc_qmap_set_powersave(0, 0, NULL);
  1088. else
  1089. rc = qmi_rmnet_set_powersave_mode(real_work->port, 0,
  1090. 0, NULL);
  1091. if (rc)
  1092. goto end;
  1093. qmi->ps_enabled = false;
  1094. if (rmnet_get_powersave_notif(real_work->port))
  1095. qmi_rmnet_ps_off_notify(real_work->port);
  1096. goto end;
  1097. }
  1098. if (!rxd && !txd) {
  1099. qmi->ps_ignore_grant = true;
  1100. qmi->ps_enabled = true;
  1101. clear_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active);
  1102. smp_mb();
  1103. num_bearers = sizeof(ps_bearer_id);
  1104. memset(ps_bearer_id, 0, sizeof(ps_bearer_id));
  1105. rmnet_prepare_ps_bearers(real_work->port, &num_bearers,
  1106. ps_bearer_id);
  1107. /* Enter powersave */
  1108. if (dfc_qmap)
  1109. dfc_qmap_set_powersave(1, num_bearers, ps_bearer_id);
  1110. else
  1111. qmi_rmnet_set_powersave_mode(real_work->port, 1,
  1112. num_bearers, ps_bearer_id);
  1113. if (rmnet_get_powersave_notif(real_work->port))
  1114. qmi_rmnet_ps_on_notify(real_work->port);
  1115. return;
  1116. }
  1117. end:
  1118. rcu_read_lock();
  1119. if (!rmnet_work_quit)
  1120. queue_delayed_work(rmnet_ps_wq, &real_work->work,
  1121. PS_INTERVAL_JF);
  1122. rcu_read_unlock();
  1123. }
  1124. static void qmi_rmnet_work_set_active(void *port, int status)
  1125. {
  1126. struct qmi_info *qmi;
  1127. qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  1128. if (unlikely(!qmi))
  1129. return;
  1130. if (status)
  1131. set_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active);
  1132. else
  1133. clear_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active);
  1134. }
  1135. void qmi_rmnet_work_init(void *port)
  1136. {
  1137. if (rmnet_ps_wq)
  1138. return;
  1139. rmnet_ps_wq = alloc_workqueue("rmnet_powersave_work",
  1140. WQ_CPU_INTENSIVE, 1);
  1141. if (!rmnet_ps_wq)
  1142. return;
  1143. rmnet_work = kzalloc(sizeof(*rmnet_work), GFP_ATOMIC);
  1144. if (!rmnet_work) {
  1145. destroy_workqueue(rmnet_ps_wq);
  1146. rmnet_ps_wq = NULL;
  1147. return;
  1148. }
  1149. if (dfc_ps_ext)
  1150. INIT_DELAYED_WORK(&rmnet_work->work,
  1151. qmi_rmnet_check_stats_2);
  1152. else
  1153. INIT_DELAYED_WORK(&rmnet_work->work, qmi_rmnet_check_stats);
  1154. rmnet_work->port = port;
  1155. rmnet_get_packets(rmnet_work->port, &rmnet_work->old_rx_pkts,
  1156. &rmnet_work->old_tx_pkts);
  1157. rmnet_work_quit = false;
  1158. qmi_rmnet_work_set_active(rmnet_work->port, 0);
  1159. rmnet_work_inited = true;
  1160. }
  1161. EXPORT_SYMBOL(qmi_rmnet_work_init);
  1162. void qmi_rmnet_work_maybe_restart(void *port)
  1163. {
  1164. struct qmi_info *qmi;
  1165. qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  1166. if (unlikely(!qmi || !rmnet_work_inited))
  1167. return;
  1168. if (!test_and_set_bit(PS_WORK_ACTIVE_BIT, &qmi->ps_work_active)) {
  1169. qmi->ps_ignore_grant = false;
  1170. qmi_rmnet_work_restart(port);
  1171. }
  1172. }
  1173. EXPORT_SYMBOL(qmi_rmnet_work_maybe_restart);
  1174. void qmi_rmnet_work_exit(void *port)
  1175. {
  1176. if (!rmnet_ps_wq || !rmnet_work)
  1177. return;
  1178. rmnet_work_quit = true;
  1179. synchronize_rcu();
  1180. rmnet_work_inited = false;
  1181. cancel_delayed_work_sync(&rmnet_work->work);
  1182. destroy_workqueue(rmnet_ps_wq);
  1183. qmi_rmnet_work_set_active(port, 0);
  1184. rmnet_ps_wq = NULL;
  1185. kfree(rmnet_work);
  1186. rmnet_work = NULL;
  1187. }
  1188. EXPORT_SYMBOL(qmi_rmnet_work_exit);
  1189. void qmi_rmnet_set_dl_msg_active(void *port)
  1190. {
  1191. struct qmi_info *qmi;
  1192. qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  1193. if (unlikely(!qmi))
  1194. return;
  1195. qmi->dl_msg_active = true;
  1196. }
  1197. EXPORT_SYMBOL(qmi_rmnet_set_dl_msg_active);
  1198. void qmi_rmnet_flush_ps_wq(void)
  1199. {
  1200. if (rmnet_ps_wq)
  1201. flush_workqueue(rmnet_ps_wq);
  1202. }
  1203. bool qmi_rmnet_ignore_grant(void *port)
  1204. {
  1205. struct qmi_info *qmi;
  1206. qmi = (struct qmi_info *)rmnet_get_qmi_pt(port);
  1207. if (unlikely(!qmi))
  1208. return false;
  1209. return qmi->ps_ignore_grant;
  1210. }
  1211. EXPORT_SYMBOL(qmi_rmnet_ignore_grant);
  1212. #endif