u_rmnet_ctrl_smd.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808
  1. /*
  2. * Copyright (c) 2011-2013, The Linux Foundation. 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. #include <linux/kernel.h>
  14. #include <linux/interrupt.h>
  15. #include <linux/device.h>
  16. #include <linux/delay.h>
  17. #include <linux/slab.h>
  18. #include <linux/termios.h>
  19. #include <mach/msm_smd.h>
  20. #include <linux/debugfs.h>
  21. #include <linux/bitops.h>
  22. #include <linux/termios.h>
  23. #include "u_rmnet.h"
  24. #define MAX_CTRL_PER_CLIENT 3
  25. #define MAX_CTRL_PORT (MAX_CTRL_PER_CLIENT * NR_CTRL_CLIENTS)
  26. static char *ctrl_names[NR_CTRL_CLIENTS][MAX_CTRL_PER_CLIENT] = {
  27. {"DATA40_CNTL", "DATA39_CNTL", "DATA38_CNTL"},
  28. {"DATA39_CNTL"},
  29. };
  30. static struct workqueue_struct *grmnet_ctrl_wq;
  31. u8 online_clients;
  32. #define SMD_CH_MAX_LEN 20
  33. #define CH_OPENED 0
  34. #define CH_READY 1
  35. #define CH_PREPARE_READY 2
  36. struct smd_ch_info {
  37. struct smd_channel *ch;
  38. char *name;
  39. unsigned long flags;
  40. wait_queue_head_t wait;
  41. wait_queue_head_t smd_wait_q;
  42. unsigned dtr;
  43. struct list_head tx_q;
  44. unsigned long tx_len;
  45. struct work_struct read_w;
  46. struct work_struct write_w;
  47. struct rmnet_ctrl_port *port;
  48. int cbits_tomodem;
  49. /* stats */
  50. unsigned long to_modem;
  51. unsigned long to_host;
  52. };
  53. struct rmnet_ctrl_port {
  54. struct smd_ch_info ctrl_ch;
  55. unsigned int port_num;
  56. struct grmnet *port_usb;
  57. spinlock_t port_lock;
  58. struct delayed_work connect_w;
  59. struct delayed_work disconnect_w;
  60. };
  61. static struct rmnet_ctrl_ports {
  62. struct rmnet_ctrl_port *port;
  63. struct platform_driver pdrv;
  64. } ctrl_smd_ports[MAX_CTRL_PORT];
  65. /*---------------misc functions---------------- */
  66. static struct rmnet_ctrl_pkt *alloc_rmnet_ctrl_pkt(unsigned len, gfp_t flags)
  67. {
  68. struct rmnet_ctrl_pkt *pkt;
  69. pkt = kzalloc(sizeof(struct rmnet_ctrl_pkt), flags);
  70. if (!pkt)
  71. return ERR_PTR(-ENOMEM);
  72. pkt->buf = kmalloc(len, flags);
  73. if (!pkt->buf) {
  74. kfree(pkt);
  75. return ERR_PTR(-ENOMEM);
  76. }
  77. pkt->len = len;
  78. return pkt;
  79. }
  80. static void free_rmnet_ctrl_pkt(struct rmnet_ctrl_pkt *pkt)
  81. {
  82. kfree(pkt->buf);
  83. kfree(pkt);
  84. }
  85. /*--------------------------------------------- */
  86. /*---------------control/smd channel functions---------------- */
  87. static void grmnet_ctrl_smd_read_w(struct work_struct *w)
  88. {
  89. struct smd_ch_info *c = container_of(w, struct smd_ch_info, read_w);
  90. struct rmnet_ctrl_port *port = c->port;
  91. int sz, total_received, read_avail;
  92. int len;
  93. void *buf;
  94. unsigned long flags;
  95. spin_lock_irqsave(&port->port_lock, flags);
  96. while (c->ch) {
  97. sz = smd_cur_packet_size(c->ch);
  98. if (sz <= 0)
  99. break;
  100. spin_unlock_irqrestore(&port->port_lock, flags);
  101. buf = kmalloc(sz, GFP_KERNEL);
  102. if (!buf)
  103. return;
  104. total_received = 0;
  105. while (total_received < sz) {
  106. wait_event(c->smd_wait_q,
  107. ((read_avail = smd_read_avail(c->ch)) ||
  108. (c->ch == 0)));
  109. if (read_avail < 0 || c->ch == 0) {
  110. pr_err("%s:smd read_avail failure:%d or channel closed ch=%pK",
  111. __func__, read_avail, c->ch);
  112. kfree(buf);
  113. return;
  114. }
  115. if (read_avail + total_received > sz) {
  116. pr_err("%s: SMD sending incorrect pkt\n",
  117. __func__);
  118. kfree(buf);
  119. return;
  120. }
  121. len = smd_read(c->ch, buf + total_received, read_avail);
  122. if (len <= 0) {
  123. pr_err("%s: smd read failure %d\n",
  124. __func__, len);
  125. kfree(buf);
  126. return;
  127. }
  128. total_received += len;
  129. }
  130. /* send it to USB here */
  131. spin_lock_irqsave(&port->port_lock, flags);
  132. if (port->port_usb && port->port_usb->send_cpkt_response) {
  133. port->port_usb->send_cpkt_response(port->port_usb,
  134. buf, sz);
  135. c->to_host++;
  136. }
  137. kfree(buf);
  138. }
  139. spin_unlock_irqrestore(&port->port_lock, flags);
  140. }
  141. static void grmnet_ctrl_smd_write_w(struct work_struct *w)
  142. {
  143. struct smd_ch_info *c = container_of(w, struct smd_ch_info, write_w);
  144. struct rmnet_ctrl_port *port = c->port;
  145. unsigned long flags;
  146. struct rmnet_ctrl_pkt *cpkt;
  147. int ret;
  148. spin_lock_irqsave(&port->port_lock, flags);
  149. while (c->ch) {
  150. if (list_empty(&c->tx_q))
  151. break;
  152. cpkt = list_first_entry(&c->tx_q, struct rmnet_ctrl_pkt, list);
  153. if (smd_write_avail(c->ch) < cpkt->len)
  154. break;
  155. list_del(&cpkt->list);
  156. spin_unlock_irqrestore(&port->port_lock, flags);
  157. ret = smd_write(c->ch, cpkt->buf, cpkt->len);
  158. spin_lock_irqsave(&port->port_lock, flags);
  159. if (ret != cpkt->len) {
  160. pr_err("%s: smd_write failed err:%d\n", __func__, ret);
  161. free_rmnet_ctrl_pkt(cpkt);
  162. break;
  163. }
  164. free_rmnet_ctrl_pkt(cpkt);
  165. c->to_modem++;
  166. }
  167. spin_unlock_irqrestore(&port->port_lock, flags);
  168. }
  169. static int is_legal_port_num(u8 portno)
  170. {
  171. if (portno >= MAX_CTRL_PORT)
  172. return false;
  173. if (ctrl_smd_ports[portno].port == NULL)
  174. return false;
  175. return true;
  176. }
  177. static int
  178. grmnet_ctrl_smd_send_cpkt_tomodem(u8 portno,
  179. void *buf, size_t len)
  180. {
  181. unsigned long flags;
  182. struct rmnet_ctrl_port *port;
  183. struct smd_ch_info *c;
  184. struct rmnet_ctrl_pkt *cpkt;
  185. if (!is_legal_port_num(portno)) {
  186. pr_err("%s: Invalid portno#%d\n", __func__, portno);
  187. return -ENODEV;
  188. }
  189. port = ctrl_smd_ports[portno].port;
  190. cpkt = alloc_rmnet_ctrl_pkt(len, GFP_ATOMIC);
  191. if (IS_ERR(cpkt)) {
  192. pr_err("%s: Unable to allocate ctrl pkt\n", __func__);
  193. return -ENOMEM;
  194. }
  195. memcpy(cpkt->buf, buf, len);
  196. cpkt->len = len;
  197. spin_lock_irqsave(&port->port_lock, flags);
  198. c = &port->ctrl_ch;
  199. /* drop cpkt if ch is not open */
  200. if (!test_bit(CH_OPENED, &c->flags)) {
  201. free_rmnet_ctrl_pkt(cpkt);
  202. spin_unlock_irqrestore(&port->port_lock, flags);
  203. return 0;
  204. }
  205. list_add_tail(&cpkt->list, &c->tx_q);
  206. queue_work(grmnet_ctrl_wq, &c->write_w);
  207. spin_unlock_irqrestore(&port->port_lock, flags);
  208. return 0;
  209. }
  210. #define RMNET_CTRL_DTR 0x01
  211. static void
  212. gsmd_ctrl_send_cbits_tomodem(void *gptr, u8 portno, int cbits)
  213. {
  214. struct rmnet_ctrl_port *port;
  215. struct smd_ch_info *c;
  216. int set_bits = 0;
  217. int clear_bits = 0;
  218. int temp = 0;
  219. if (!is_legal_port_num(portno)) {
  220. pr_err("%s: Invalid portno#%d\n", __func__, portno);
  221. return;
  222. }
  223. if (!gptr) {
  224. pr_err("%s: grmnet is null\n", __func__);
  225. return;
  226. }
  227. port = ctrl_smd_ports[portno].port;
  228. cbits = cbits & RMNET_CTRL_DTR;
  229. c = &port->ctrl_ch;
  230. /* host driver will only send DTR, but to have generic
  231. * set and clear bit implementation using two separate
  232. * checks
  233. */
  234. if (cbits & RMNET_CTRL_DTR)
  235. set_bits |= TIOCM_DTR;
  236. else
  237. clear_bits |= TIOCM_DTR;
  238. temp |= set_bits;
  239. temp &= ~clear_bits;
  240. if (temp == c->cbits_tomodem)
  241. return;
  242. c->cbits_tomodem = temp;
  243. if (!test_bit(CH_OPENED, &c->flags))
  244. return;
  245. pr_debug("%s: ctrl_tomodem:%d ctrl_bits:%d setbits:%d clearbits:%d\n",
  246. __func__, temp, cbits, set_bits, clear_bits);
  247. smd_tiocmset(c->ch, set_bits, clear_bits);
  248. }
  249. static char *get_smd_event(unsigned event)
  250. {
  251. switch (event) {
  252. case SMD_EVENT_DATA:
  253. return "DATA";
  254. case SMD_EVENT_OPEN:
  255. return "OPEN";
  256. case SMD_EVENT_CLOSE:
  257. return "CLOSE";
  258. }
  259. return "UNDEFINED";
  260. }
  261. static void grmnet_ctrl_smd_notify(void *p, unsigned event)
  262. {
  263. struct rmnet_ctrl_port *port = p;
  264. struct smd_ch_info *c = &port->ctrl_ch;
  265. struct rmnet_ctrl_pkt *cpkt;
  266. unsigned long flags;
  267. pr_debug("%s: EVENT_(%s)\n", __func__, get_smd_event(event));
  268. switch (event) {
  269. case SMD_EVENT_DATA:
  270. if (smd_read_avail(c->ch) && !waitqueue_active(&c->smd_wait_q))
  271. queue_work(grmnet_ctrl_wq, &c->read_w);
  272. if (smd_write_avail(c->ch))
  273. queue_work(grmnet_ctrl_wq, &c->write_w);
  274. break;
  275. case SMD_EVENT_OPEN:
  276. set_bit(CH_OPENED, &c->flags);
  277. if (port && port->port_usb && port->port_usb->connect)
  278. port->port_usb->connect(port->port_usb);
  279. break;
  280. case SMD_EVENT_CLOSE:
  281. clear_bit(CH_OPENED, &c->flags);
  282. if (port && port->port_usb && port->port_usb->disconnect)
  283. port->port_usb->disconnect(port->port_usb);
  284. spin_lock_irqsave(&port->port_lock, flags);
  285. while (!list_empty(&c->tx_q)) {
  286. cpkt = list_first_entry(&c->tx_q,
  287. struct rmnet_ctrl_pkt, list);
  288. list_del(&cpkt->list);
  289. free_rmnet_ctrl_pkt(cpkt);
  290. }
  291. spin_unlock_irqrestore(&port->port_lock, flags);
  292. break;
  293. }
  294. wake_up(&c->smd_wait_q);
  295. }
  296. /*------------------------------------------------------------ */
  297. static void grmnet_ctrl_smd_connect_w(struct work_struct *w)
  298. {
  299. struct rmnet_ctrl_port *port =
  300. container_of(w, struct rmnet_ctrl_port, connect_w.work);
  301. struct rmnet_ctrl_ports *port_entry = &ctrl_smd_ports[port->port_num];
  302. struct smd_ch_info *c = &port->ctrl_ch;
  303. unsigned long flags;
  304. int set_bits = 0;
  305. int clear_bits = 0;
  306. int ret;
  307. pr_debug("%s:\n", __func__);
  308. if (!test_bit(CH_READY, &c->flags)) {
  309. if (!test_bit(CH_PREPARE_READY, &c->flags)) {
  310. set_bit(CH_PREPARE_READY, &c->flags);
  311. platform_driver_register(&(port_entry->pdrv));
  312. }
  313. return;
  314. }
  315. ret = smd_open(c->name, &c->ch, port, grmnet_ctrl_smd_notify);
  316. if (ret) {
  317. if (ret == -EAGAIN) {
  318. /* port not ready - retry */
  319. pr_debug("%s: SMD port not ready - rescheduling:%s err:%d\n",
  320. __func__, c->name, ret);
  321. queue_delayed_work(grmnet_ctrl_wq, &port->connect_w,
  322. msecs_to_jiffies(250));
  323. } else {
  324. pr_err("%s: unable to open smd port:%s err:%d\n",
  325. __func__, c->name, ret);
  326. }
  327. return;
  328. }
  329. set_bits = c->cbits_tomodem;
  330. clear_bits = ~(c->cbits_tomodem | TIOCM_RTS);
  331. spin_lock_irqsave(&port->port_lock, flags);
  332. if (port->port_usb)
  333. smd_tiocmset(c->ch, set_bits, clear_bits);
  334. spin_unlock_irqrestore(&port->port_lock, flags);
  335. }
  336. int gsmd_ctrl_connect(struct grmnet *gr, int port_num)
  337. {
  338. struct rmnet_ctrl_port *port;
  339. struct smd_ch_info *c;
  340. unsigned long flags;
  341. pr_debug("%s: grmnet:%pK port#%d\n", __func__, gr, port_num);
  342. if (!is_legal_port_num(port_num)) {
  343. pr_err("%s: Invalid port_num#%d\n", __func__, port_num);
  344. return -ENODEV;
  345. }
  346. if (!gr) {
  347. pr_err("%s: grmnet port is null\n", __func__);
  348. return -ENODEV;
  349. }
  350. port = ctrl_smd_ports[port_num].port;
  351. c = &port->ctrl_ch;
  352. spin_lock_irqsave(&port->port_lock, flags);
  353. port->port_usb = gr;
  354. gr->send_encap_cmd = grmnet_ctrl_smd_send_cpkt_tomodem;
  355. gr->notify_modem = gsmd_ctrl_send_cbits_tomodem;
  356. spin_unlock_irqrestore(&port->port_lock, flags);
  357. queue_delayed_work(grmnet_ctrl_wq, &port->connect_w, 0);
  358. return 0;
  359. }
  360. static void grmnet_ctrl_smd_disconnect_w(struct work_struct *w)
  361. {
  362. struct rmnet_ctrl_port *port =
  363. container_of(w, struct rmnet_ctrl_port,
  364. disconnect_w.work);
  365. struct smd_ch_info *c;
  366. struct platform_driver *pdrv;
  367. c = &port->ctrl_ch;
  368. if (c->ch) {
  369. smd_close(c->ch);
  370. c->ch = NULL;
  371. }
  372. if (test_bit(CH_READY, &c->flags) ||
  373. test_bit(CH_PREPARE_READY, &c->flags)) {
  374. clear_bit(CH_PREPARE_READY, &c->flags);
  375. pdrv = &ctrl_smd_ports[port->port_num].pdrv;
  376. platform_driver_unregister(pdrv);
  377. }
  378. }
  379. void gsmd_ctrl_disconnect(struct grmnet *gr, u8 port_num)
  380. {
  381. struct rmnet_ctrl_port *port;
  382. unsigned long flags;
  383. struct smd_ch_info *c;
  384. struct rmnet_ctrl_pkt *cpkt;
  385. int clear_bits;
  386. pr_debug("%s: grmnet:%pK port#%d\n", __func__, gr, port_num);
  387. if (!is_legal_port_num(port_num)) {
  388. pr_err("%s: Invalid port_num#%d\n", __func__, port_num);
  389. return;
  390. }
  391. if (!gr) {
  392. pr_err("%s: grmnet port is null\n", __func__);
  393. return;
  394. }
  395. port = ctrl_smd_ports[port_num].port;
  396. c = &port->ctrl_ch;
  397. spin_lock_irqsave(&port->port_lock, flags);
  398. port->port_usb = 0;
  399. gr->send_encap_cmd = 0;
  400. gr->notify_modem = 0;
  401. c->cbits_tomodem = 0;
  402. while (!list_empty(&c->tx_q)) {
  403. cpkt = list_first_entry(&c->tx_q, struct rmnet_ctrl_pkt, list);
  404. list_del(&cpkt->list);
  405. free_rmnet_ctrl_pkt(cpkt);
  406. }
  407. spin_unlock_irqrestore(&port->port_lock, flags);
  408. if (test_and_clear_bit(CH_OPENED, &c->flags)) {
  409. clear_bits = ~(c->cbits_tomodem | TIOCM_RTS);
  410. /* send dtr zero */
  411. smd_tiocmset(c->ch, c->cbits_tomodem, clear_bits);
  412. }
  413. queue_delayed_work(grmnet_ctrl_wq, &port->disconnect_w, 0);
  414. }
  415. #define SMD_CH_MAX_LEN 20
  416. static int grmnet_ctrl_smd_ch_probe(struct platform_device *pdev)
  417. {
  418. struct rmnet_ctrl_port *port;
  419. struct smd_ch_info *c;
  420. int i;
  421. unsigned long flags;
  422. pr_debug("%s: name:%s\n", __func__, pdev->name);
  423. for (i = 0; i < MAX_CTRL_PORT; i++) {
  424. if (!ctrl_smd_ports[i].port)
  425. continue;
  426. port = ctrl_smd_ports[i].port;
  427. c = &port->ctrl_ch;
  428. if (!strncmp(c->name, pdev->name, SMD_CH_MAX_LEN)) {
  429. clear_bit(CH_PREPARE_READY, &c->flags);
  430. set_bit(CH_READY, &c->flags);
  431. /* if usb is online, try opening smd_ch */
  432. spin_lock_irqsave(&port->port_lock, flags);
  433. if (port->port_usb)
  434. queue_delayed_work(grmnet_ctrl_wq,
  435. &port->connect_w, 0);
  436. spin_unlock_irqrestore(&port->port_lock, flags);
  437. break;
  438. }
  439. }
  440. return 0;
  441. }
  442. static int grmnet_ctrl_smd_ch_remove(struct platform_device *pdev)
  443. {
  444. struct rmnet_ctrl_port *port;
  445. struct smd_ch_info *c;
  446. int i;
  447. pr_debug("%s: name:%s\n", __func__, pdev->name);
  448. for (i = 0; i < MAX_CTRL_PORT; i++) {
  449. if (!ctrl_smd_ports[i].port)
  450. continue;
  451. port = ctrl_smd_ports[i].port;
  452. c = &port->ctrl_ch;
  453. if (!strncmp(c->name, pdev->name, SMD_CH_MAX_LEN)) {
  454. clear_bit(CH_READY, &c->flags);
  455. clear_bit(CH_OPENED, &c->flags);
  456. if (c->ch) {
  457. smd_close(c->ch);
  458. c->ch = NULL;
  459. }
  460. break;
  461. }
  462. }
  463. return 0;
  464. }
  465. static void grmnet_ctrl_smd_port_free(int portno)
  466. {
  467. struct rmnet_ctrl_port *port = ctrl_smd_ports[portno].port;
  468. struct platform_driver *pdrv = &ctrl_smd_ports[portno].pdrv;
  469. if (port) {
  470. kfree(port);
  471. platform_driver_unregister(pdrv);
  472. }
  473. }
  474. static int grmnet_ctrl_smd_port_alloc(int portno)
  475. {
  476. struct rmnet_ctrl_port *port;
  477. struct smd_ch_info *c;
  478. struct platform_driver *pdrv;
  479. port = kzalloc(sizeof(struct rmnet_ctrl_port), GFP_KERNEL);
  480. if (!port)
  481. return -ENOMEM;
  482. port->port_num = portno;
  483. spin_lock_init(&port->port_lock);
  484. INIT_DELAYED_WORK(&port->connect_w, grmnet_ctrl_smd_connect_w);
  485. INIT_DELAYED_WORK(&port->disconnect_w, grmnet_ctrl_smd_disconnect_w);
  486. c = &port->ctrl_ch;
  487. c->name = ctrl_names[portno / MAX_CTRL_PER_CLIENT]
  488. [portno % MAX_CTRL_PER_CLIENT];
  489. c->port = port;
  490. init_waitqueue_head(&c->wait);
  491. init_waitqueue_head(&c->smd_wait_q);
  492. INIT_LIST_HEAD(&c->tx_q);
  493. INIT_WORK(&c->read_w, grmnet_ctrl_smd_read_w);
  494. INIT_WORK(&c->write_w, grmnet_ctrl_smd_write_w);
  495. ctrl_smd_ports[portno].port = port;
  496. pdrv = &ctrl_smd_ports[portno].pdrv;
  497. pdrv->probe = grmnet_ctrl_smd_ch_probe;
  498. pdrv->remove = grmnet_ctrl_smd_ch_remove;
  499. pdrv->driver.name = c->name;
  500. pdrv->driver.owner = THIS_MODULE;
  501. pr_debug("%s: port:%pK portno:%d\n", __func__, port, portno);
  502. return 0;
  503. }
  504. int gsmd_ctrl_setup(enum ctrl_client client_num, unsigned int count,
  505. u8 *first_port_idx)
  506. {
  507. int i, start_port, allocated_ports;
  508. int ret;
  509. pr_debug("%s: requested ports:%d\n", __func__, count);
  510. if (client_num >= NR_CTRL_CLIENTS) {
  511. pr_err("%s: Invalid client:%d\n", __func__, client_num);
  512. return -EINVAL;
  513. }
  514. if (!count || count > MAX_CTRL_PER_CLIENT) {
  515. pr_err("%s: Invalid num of ports count:%d\n",
  516. __func__, count);
  517. return -EINVAL;
  518. }
  519. if (!online_clients) {
  520. grmnet_ctrl_wq = alloc_workqueue("gsmd_ctrl",
  521. WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
  522. if (!grmnet_ctrl_wq) {
  523. pr_err("%s: Unable to create workqueue grmnet_ctrl\n",
  524. __func__);
  525. return -ENOMEM;
  526. }
  527. }
  528. online_clients++;
  529. start_port = MAX_CTRL_PER_CLIENT * client_num;
  530. allocated_ports = 0;
  531. for (i = start_port; i < count + start_port; i++) {
  532. allocated_ports++;
  533. ret = grmnet_ctrl_smd_port_alloc(i);
  534. if (ret) {
  535. pr_err("%s: Unable to alloc port:%d\n", __func__, i);
  536. allocated_ports--;
  537. goto free_ctrl_smd_ports;
  538. }
  539. }
  540. if (first_port_idx)
  541. *first_port_idx = start_port;
  542. return 0;
  543. free_ctrl_smd_ports:
  544. for (i = 0; i < allocated_ports; i++)
  545. grmnet_ctrl_smd_port_free(start_port + i);
  546. online_clients--;
  547. if (!online_clients)
  548. destroy_workqueue(grmnet_ctrl_wq);
  549. return ret;
  550. }
  551. #if defined(CONFIG_DEBUG_FS)
  552. #define DEBUG_BUF_SIZE 1024
  553. static ssize_t gsmd_ctrl_read_stats(struct file *file, char __user *ubuf,
  554. size_t count, loff_t *ppos)
  555. {
  556. struct rmnet_ctrl_port *port;
  557. struct smd_ch_info *c;
  558. char *buf;
  559. unsigned long flags;
  560. int ret;
  561. int i;
  562. int temp = 0;
  563. buf = kzalloc(sizeof(char) * DEBUG_BUF_SIZE, GFP_KERNEL);
  564. if (!buf)
  565. return -ENOMEM;
  566. for (i = 0; i < MAX_CTRL_PORT; i++) {
  567. if (!ctrl_smd_ports[i].port)
  568. continue;
  569. port = ctrl_smd_ports[i].port;
  570. spin_lock_irqsave(&port->port_lock, flags);
  571. c = &port->ctrl_ch;
  572. temp += scnprintf(buf + temp, DEBUG_BUF_SIZE - temp,
  573. "#PORT:%d port:%pK ctrl_ch:%pK#\n"
  574. "to_usbhost: %lu\n"
  575. "to_modem: %lu\n"
  576. "DTR: %s\n"
  577. "ch_open: %d\n"
  578. "ch_ready: %d\n"
  579. "read_avail: %d\n"
  580. "write_avail:%d\n",
  581. i, port, &port->ctrl_ch,
  582. c->to_host, c->to_modem,
  583. c->cbits_tomodem ? "HIGH" : "LOW",
  584. test_bit(CH_OPENED, &c->flags),
  585. test_bit(CH_READY, &c->flags),
  586. c->ch ? smd_read_avail(c->ch) : 0,
  587. c->ch ? smd_write_avail(c->ch) : 0);
  588. spin_unlock_irqrestore(&port->port_lock, flags);
  589. }
  590. ret = simple_read_from_buffer(ubuf, count, ppos, buf, temp);
  591. kfree(buf);
  592. return ret;
  593. }
  594. static ssize_t gsmd_ctrl_reset_stats(struct file *file, const char __user *buf,
  595. size_t count, loff_t *ppos)
  596. {
  597. struct rmnet_ctrl_port *port;
  598. struct smd_ch_info *c;
  599. int i;
  600. unsigned long flags;
  601. for (i = 0; i < MAX_CTRL_PORT; i++) {
  602. if (!ctrl_smd_ports[i].port)
  603. continue;
  604. port = ctrl_smd_ports[i].port;
  605. spin_lock_irqsave(&port->port_lock, flags);
  606. c = &port->ctrl_ch;
  607. c->to_host = 0;
  608. c->to_modem = 0;
  609. spin_unlock_irqrestore(&port->port_lock, flags);
  610. }
  611. return count;
  612. }
  613. const struct file_operations gsmd_ctrl_stats_ops = {
  614. .read = gsmd_ctrl_read_stats,
  615. .write = gsmd_ctrl_reset_stats,
  616. };
  617. struct dentry *smd_ctrl_dent;
  618. struct dentry *smd_ctrl_dfile;
  619. static void gsmd_ctrl_debugfs_init(void)
  620. {
  621. smd_ctrl_dent = debugfs_create_dir("usb_rmnet_ctrl_smd", 0);
  622. if (IS_ERR(smd_ctrl_dent))
  623. return;
  624. smd_ctrl_dfile = debugfs_create_file("status", 0444, smd_ctrl_dent, 0,
  625. &gsmd_ctrl_stats_ops);
  626. if (!smd_ctrl_dfile || IS_ERR(smd_ctrl_dfile))
  627. debugfs_remove(smd_ctrl_dent);
  628. }
  629. static void gsmd_ctrl_debugfs_exit(void)
  630. {
  631. debugfs_remove(smd_ctrl_dfile);
  632. debugfs_remove(smd_ctrl_dent);
  633. }
  634. #else
  635. static void gsmd_ctrl_debugfs_init(void) { }
  636. static void gsmd_ctrl_debugfs_exit(void) { }
  637. #endif
  638. static int __init gsmd_ctrl_init(void)
  639. {
  640. gsmd_ctrl_debugfs_init();
  641. online_clients = 0;
  642. return 0;
  643. }
  644. module_init(gsmd_ctrl_init);
  645. static void __exit gsmd_ctrl_exit(void)
  646. {
  647. gsmd_ctrl_debugfs_exit();
  648. }
  649. module_exit(gsmd_ctrl_exit);
  650. MODULE_DESCRIPTION("smd control driver");
  651. MODULE_LICENSE("GPL v2");