cfcnfg.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (C) ST-Ericsson AB 2010
  4. * Author: Sjur Brendeland
  5. */
  6. #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
  7. #include <linux/kernel.h>
  8. #include <linux/stddef.h>
  9. #include <linux/slab.h>
  10. #include <linux/netdevice.h>
  11. #include <linux/module.h>
  12. #include <net/caif/caif_layer.h>
  13. #include <net/caif/cfpkt.h>
  14. #include <net/caif/cfcnfg.h>
  15. #include <net/caif/cfctrl.h>
  16. #include <net/caif/cfmuxl.h>
  17. #include <net/caif/cffrml.h>
  18. #include <net/caif/cfserl.h>
  19. #include <net/caif/cfsrvl.h>
  20. #include <net/caif/caif_dev.h>
  21. #define container_obj(layr) container_of(layr, struct cfcnfg, layer)
  22. /* Information about CAIF physical interfaces held by Config Module in order
  23. * to manage physical interfaces
  24. */
  25. struct cfcnfg_phyinfo {
  26. struct list_head node;
  27. bool up;
  28. /* Pointer to the layer below the MUX (framing layer) */
  29. struct cflayer *frm_layer;
  30. /* Pointer to the lowest actual physical layer */
  31. struct cflayer *phy_layer;
  32. /* Unique identifier of the physical interface */
  33. unsigned int id;
  34. /* Preference of the physical in interface */
  35. enum cfcnfg_phy_preference pref;
  36. /* Information about the physical device */
  37. struct dev_info dev_info;
  38. /* Interface index */
  39. int ifindex;
  40. /* Protocol head room added for CAIF link layer */
  41. int head_room;
  42. /* Use Start of frame checksum */
  43. bool use_fcs;
  44. };
  45. struct cfcnfg {
  46. struct cflayer layer;
  47. struct cflayer *ctrl;
  48. struct cflayer *mux;
  49. struct list_head phys;
  50. struct mutex lock;
  51. };
  52. static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
  53. enum cfctrl_srv serv, u8 phyid,
  54. struct cflayer *adapt_layer);
  55. static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
  56. static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
  57. struct cflayer *adapt_layer);
  58. static void cfctrl_resp_func(void);
  59. static void cfctrl_enum_resp(void);
  60. struct cfcnfg *cfcnfg_create(void)
  61. {
  62. struct cfcnfg *this;
  63. struct cfctrl_rsp *resp;
  64. might_sleep();
  65. /* Initiate this layer */
  66. this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
  67. if (!this)
  68. return NULL;
  69. this->mux = cfmuxl_create();
  70. if (!this->mux)
  71. goto out_of_mem;
  72. this->ctrl = cfctrl_create();
  73. if (!this->ctrl)
  74. goto out_of_mem;
  75. /* Initiate response functions */
  76. resp = cfctrl_get_respfuncs(this->ctrl);
  77. resp->enum_rsp = cfctrl_enum_resp;
  78. resp->linkerror_ind = cfctrl_resp_func;
  79. resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
  80. resp->sleep_rsp = cfctrl_resp_func;
  81. resp->wake_rsp = cfctrl_resp_func;
  82. resp->restart_rsp = cfctrl_resp_func;
  83. resp->radioset_rsp = cfctrl_resp_func;
  84. resp->linksetup_rsp = cfcnfg_linkup_rsp;
  85. resp->reject_rsp = cfcnfg_reject_rsp;
  86. INIT_LIST_HEAD(&this->phys);
  87. cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
  88. layer_set_dn(this->ctrl, this->mux);
  89. layer_set_up(this->ctrl, this);
  90. mutex_init(&this->lock);
  91. return this;
  92. out_of_mem:
  93. synchronize_rcu();
  94. kfree(this->mux);
  95. kfree(this->ctrl);
  96. kfree(this);
  97. return NULL;
  98. }
  99. void cfcnfg_remove(struct cfcnfg *cfg)
  100. {
  101. might_sleep();
  102. if (cfg) {
  103. synchronize_rcu();
  104. kfree(cfg->mux);
  105. cfctrl_remove(cfg->ctrl);
  106. kfree(cfg);
  107. }
  108. }
  109. static void cfctrl_resp_func(void)
  110. {
  111. }
  112. static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
  113. u8 phyid)
  114. {
  115. struct cfcnfg_phyinfo *phy;
  116. list_for_each_entry_rcu(phy, &cnfg->phys, node)
  117. if (phy->id == phyid)
  118. return phy;
  119. return NULL;
  120. }
  121. static void cfctrl_enum_resp(void)
  122. {
  123. }
  124. static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
  125. enum cfcnfg_phy_preference phy_pref)
  126. {
  127. /* Try to match with specified preference */
  128. struct cfcnfg_phyinfo *phy;
  129. list_for_each_entry_rcu(phy, &cnfg->phys, node) {
  130. if (phy->up && phy->pref == phy_pref &&
  131. phy->frm_layer != NULL)
  132. return &phy->dev_info;
  133. }
  134. /* Otherwise just return something */
  135. list_for_each_entry_rcu(phy, &cnfg->phys, node)
  136. if (phy->up)
  137. return &phy->dev_info;
  138. return NULL;
  139. }
  140. static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
  141. {
  142. struct cfcnfg_phyinfo *phy;
  143. list_for_each_entry_rcu(phy, &cnfg->phys, node)
  144. if (phy->ifindex == ifi && phy->up)
  145. return phy->id;
  146. return -ENODEV;
  147. }
  148. int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
  149. {
  150. u8 channel_id;
  151. struct cfcnfg *cfg = get_cfcnfg(net);
  152. caif_assert(adap_layer != NULL);
  153. cfctrl_cancel_req(cfg->ctrl, adap_layer);
  154. channel_id = adap_layer->id;
  155. if (channel_id != 0) {
  156. struct cflayer *servl;
  157. servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
  158. cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
  159. if (servl != NULL)
  160. layer_set_up(servl, NULL);
  161. } else
  162. pr_debug("nothing to disconnect\n");
  163. /* Do RCU sync before initiating cleanup */
  164. synchronize_rcu();
  165. if (adap_layer->ctrlcmd != NULL)
  166. adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
  167. return 0;
  168. }
  169. EXPORT_SYMBOL(caif_disconnect_client);
  170. static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
  171. {
  172. }
  173. static const int protohead[CFCTRL_SRV_MASK] = {
  174. [CFCTRL_SRV_VEI] = 4,
  175. [CFCTRL_SRV_DATAGRAM] = 7,
  176. [CFCTRL_SRV_UTIL] = 4,
  177. [CFCTRL_SRV_RFM] = 3,
  178. [CFCTRL_SRV_DBG] = 3,
  179. };
  180. static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
  181. struct caif_connect_request *s,
  182. struct cfctrl_link_param *l)
  183. {
  184. struct dev_info *dev_info;
  185. enum cfcnfg_phy_preference pref;
  186. int res;
  187. memset(l, 0, sizeof(*l));
  188. /* In caif protocol low value is high priority */
  189. l->priority = CAIF_PRIO_MAX - s->priority + 1;
  190. if (s->ifindex != 0) {
  191. res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
  192. if (res < 0)
  193. return res;
  194. l->phyid = res;
  195. } else {
  196. switch (s->link_selector) {
  197. case CAIF_LINK_HIGH_BANDW:
  198. pref = CFPHYPREF_HIGH_BW;
  199. break;
  200. case CAIF_LINK_LOW_LATENCY:
  201. pref = CFPHYPREF_LOW_LAT;
  202. break;
  203. default:
  204. return -EINVAL;
  205. }
  206. dev_info = cfcnfg_get_phyid(cnfg, pref);
  207. if (dev_info == NULL)
  208. return -ENODEV;
  209. l->phyid = dev_info->id;
  210. }
  211. switch (s->protocol) {
  212. case CAIFPROTO_AT:
  213. l->linktype = CFCTRL_SRV_VEI;
  214. l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
  215. l->chtype = s->sockaddr.u.at.type & 0x3;
  216. break;
  217. case CAIFPROTO_DATAGRAM:
  218. l->linktype = CFCTRL_SRV_DATAGRAM;
  219. l->chtype = 0x00;
  220. l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
  221. break;
  222. case CAIFPROTO_DATAGRAM_LOOP:
  223. l->linktype = CFCTRL_SRV_DATAGRAM;
  224. l->chtype = 0x03;
  225. l->endpoint = 0x00;
  226. l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
  227. break;
  228. case CAIFPROTO_RFM:
  229. l->linktype = CFCTRL_SRV_RFM;
  230. l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
  231. strlcpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
  232. sizeof(l->u.rfm.volume));
  233. break;
  234. case CAIFPROTO_UTIL:
  235. l->linktype = CFCTRL_SRV_UTIL;
  236. l->endpoint = 0x00;
  237. l->chtype = 0x00;
  238. strlcpy(l->u.utility.name, s->sockaddr.u.util.service,
  239. sizeof(l->u.utility.name));
  240. caif_assert(sizeof(l->u.utility.name) > 10);
  241. l->u.utility.paramlen = s->param.size;
  242. if (l->u.utility.paramlen > sizeof(l->u.utility.params))
  243. l->u.utility.paramlen = sizeof(l->u.utility.params);
  244. memcpy(l->u.utility.params, s->param.data,
  245. l->u.utility.paramlen);
  246. break;
  247. case CAIFPROTO_DEBUG:
  248. l->linktype = CFCTRL_SRV_DBG;
  249. l->endpoint = s->sockaddr.u.dbg.service;
  250. l->chtype = s->sockaddr.u.dbg.type;
  251. break;
  252. default:
  253. return -EINVAL;
  254. }
  255. return 0;
  256. }
  257. int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
  258. struct cflayer *adap_layer, int *ifindex,
  259. int *proto_head, int *proto_tail)
  260. {
  261. struct cflayer *frml;
  262. struct cfcnfg_phyinfo *phy;
  263. int err;
  264. struct cfctrl_link_param param;
  265. struct cfcnfg *cfg = get_cfcnfg(net);
  266. rcu_read_lock();
  267. err = caif_connect_req_to_link_param(cfg, conn_req, &param);
  268. if (err)
  269. goto unlock;
  270. phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
  271. if (!phy) {
  272. err = -ENODEV;
  273. goto unlock;
  274. }
  275. err = -EINVAL;
  276. if (adap_layer == NULL) {
  277. pr_err("adap_layer is zero\n");
  278. goto unlock;
  279. }
  280. if (adap_layer->receive == NULL) {
  281. pr_err("adap_layer->receive is NULL\n");
  282. goto unlock;
  283. }
  284. if (adap_layer->ctrlcmd == NULL) {
  285. pr_err("adap_layer->ctrlcmd == NULL\n");
  286. goto unlock;
  287. }
  288. err = -ENODEV;
  289. frml = phy->frm_layer;
  290. if (frml == NULL) {
  291. pr_err("Specified PHY type does not exist!\n");
  292. goto unlock;
  293. }
  294. caif_assert(param.phyid == phy->id);
  295. caif_assert(phy->frm_layer->id ==
  296. param.phyid);
  297. caif_assert(phy->phy_layer->id ==
  298. param.phyid);
  299. *ifindex = phy->ifindex;
  300. *proto_tail = 2;
  301. *proto_head = protohead[param.linktype] + phy->head_room;
  302. rcu_read_unlock();
  303. /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
  304. cfctrl_enum_req(cfg->ctrl, param.phyid);
  305. return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
  306. unlock:
  307. rcu_read_unlock();
  308. return err;
  309. }
  310. EXPORT_SYMBOL(caif_connect_client);
  311. static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
  312. struct cflayer *adapt_layer)
  313. {
  314. if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
  315. adapt_layer->ctrlcmd(adapt_layer,
  316. CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
  317. }
  318. static void
  319. cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
  320. u8 phyid, struct cflayer *adapt_layer)
  321. {
  322. struct cfcnfg *cnfg = container_obj(layer);
  323. struct cflayer *servicel = NULL;
  324. struct cfcnfg_phyinfo *phyinfo;
  325. struct net_device *netdev;
  326. if (channel_id == 0) {
  327. pr_warn("received channel_id zero\n");
  328. if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
  329. adapt_layer->ctrlcmd(adapt_layer,
  330. CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
  331. return;
  332. }
  333. rcu_read_lock();
  334. if (adapt_layer == NULL) {
  335. pr_debug("link setup response but no client exist, send linkdown back\n");
  336. cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
  337. goto unlock;
  338. }
  339. caif_assert(cnfg != NULL);
  340. caif_assert(phyid != 0);
  341. phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
  342. if (phyinfo == NULL) {
  343. pr_err("ERROR: Link Layer Device disappeared while connecting\n");
  344. goto unlock;
  345. }
  346. caif_assert(phyinfo != NULL);
  347. caif_assert(phyinfo->id == phyid);
  348. caif_assert(phyinfo->phy_layer != NULL);
  349. caif_assert(phyinfo->phy_layer->id == phyid);
  350. adapt_layer->id = channel_id;
  351. switch (serv) {
  352. case CFCTRL_SRV_VEI:
  353. servicel = cfvei_create(channel_id, &phyinfo->dev_info);
  354. break;
  355. case CFCTRL_SRV_DATAGRAM:
  356. servicel = cfdgml_create(channel_id,
  357. &phyinfo->dev_info);
  358. break;
  359. case CFCTRL_SRV_RFM:
  360. netdev = phyinfo->dev_info.dev;
  361. servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
  362. netdev->mtu);
  363. break;
  364. case CFCTRL_SRV_UTIL:
  365. servicel = cfutill_create(channel_id, &phyinfo->dev_info);
  366. break;
  367. case CFCTRL_SRV_VIDEO:
  368. servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
  369. break;
  370. case CFCTRL_SRV_DBG:
  371. servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
  372. break;
  373. default:
  374. pr_err("Protocol error. Link setup response - unknown channel type\n");
  375. goto unlock;
  376. }
  377. if (!servicel)
  378. goto unlock;
  379. layer_set_dn(servicel, cnfg->mux);
  380. cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
  381. layer_set_up(servicel, adapt_layer);
  382. layer_set_dn(adapt_layer, servicel);
  383. rcu_read_unlock();
  384. servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
  385. return;
  386. unlock:
  387. rcu_read_unlock();
  388. }
  389. int
  390. cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
  391. struct net_device *dev, struct cflayer *phy_layer,
  392. enum cfcnfg_phy_preference pref,
  393. struct cflayer *link_support,
  394. bool fcs, int head_room)
  395. {
  396. struct cflayer *frml;
  397. struct cfcnfg_phyinfo *phyinfo = NULL;
  398. int i, res = 0;
  399. u8 phyid;
  400. mutex_lock(&cnfg->lock);
  401. /* CAIF protocol allow maximum 6 link-layers */
  402. for (i = 0; i < 7; i++) {
  403. phyid = (dev->ifindex + i) & 0x7;
  404. if (phyid == 0)
  405. continue;
  406. if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
  407. goto got_phyid;
  408. }
  409. pr_warn("Too many CAIF Link Layers (max 6)\n");
  410. res = -EEXIST;
  411. goto out;
  412. got_phyid:
  413. phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
  414. if (!phyinfo) {
  415. res = -ENOMEM;
  416. goto out_err;
  417. }
  418. phy_layer->id = phyid;
  419. phyinfo->pref = pref;
  420. phyinfo->id = phyid;
  421. phyinfo->dev_info.id = phyid;
  422. phyinfo->dev_info.dev = dev;
  423. phyinfo->phy_layer = phy_layer;
  424. phyinfo->ifindex = dev->ifindex;
  425. phyinfo->head_room = head_room;
  426. phyinfo->use_fcs = fcs;
  427. frml = cffrml_create(phyid, fcs);
  428. if (!frml) {
  429. res = -ENOMEM;
  430. goto out_err;
  431. }
  432. phyinfo->frm_layer = frml;
  433. layer_set_up(frml, cnfg->mux);
  434. if (link_support != NULL) {
  435. link_support->id = phyid;
  436. layer_set_dn(frml, link_support);
  437. layer_set_up(link_support, frml);
  438. layer_set_dn(link_support, phy_layer);
  439. layer_set_up(phy_layer, link_support);
  440. } else {
  441. layer_set_dn(frml, phy_layer);
  442. layer_set_up(phy_layer, frml);
  443. }
  444. list_add_rcu(&phyinfo->node, &cnfg->phys);
  445. out:
  446. mutex_unlock(&cnfg->lock);
  447. return res;
  448. out_err:
  449. kfree(phyinfo);
  450. mutex_unlock(&cnfg->lock);
  451. return res;
  452. }
  453. EXPORT_SYMBOL(cfcnfg_add_phy_layer);
  454. int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
  455. bool up)
  456. {
  457. struct cfcnfg_phyinfo *phyinfo;
  458. rcu_read_lock();
  459. phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
  460. if (phyinfo == NULL) {
  461. rcu_read_unlock();
  462. return -ENODEV;
  463. }
  464. if (phyinfo->up == up) {
  465. rcu_read_unlock();
  466. return 0;
  467. }
  468. phyinfo->up = up;
  469. if (up) {
  470. cffrml_hold(phyinfo->frm_layer);
  471. cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
  472. phy_layer->id);
  473. } else {
  474. cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
  475. cffrml_put(phyinfo->frm_layer);
  476. }
  477. rcu_read_unlock();
  478. return 0;
  479. }
  480. EXPORT_SYMBOL(cfcnfg_set_phy_state);
  481. int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
  482. {
  483. struct cflayer *frml, *frml_dn;
  484. u16 phyid;
  485. struct cfcnfg_phyinfo *phyinfo;
  486. might_sleep();
  487. mutex_lock(&cnfg->lock);
  488. phyid = phy_layer->id;
  489. phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
  490. if (phyinfo == NULL) {
  491. mutex_unlock(&cnfg->lock);
  492. return 0;
  493. }
  494. caif_assert(phyid == phyinfo->id);
  495. caif_assert(phy_layer == phyinfo->phy_layer);
  496. caif_assert(phy_layer->id == phyid);
  497. caif_assert(phyinfo->frm_layer->id == phyid);
  498. list_del_rcu(&phyinfo->node);
  499. synchronize_rcu();
  500. /* Fail if reference count is not zero */
  501. if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
  502. pr_info("Wait for device inuse\n");
  503. list_add_rcu(&phyinfo->node, &cnfg->phys);
  504. mutex_unlock(&cnfg->lock);
  505. return -EAGAIN;
  506. }
  507. frml = phyinfo->frm_layer;
  508. frml_dn = frml->dn;
  509. cffrml_set_uplayer(frml, NULL);
  510. cffrml_set_dnlayer(frml, NULL);
  511. if (phy_layer != frml_dn) {
  512. layer_set_up(frml_dn, NULL);
  513. layer_set_dn(frml_dn, NULL);
  514. }
  515. layer_set_up(phy_layer, NULL);
  516. if (phyinfo->phy_layer != frml_dn)
  517. kfree(frml_dn);
  518. cffrml_free(frml);
  519. kfree(phyinfo);
  520. mutex_unlock(&cnfg->lock);
  521. return 0;
  522. }
  523. EXPORT_SYMBOL(cfcnfg_del_phy_layer);