ns.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816
  1. // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
  2. /*
  3. * Copyright (c) 2015, Sony Mobile Communications Inc.
  4. * Copyright (c) 2013, The Linux Foundation. All rights reserved.
  5. * Copyright (c) 2020, Linaro Ltd.
  6. * Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved.
  7. */
  8. #define pr_fmt(fmt) "qrtr: %s(): " fmt, __func__
  9. #include <linux/ipc_logging.h>
  10. #include <linux/module.h>
  11. #include <linux/qrtr.h>
  12. #include <linux/workqueue.h>
  13. #include <linux/xarray.h>
  14. #include <net/sock.h>
  15. #include "qrtr.h"
  16. #define CREATE_TRACE_POINTS
  17. #include <trace/events/qrtr.h>
  18. #define NS_LOG_PAGE_CNT 4
  19. static void *ns_ilc;
  20. #define NS_INFO(x, ...) ipc_log_string(ns_ilc, x, ##__VA_ARGS__)
  21. static DEFINE_XARRAY(nodes);
  22. static struct {
  23. struct socket *sock;
  24. struct sockaddr_qrtr bcast_sq;
  25. struct list_head lookups;
  26. struct kthread_worker kworker;
  27. struct kthread_work work;
  28. struct task_struct *task;
  29. int local_node;
  30. } qrtr_ns;
  31. static const char * const qrtr_ctrl_pkt_strings[] = {
  32. [QRTR_TYPE_HELLO] = "hello",
  33. [QRTR_TYPE_BYE] = "bye",
  34. [QRTR_TYPE_NEW_SERVER] = "new-server",
  35. [QRTR_TYPE_DEL_SERVER] = "del-server",
  36. [QRTR_TYPE_DEL_CLIENT] = "del-client",
  37. [QRTR_TYPE_RESUME_TX] = "resume-tx",
  38. [QRTR_TYPE_EXIT] = "exit",
  39. [QRTR_TYPE_PING] = "ping",
  40. [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
  41. [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
  42. };
  43. struct qrtr_server_filter {
  44. unsigned int service;
  45. unsigned int instance;
  46. unsigned int ifilter;
  47. };
  48. struct qrtr_lookup {
  49. unsigned int service;
  50. unsigned int instance;
  51. struct sockaddr_qrtr sq;
  52. struct list_head li;
  53. };
  54. struct qrtr_server {
  55. unsigned int service;
  56. unsigned int instance;
  57. unsigned int node;
  58. unsigned int port;
  59. struct list_head qli;
  60. };
  61. struct qrtr_node {
  62. unsigned int id;
  63. struct xarray servers;
  64. };
  65. static struct qrtr_node *node_get(unsigned int node_id)
  66. {
  67. struct qrtr_node *node;
  68. node = xa_load(&nodes, node_id);
  69. if (node)
  70. return node;
  71. /* If node didn't exist, allocate and insert it to the tree */
  72. node = kzalloc(sizeof(*node), GFP_ATOMIC);
  73. if (!node)
  74. return NULL;
  75. node->id = node_id;
  76. xa_init(&node->servers);
  77. xa_store(&nodes, node_id, node, GFP_ATOMIC);
  78. return node;
  79. }
  80. int qrtr_get_service_id(unsigned int node_id, unsigned int port_id)
  81. {
  82. struct qrtr_server *srv;
  83. struct qrtr_node *node;
  84. unsigned long index;
  85. node = xa_load(&nodes, node_id);
  86. if (!node)
  87. return -EINVAL;
  88. xa_for_each(&node->servers, index, srv) {
  89. if (srv->node == node_id && srv->port == port_id)
  90. return srv->service;
  91. }
  92. return -EINVAL;
  93. }
  94. EXPORT_SYMBOL(qrtr_get_service_id);
  95. static int server_match(const struct qrtr_server *srv,
  96. const struct qrtr_server_filter *f)
  97. {
  98. unsigned int ifilter = f->ifilter;
  99. if (f->service != 0 && srv->service != f->service)
  100. return 0;
  101. if (!ifilter && f->instance)
  102. ifilter = ~0;
  103. return (srv->instance & ifilter) == f->instance;
  104. }
  105. static int service_announce_new(struct sockaddr_qrtr *dest,
  106. struct qrtr_server *srv)
  107. {
  108. struct qrtr_ctrl_pkt pkt;
  109. struct msghdr msg = { };
  110. struct kvec iv;
  111. trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
  112. srv->node, srv->port);
  113. NS_INFO("%s: [0x%x:0x%x]@[0x%x:0x%x]\n", __func__, srv->service,
  114. srv->instance, srv->node, srv->port);
  115. iv.iov_base = &pkt;
  116. iv.iov_len = sizeof(pkt);
  117. memset(&pkt, 0, sizeof(pkt));
  118. pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
  119. pkt.server.service = cpu_to_le32(srv->service);
  120. pkt.server.instance = cpu_to_le32(srv->instance);
  121. pkt.server.node = cpu_to_le32(srv->node);
  122. pkt.server.port = cpu_to_le32(srv->port);
  123. msg.msg_name = (struct sockaddr *)dest;
  124. msg.msg_namelen = sizeof(*dest);
  125. return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  126. }
  127. static int service_announce_del(struct sockaddr_qrtr *dest,
  128. struct qrtr_server *srv)
  129. {
  130. struct qrtr_ctrl_pkt pkt;
  131. struct msghdr msg = { };
  132. struct kvec iv;
  133. int ret;
  134. trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
  135. srv->node, srv->port);
  136. NS_INFO("%s: [0x%x:0x%x]@[0x%x:0x%x]\n", __func__, srv->service,
  137. srv->instance, srv->node, srv->port);
  138. iv.iov_base = &pkt;
  139. iv.iov_len = sizeof(pkt);
  140. memset(&pkt, 0, sizeof(pkt));
  141. pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
  142. pkt.server.service = cpu_to_le32(srv->service);
  143. pkt.server.instance = cpu_to_le32(srv->instance);
  144. pkt.server.node = cpu_to_le32(srv->node);
  145. pkt.server.port = cpu_to_le32(srv->port);
  146. msg.msg_name = (struct sockaddr *)dest;
  147. msg.msg_namelen = sizeof(*dest);
  148. ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  149. if (ret < 0 && ret != -ENODEV)
  150. pr_err_ratelimited("failed to announce del service %d\n", ret);
  151. return ret;
  152. }
  153. static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
  154. bool new)
  155. {
  156. struct qrtr_ctrl_pkt pkt;
  157. struct msghdr msg = { };
  158. struct kvec iv;
  159. int ret;
  160. iv.iov_base = &pkt;
  161. iv.iov_len = sizeof(pkt);
  162. memset(&pkt, 0, sizeof(pkt));
  163. pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
  164. cpu_to_le32(QRTR_TYPE_DEL_SERVER);
  165. if (srv) {
  166. pkt.server.service = cpu_to_le32(srv->service);
  167. pkt.server.instance = cpu_to_le32(srv->instance);
  168. pkt.server.node = cpu_to_le32(srv->node);
  169. pkt.server.port = cpu_to_le32(srv->port);
  170. }
  171. msg.msg_name = (struct sockaddr *)to;
  172. msg.msg_namelen = sizeof(*to);
  173. ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  174. if (ret < 0 && ret != -ENODEV)
  175. pr_err_ratelimited("failed to send lookup notification %d\n",
  176. ret);
  177. }
  178. static int announce_servers(struct sockaddr_qrtr *sq)
  179. {
  180. struct qrtr_server *srv;
  181. struct qrtr_node *node;
  182. unsigned long index;
  183. unsigned long node_idx;
  184. int ret;
  185. /* Announce the list of servers registered in this node */
  186. xa_for_each(&nodes, node_idx, node) {
  187. if (node->id == sq->sq_node) {
  188. pr_info("Avoiding duplicate announce for NODE ID %u\n", node->id);
  189. continue;
  190. }
  191. xa_for_each(&node->servers, index, srv) {
  192. ret = service_announce_new(sq, srv);
  193. if (ret < 0) {
  194. if (ret == -ENODEV)
  195. continue;
  196. pr_err("failed to announce new service %d\n", ret);
  197. return ret;
  198. }
  199. }
  200. }
  201. return 0;
  202. }
  203. static struct qrtr_server *server_add(unsigned int service,
  204. unsigned int instance,
  205. unsigned int node_id,
  206. unsigned int port)
  207. {
  208. struct qrtr_server *srv;
  209. struct qrtr_server *old;
  210. struct qrtr_node *node;
  211. if (!service || !port)
  212. return NULL;
  213. srv = kzalloc(sizeof(*srv), GFP_KERNEL);
  214. if (!srv)
  215. return NULL;
  216. srv->service = service;
  217. srv->instance = instance;
  218. srv->node = node_id;
  219. srv->port = port;
  220. node = node_get(node_id);
  221. if (!node)
  222. goto err;
  223. /* Delete the old server on the same port */
  224. old = xa_store(&node->servers, port, srv, GFP_KERNEL);
  225. if (old) {
  226. if (xa_is_err(old)) {
  227. pr_err("failed to add server [0x%x:0x%x] ret:%d\n",
  228. srv->service, srv->instance, xa_err(old));
  229. goto err;
  230. } else {
  231. kfree(old);
  232. }
  233. }
  234. trace_qrtr_ns_server_add(srv->service, srv->instance,
  235. srv->node, srv->port);
  236. NS_INFO("%s: [0x%x:0x%x]@[0x%x:0x%x]\n", __func__, srv->service,
  237. srv->instance, srv->node, srv->port);
  238. return srv;
  239. err:
  240. kfree(srv);
  241. return NULL;
  242. }
  243. static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
  244. {
  245. struct qrtr_lookup *lookup;
  246. struct qrtr_server *srv;
  247. struct list_head *li;
  248. srv = xa_load(&node->servers, port);
  249. if (!srv)
  250. return 0;
  251. xa_erase(&node->servers, port);
  252. /* Broadcast the removal of local servers */
  253. if (srv->node == qrtr_ns.local_node && bcast)
  254. service_announce_del(&qrtr_ns.bcast_sq, srv);
  255. /* Announce the service's disappearance to observers */
  256. list_for_each(li, &qrtr_ns.lookups) {
  257. lookup = container_of(li, struct qrtr_lookup, li);
  258. if (lookup->service && lookup->service != srv->service)
  259. continue;
  260. if (lookup->instance && lookup->instance != srv->instance)
  261. continue;
  262. lookup_notify(&lookup->sq, srv, false);
  263. }
  264. kfree(srv);
  265. return 0;
  266. }
  267. static int say_hello(struct sockaddr_qrtr *dest)
  268. {
  269. struct qrtr_ctrl_pkt pkt;
  270. struct msghdr msg = { };
  271. struct kvec iv;
  272. int ret;
  273. iv.iov_base = &pkt;
  274. iv.iov_len = sizeof(pkt);
  275. memset(&pkt, 0, sizeof(pkt));
  276. pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
  277. msg.msg_name = (struct sockaddr *)dest;
  278. msg.msg_namelen = sizeof(*dest);
  279. ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  280. if (ret < 0)
  281. pr_err("failed to send hello msg %d\n", ret);
  282. return ret;
  283. }
  284. /* Announce the list of servers registered on the local node */
  285. static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
  286. {
  287. int ret;
  288. ret = say_hello(sq);
  289. if (ret < 0)
  290. return ret;
  291. return announce_servers(sq);
  292. }
  293. static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
  294. {
  295. struct qrtr_node *local_node;
  296. struct qrtr_ctrl_pkt pkt;
  297. struct qrtr_server *srv;
  298. struct sockaddr_qrtr sq;
  299. struct msghdr msg = { };
  300. struct qrtr_node *node;
  301. unsigned long index;
  302. struct kvec iv;
  303. int ret;
  304. iv.iov_base = &pkt;
  305. iv.iov_len = sizeof(pkt);
  306. node = node_get(from->sq_node);
  307. if (!node)
  308. return 0;
  309. /* Advertise removal of this client to all servers of remote node */
  310. xa_for_each(&node->servers, index, srv)
  311. server_del(node, srv->port, true);
  312. /* Advertise the removal of this client to all local servers */
  313. local_node = node_get(qrtr_ns.local_node);
  314. if (!local_node)
  315. return 0;
  316. memset(&pkt, 0, sizeof(pkt));
  317. pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
  318. pkt.client.node = cpu_to_le32(from->sq_node);
  319. xa_for_each(&local_node->servers, index, srv) {
  320. sq.sq_family = AF_QIPCRTR;
  321. sq.sq_node = srv->node;
  322. sq.sq_port = srv->port;
  323. msg.msg_name = (struct sockaddr *)&sq;
  324. msg.msg_namelen = sizeof(sq);
  325. ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  326. if (ret < 0 && ret != -ENODEV)
  327. pr_err_ratelimited("send bye failed: [0x%x:0x%x] 0x%x ret: %d\n",
  328. srv->service, srv->instance,
  329. srv->port, ret);
  330. }
  331. return 0;
  332. }
  333. static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
  334. unsigned int node_id, unsigned int port)
  335. {
  336. struct qrtr_node *local_node;
  337. struct qrtr_lookup *lookup;
  338. struct qrtr_ctrl_pkt pkt;
  339. struct msghdr msg = { };
  340. struct qrtr_server *srv;
  341. struct sockaddr_qrtr sq;
  342. struct qrtr_node *node;
  343. struct list_head *tmp;
  344. struct list_head *li;
  345. unsigned long index;
  346. struct kvec iv;
  347. int ret;
  348. iv.iov_base = &pkt;
  349. iv.iov_len = sizeof(pkt);
  350. /* Local DEL_CLIENT messages comes from the port being closed */
  351. if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
  352. return -EINVAL;
  353. /* Remove any lookups by this client */
  354. list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
  355. lookup = container_of(li, struct qrtr_lookup, li);
  356. if (lookup->sq.sq_node != node_id)
  357. continue;
  358. if (lookup->sq.sq_port != port)
  359. continue;
  360. list_del(&lookup->li);
  361. kfree(lookup);
  362. }
  363. /* Remove the server belonging to this port but don't broadcast
  364. * DEL_SERVER. Neighbours would've already removed the server belonging
  365. * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
  366. */
  367. node = node_get(node_id);
  368. if (node)
  369. server_del(node, port, false);
  370. /* Advertise the removal of this client to all local servers */
  371. local_node = node_get(qrtr_ns.local_node);
  372. if (!local_node)
  373. return 0;
  374. memset(&pkt, 0, sizeof(pkt));
  375. pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
  376. pkt.client.node = cpu_to_le32(node_id);
  377. pkt.client.port = cpu_to_le32(port);
  378. xa_for_each(&local_node->servers, index, srv) {
  379. sq.sq_family = AF_QIPCRTR;
  380. sq.sq_node = srv->node;
  381. sq.sq_port = srv->port;
  382. msg.msg_name = (struct sockaddr *)&sq;
  383. msg.msg_namelen = sizeof(sq);
  384. ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  385. if (ret < 0 && ret != -ENODEV)
  386. pr_err_ratelimited("del client cmd failed: [0x%x:0x%x] 0x%x %d\n",
  387. srv->service, srv->instance,
  388. srv->port, ret);
  389. }
  390. return 0;
  391. }
  392. static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
  393. unsigned int service, unsigned int instance,
  394. unsigned int node_id, unsigned int port)
  395. {
  396. struct qrtr_lookup *lookup;
  397. struct qrtr_server *srv;
  398. struct list_head *li;
  399. int ret = 0;
  400. /* Ignore specified node and port for local servers */
  401. if (from->sq_node == qrtr_ns.local_node) {
  402. node_id = from->sq_node;
  403. port = from->sq_port;
  404. }
  405. srv = server_add(service, instance, node_id, port);
  406. if (!srv)
  407. return -EINVAL;
  408. if (srv->node == qrtr_ns.local_node) {
  409. ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
  410. if (ret < 0) {
  411. pr_err("failed to announce new service %d\n", ret);
  412. return ret;
  413. }
  414. }
  415. /* Notify any potential lookups about the new server */
  416. list_for_each(li, &qrtr_ns.lookups) {
  417. lookup = container_of(li, struct qrtr_lookup, li);
  418. if (lookup->service && lookup->service != service)
  419. continue;
  420. if (lookup->instance && lookup->instance != instance)
  421. continue;
  422. lookup_notify(&lookup->sq, srv, true);
  423. }
  424. return ret;
  425. }
  426. static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
  427. unsigned int service, unsigned int instance,
  428. unsigned int node_id, unsigned int port)
  429. {
  430. struct qrtr_node *node;
  431. /* Ignore specified node and port for local servers*/
  432. if (from->sq_node == qrtr_ns.local_node) {
  433. node_id = from->sq_node;
  434. port = from->sq_port;
  435. }
  436. /* Local servers may only unregister themselves */
  437. if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
  438. return -EINVAL;
  439. node = node_get(node_id);
  440. if (!node)
  441. return -ENOENT;
  442. return server_del(node, port, true);
  443. }
  444. static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
  445. unsigned int service, unsigned int instance)
  446. {
  447. struct qrtr_server_filter filter;
  448. struct qrtr_lookup *lookup;
  449. struct qrtr_server *srv;
  450. struct qrtr_node *node;
  451. unsigned long node_idx;
  452. unsigned long srv_idx;
  453. /* Accept only local observers */
  454. if (from->sq_node != qrtr_ns.local_node)
  455. return -EINVAL;
  456. lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
  457. if (!lookup)
  458. return -ENOMEM;
  459. lookup->sq = *from;
  460. lookup->service = service;
  461. lookup->instance = instance;
  462. list_add_tail(&lookup->li, &qrtr_ns.lookups);
  463. memset(&filter, 0, sizeof(filter));
  464. filter.service = service;
  465. filter.instance = instance;
  466. xa_for_each(&nodes, node_idx, node) {
  467. xa_for_each(&node->servers, srv_idx, srv) {
  468. if (!server_match(srv, &filter))
  469. continue;
  470. lookup_notify(from, srv, true);
  471. }
  472. }
  473. /* Empty notification, to indicate end of listing */
  474. lookup_notify(from, NULL, true);
  475. return 0;
  476. }
  477. static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
  478. unsigned int service, unsigned int instance)
  479. {
  480. struct qrtr_lookup *lookup;
  481. struct list_head *tmp;
  482. struct list_head *li;
  483. list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
  484. lookup = container_of(li, struct qrtr_lookup, li);
  485. if (lookup->sq.sq_node != from->sq_node)
  486. continue;
  487. if (lookup->sq.sq_port != from->sq_port)
  488. continue;
  489. if (lookup->service != service)
  490. continue;
  491. if (lookup->instance && lookup->instance != instance)
  492. continue;
  493. list_del(&lookup->li);
  494. kfree(lookup);
  495. }
  496. }
  497. static void ns_log_msg(const struct qrtr_ctrl_pkt *pkt,
  498. struct sockaddr_qrtr *sq)
  499. {
  500. unsigned int cmd = le32_to_cpu(pkt->cmd);
  501. if (cmd == QRTR_TYPE_HELLO || cmd == QRTR_TYPE_BYE)
  502. NS_INFO("cmd:0x%x node[0x%x]\n", cmd, sq->sq_node);
  503. else if (cmd == QRTR_TYPE_DEL_CLIENT)
  504. NS_INFO("cmd:0x%x addr[0x%x:0x%x]\n", cmd,
  505. le32_to_cpu(pkt->client.node),
  506. le32_to_cpu(pkt->client.port));
  507. else if (cmd == QRTR_TYPE_NEW_SERVER || cmd == QRTR_TYPE_DEL_SERVER)
  508. NS_INFO("cmd:0x%x SVC[0x%x:0x%x] addr[0x%x:0x%x]\n", cmd,
  509. le32_to_cpu(pkt->server.service),
  510. le32_to_cpu(pkt->server.instance),
  511. le32_to_cpu(pkt->server.node),
  512. le32_to_cpu(pkt->server.port));
  513. else if (cmd == QRTR_TYPE_NEW_LOOKUP || cmd == QRTR_TYPE_DEL_LOOKUP)
  514. NS_INFO("cmd:0x%x SVC[0x%x:0x%x]\n", cmd,
  515. le32_to_cpu(pkt->server.service),
  516. le32_to_cpu(pkt->server.instance));
  517. }
  518. static void qrtr_ns_worker(struct kthread_work *work)
  519. {
  520. const struct qrtr_ctrl_pkt *pkt;
  521. size_t recv_buf_size = 4096;
  522. struct sockaddr_qrtr sq;
  523. struct msghdr msg = { };
  524. unsigned int cmd;
  525. ssize_t msglen;
  526. void *recv_buf;
  527. struct kvec iv;
  528. int ret;
  529. msg.msg_name = (struct sockaddr *)&sq;
  530. msg.msg_namelen = sizeof(sq);
  531. recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
  532. if (!recv_buf)
  533. return;
  534. for (;;) {
  535. iv.iov_base = recv_buf;
  536. iv.iov_len = recv_buf_size;
  537. msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
  538. iv.iov_len, MSG_DONTWAIT);
  539. if (msglen == -EAGAIN)
  540. break;
  541. if (msglen < 0) {
  542. pr_err("error receiving packet: %zd\n", msglen);
  543. break;
  544. }
  545. pkt = recv_buf;
  546. cmd = le32_to_cpu(pkt->cmd);
  547. if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
  548. qrtr_ctrl_pkt_strings[cmd])
  549. trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
  550. sq.sq_node, sq.sq_port);
  551. ns_log_msg(pkt, &sq);
  552. ret = 0;
  553. switch (cmd) {
  554. case QRTR_TYPE_HELLO:
  555. ret = ctrl_cmd_hello(&sq);
  556. break;
  557. case QRTR_TYPE_BYE:
  558. ret = ctrl_cmd_bye(&sq);
  559. break;
  560. case QRTR_TYPE_DEL_CLIENT:
  561. ret = ctrl_cmd_del_client(&sq,
  562. le32_to_cpu(pkt->client.node),
  563. le32_to_cpu(pkt->client.port));
  564. break;
  565. case QRTR_TYPE_NEW_SERVER:
  566. ret = ctrl_cmd_new_server(&sq,
  567. le32_to_cpu(pkt->server.service),
  568. le32_to_cpu(pkt->server.instance),
  569. le32_to_cpu(pkt->server.node),
  570. le32_to_cpu(pkt->server.port));
  571. break;
  572. case QRTR_TYPE_DEL_SERVER:
  573. ret = ctrl_cmd_del_server(&sq,
  574. le32_to_cpu(pkt->server.service),
  575. le32_to_cpu(pkt->server.instance),
  576. le32_to_cpu(pkt->server.node),
  577. le32_to_cpu(pkt->server.port));
  578. break;
  579. case QRTR_TYPE_EXIT:
  580. case QRTR_TYPE_PING:
  581. case QRTR_TYPE_RESUME_TX:
  582. break;
  583. case QRTR_TYPE_NEW_LOOKUP:
  584. ret = ctrl_cmd_new_lookup(&sq,
  585. le32_to_cpu(pkt->server.service),
  586. le32_to_cpu(pkt->server.instance));
  587. break;
  588. case QRTR_TYPE_DEL_LOOKUP:
  589. ctrl_cmd_del_lookup(&sq,
  590. le32_to_cpu(pkt->server.service),
  591. le32_to_cpu(pkt->server.instance));
  592. break;
  593. }
  594. if (ret < 0)
  595. pr_err("failed while handling packet from %d:%d",
  596. sq.sq_node, sq.sq_port);
  597. }
  598. kfree(recv_buf);
  599. }
  600. static void qrtr_ns_data_ready(struct sock *sk)
  601. {
  602. kthread_queue_work(&qrtr_ns.kworker, &qrtr_ns.work);
  603. }
  604. int qrtr_ns_init(void)
  605. {
  606. struct sockaddr_qrtr sq;
  607. int rx_buf_sz = INT_MAX;
  608. int ret;
  609. INIT_LIST_HEAD(&qrtr_ns.lookups);
  610. kthread_init_worker(&qrtr_ns.kworker);
  611. kthread_init_work(&qrtr_ns.work, qrtr_ns_worker);
  612. ns_ilc = ipc_log_context_create(NS_LOG_PAGE_CNT, "qrtr_ns", 0);
  613. ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
  614. PF_QIPCRTR, &qrtr_ns.sock);
  615. if (ret < 0)
  616. return ret;
  617. ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
  618. if (ret < 0) {
  619. pr_err("failed to get socket name\n");
  620. goto err_sock;
  621. }
  622. qrtr_ns.task = kthread_run(kthread_worker_fn, &qrtr_ns.kworker,
  623. "qrtr_ns");
  624. if (IS_ERR(qrtr_ns.task)) {
  625. pr_err("failed to spawn worker thread %ld\n",
  626. PTR_ERR(qrtr_ns.task));
  627. goto err_sock;
  628. }
  629. qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
  630. sq.sq_port = QRTR_PORT_CTRL;
  631. qrtr_ns.local_node = sq.sq_node;
  632. ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
  633. if (ret < 0) {
  634. pr_err("failed to bind to socket\n");
  635. goto err_wq;
  636. }
  637. sock_setsockopt(qrtr_ns.sock, SOL_SOCKET, SO_RCVBUF,
  638. KERNEL_SOCKPTR((void *)&rx_buf_sz), sizeof(rx_buf_sz));
  639. qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
  640. qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
  641. qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
  642. ret = say_hello(&qrtr_ns.bcast_sq);
  643. if (ret < 0)
  644. goto err_wq;
  645. return 0;
  646. err_wq:
  647. kthread_stop(qrtr_ns.task);
  648. err_sock:
  649. sock_release(qrtr_ns.sock);
  650. return ret;
  651. }
  652. EXPORT_SYMBOL_GPL(qrtr_ns_init);
  653. void qrtr_ns_remove(void)
  654. {
  655. kthread_flush_worker(&qrtr_ns.kworker);
  656. kthread_stop(qrtr_ns.task);
  657. sock_release(qrtr_ns.sock);
  658. }
  659. EXPORT_SYMBOL_GPL(qrtr_ns_remove);
  660. MODULE_AUTHOR("Manivannan Sadhasivam <[email protected]>");
  661. MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
  662. MODULE_LICENSE("Dual BSD/GPL");