cfmuxl.c 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267
  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/stddef.h>
  8. #include <linux/spinlock.h>
  9. #include <linux/slab.h>
  10. #include <linux/rculist.h>
  11. #include <net/caif/cfpkt.h>
  12. #include <net/caif/cfmuxl.h>
  13. #include <net/caif/cfsrvl.h>
  14. #include <net/caif/cffrml.h>
  15. #define container_obj(layr) container_of(layr, struct cfmuxl, layer)
  16. #define CAIF_CTRL_CHANNEL 0
  17. #define UP_CACHE_SIZE 8
  18. #define DN_CACHE_SIZE 8
  19. struct cfmuxl {
  20. struct cflayer layer;
  21. struct list_head srvl_list;
  22. struct list_head frml_list;
  23. struct cflayer *up_cache[UP_CACHE_SIZE];
  24. struct cflayer *dn_cache[DN_CACHE_SIZE];
  25. /*
  26. * Set when inserting or removing downwards layers.
  27. */
  28. spinlock_t transmit_lock;
  29. /*
  30. * Set when inserting or removing upwards layers.
  31. */
  32. spinlock_t receive_lock;
  33. };
  34. static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt);
  35. static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt);
  36. static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
  37. int phyid);
  38. static struct cflayer *get_up(struct cfmuxl *muxl, u16 id);
  39. struct cflayer *cfmuxl_create(void)
  40. {
  41. struct cfmuxl *this = kzalloc(sizeof(struct cfmuxl), GFP_ATOMIC);
  42. if (!this)
  43. return NULL;
  44. this->layer.receive = cfmuxl_receive;
  45. this->layer.transmit = cfmuxl_transmit;
  46. this->layer.ctrlcmd = cfmuxl_ctrlcmd;
  47. INIT_LIST_HEAD(&this->srvl_list);
  48. INIT_LIST_HEAD(&this->frml_list);
  49. spin_lock_init(&this->transmit_lock);
  50. spin_lock_init(&this->receive_lock);
  51. snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "mux");
  52. return &this->layer;
  53. }
  54. int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid)
  55. {
  56. struct cfmuxl *muxl = (struct cfmuxl *) layr;
  57. spin_lock_bh(&muxl->transmit_lock);
  58. list_add_rcu(&dn->node, &muxl->frml_list);
  59. spin_unlock_bh(&muxl->transmit_lock);
  60. return 0;
  61. }
  62. static struct cflayer *get_from_id(struct list_head *list, u16 id)
  63. {
  64. struct cflayer *lyr;
  65. list_for_each_entry_rcu(lyr, list, node) {
  66. if (lyr->id == id)
  67. return lyr;
  68. }
  69. return NULL;
  70. }
  71. int cfmuxl_set_uplayer(struct cflayer *layr, struct cflayer *up, u8 linkid)
  72. {
  73. struct cfmuxl *muxl = container_obj(layr);
  74. struct cflayer *old;
  75. spin_lock_bh(&muxl->receive_lock);
  76. /* Two entries with same id is wrong, so remove old layer from mux */
  77. old = get_from_id(&muxl->srvl_list, linkid);
  78. if (old != NULL)
  79. list_del_rcu(&old->node);
  80. list_add_rcu(&up->node, &muxl->srvl_list);
  81. spin_unlock_bh(&muxl->receive_lock);
  82. return 0;
  83. }
  84. struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid)
  85. {
  86. struct cfmuxl *muxl = container_obj(layr);
  87. struct cflayer *dn;
  88. int idx = phyid % DN_CACHE_SIZE;
  89. spin_lock_bh(&muxl->transmit_lock);
  90. RCU_INIT_POINTER(muxl->dn_cache[idx], NULL);
  91. dn = get_from_id(&muxl->frml_list, phyid);
  92. if (dn == NULL)
  93. goto out;
  94. list_del_rcu(&dn->node);
  95. caif_assert(dn != NULL);
  96. out:
  97. spin_unlock_bh(&muxl->transmit_lock);
  98. return dn;
  99. }
  100. static struct cflayer *get_up(struct cfmuxl *muxl, u16 id)
  101. {
  102. struct cflayer *up;
  103. int idx = id % UP_CACHE_SIZE;
  104. up = rcu_dereference(muxl->up_cache[idx]);
  105. if (up == NULL || up->id != id) {
  106. spin_lock_bh(&muxl->receive_lock);
  107. up = get_from_id(&muxl->srvl_list, id);
  108. rcu_assign_pointer(muxl->up_cache[idx], up);
  109. spin_unlock_bh(&muxl->receive_lock);
  110. }
  111. return up;
  112. }
  113. static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info)
  114. {
  115. struct cflayer *dn;
  116. int idx = dev_info->id % DN_CACHE_SIZE;
  117. dn = rcu_dereference(muxl->dn_cache[idx]);
  118. if (dn == NULL || dn->id != dev_info->id) {
  119. spin_lock_bh(&muxl->transmit_lock);
  120. dn = get_from_id(&muxl->frml_list, dev_info->id);
  121. rcu_assign_pointer(muxl->dn_cache[idx], dn);
  122. spin_unlock_bh(&muxl->transmit_lock);
  123. }
  124. return dn;
  125. }
  126. struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id)
  127. {
  128. struct cflayer *up;
  129. struct cfmuxl *muxl = container_obj(layr);
  130. int idx = id % UP_CACHE_SIZE;
  131. if (id == 0) {
  132. pr_warn("Trying to remove control layer\n");
  133. return NULL;
  134. }
  135. spin_lock_bh(&muxl->receive_lock);
  136. up = get_from_id(&muxl->srvl_list, id);
  137. if (up == NULL)
  138. goto out;
  139. RCU_INIT_POINTER(muxl->up_cache[idx], NULL);
  140. list_del_rcu(&up->node);
  141. out:
  142. spin_unlock_bh(&muxl->receive_lock);
  143. return up;
  144. }
  145. static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt)
  146. {
  147. int ret;
  148. struct cfmuxl *muxl = container_obj(layr);
  149. u8 id;
  150. struct cflayer *up;
  151. if (cfpkt_extr_head(pkt, &id, 1) < 0) {
  152. pr_err("erroneous Caif Packet\n");
  153. cfpkt_destroy(pkt);
  154. return -EPROTO;
  155. }
  156. rcu_read_lock();
  157. up = get_up(muxl, id);
  158. if (up == NULL) {
  159. pr_debug("Received data on unknown link ID = %d (0x%x)"
  160. " up == NULL", id, id);
  161. cfpkt_destroy(pkt);
  162. /*
  163. * Don't return ERROR, since modem misbehaves and sends out
  164. * flow on before linksetup response.
  165. */
  166. rcu_read_unlock();
  167. return /* CFGLU_EPROT; */ 0;
  168. }
  169. /* We can't hold rcu_lock during receive, so take a ref count instead */
  170. cfsrvl_get(up);
  171. rcu_read_unlock();
  172. ret = up->receive(up, pkt);
  173. cfsrvl_put(up);
  174. return ret;
  175. }
  176. static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt)
  177. {
  178. struct cfmuxl *muxl = container_obj(layr);
  179. int err;
  180. u8 linkid;
  181. struct cflayer *dn;
  182. struct caif_payload_info *info = cfpkt_info(pkt);
  183. BUG_ON(!info);
  184. rcu_read_lock();
  185. dn = get_dn(muxl, info->dev_info);
  186. if (dn == NULL) {
  187. pr_debug("Send data on unknown phy ID = %d (0x%x)\n",
  188. info->dev_info->id, info->dev_info->id);
  189. rcu_read_unlock();
  190. cfpkt_destroy(pkt);
  191. return -ENOTCONN;
  192. }
  193. info->hdr_len += 1;
  194. linkid = info->channel_id;
  195. cfpkt_add_head(pkt, &linkid, 1);
  196. /* We can't hold rcu_lock during receive, so take a ref count instead */
  197. cffrml_hold(dn);
  198. rcu_read_unlock();
  199. err = dn->transmit(dn, pkt);
  200. cffrml_put(dn);
  201. return err;
  202. }
  203. static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
  204. int phyid)
  205. {
  206. struct cfmuxl *muxl = container_obj(layr);
  207. struct cflayer *layer;
  208. rcu_read_lock();
  209. list_for_each_entry_rcu(layer, &muxl->srvl_list, node) {
  210. if (cfsrvl_phyid_match(layer, phyid) && layer->ctrlcmd) {
  211. if ((ctrl == _CAIF_CTRLCMD_PHYIF_DOWN_IND ||
  212. ctrl == CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND) &&
  213. layer->id != 0)
  214. cfmuxl_remove_uplayer(layr, layer->id);
  215. /* NOTE: ctrlcmd is not allowed to block */
  216. layer->ctrlcmd(layer, ctrl, phyid);
  217. }
  218. }
  219. rcu_read_unlock();
  220. }