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