blob: e857d8995ca38586f7ddda4fe6ece7a9a2c35506 [file] [log] [blame]
Sjur Braendeland15c9ac02010-03-30 13:56:24 +00001/*
2 * Copyright (C) ST-Ericsson AB 2010
3 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com
4 * License terms: GNU General Public License (GPL) version 2
5 */
Joe Perchesb31fa5b2010-09-05 21:31:11 +00006
7#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8
Sjur Braendeland15c9ac02010-03-30 13:56:24 +00009#include <linux/kernel.h>
10#include <linux/stddef.h>
Stephen Rothwell6c579902010-04-01 00:28:49 -070011#include <linux/slab.h>
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +000012#include <linux/netdevice.h>
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +000013#include <linux/module.h>
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000014#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>
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +000022#include <net/caif/caif_dev.h>
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000023
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 */
29struct cfcnfg_phyinfo {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +000030 struct list_head node;
31 bool up;
32
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000033 /* 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
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000042 /* Information about the physical device */
43 struct dev_info dev_info;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +000044
45 /* Interface index */
46 int ifindex;
47
48 /* Use Start of frame extension */
49 bool use_stx;
50
51 /* Use Start of frame checksum */
52 bool use_fcs;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000053};
54
55struct cfcnfg {
56 struct cflayer layer;
57 struct cflayer *ctrl;
58 struct cflayer *mux;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +000059 struct list_head phys;
60 struct mutex lock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000061};
62
Sjur Braendelande539d832010-04-28 08:54:35 +000063static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000064 enum cfctrl_srv serv, u8 phyid,
65 struct cflayer *adapt_layer);
Sjur Braendeland8d545c82010-04-28 08:54:37 +000066static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
Sjur Braendelande539d832010-04-28 08:54:35 +000067static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000068 struct cflayer *adapt_layer);
69static void cfctrl_resp_func(void);
70static void cfctrl_enum_resp(void);
71
72struct cfcnfg *cfcnfg_create(void)
73{
74 struct cfcnfg *this;
75 struct cfctrl_rsp *resp;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +000076
77 might_sleep();
78
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000079 /* Initiate this layer */
Julia Lawall49afa552010-05-13 10:03:32 +000080 this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000081 if (!this) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +000082 pr_warn("Out of memory\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000083 return NULL;
84 }
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000085 this->mux = cfmuxl_create();
86 if (!this->mux)
87 goto out_of_mem;
88 this->ctrl = cfctrl_create();
89 if (!this->ctrl)
90 goto out_of_mem;
91 /* Initiate response functions */
92 resp = cfctrl_get_respfuncs(this->ctrl);
93 resp->enum_rsp = cfctrl_enum_resp;
94 resp->linkerror_ind = cfctrl_resp_func;
Sjur Braendelande539d832010-04-28 08:54:35 +000095 resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000096 resp->sleep_rsp = cfctrl_resp_func;
97 resp->wake_rsp = cfctrl_resp_func;
98 resp->restart_rsp = cfctrl_resp_func;
99 resp->radioset_rsp = cfctrl_resp_func;
Sjur Braendelande539d832010-04-28 08:54:35 +0000100 resp->linksetup_rsp = cfcnfg_linkup_rsp;
101 resp->reject_rsp = cfcnfg_reject_rsp;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000102 INIT_LIST_HEAD(&this->phys);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000103
104 cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
105 layer_set_dn(this->ctrl, this->mux);
106 layer_set_up(this->ctrl, this);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000107 mutex_init(&this->lock);
108
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000109 return this;
110out_of_mem:
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000111 pr_warn("Out of memory\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000112
113 synchronize_rcu();
114
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000115 kfree(this->mux);
116 kfree(this->ctrl);
117 kfree(this);
118 return NULL;
119}
120EXPORT_SYMBOL(cfcnfg_create);
121
122void cfcnfg_remove(struct cfcnfg *cfg)
123{
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000124 might_sleep();
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000125 if (cfg) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000126 synchronize_rcu();
127
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000128 kfree(cfg->mux);
129 kfree(cfg->ctrl);
130 kfree(cfg);
131 }
132}
133
134static void cfctrl_resp_func(void)
135{
136}
137
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000138static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
139 u8 phyid)
140{
141 struct cfcnfg_phyinfo *phy;
142
143 list_for_each_entry_rcu(phy, &cnfg->phys, node)
144 if (phy->id == phyid)
145 return phy;
146 return NULL;
147}
148
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000149static void cfctrl_enum_resp(void)
150{
151}
152
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000153static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000154 enum cfcnfg_phy_preference phy_pref)
155{
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000156 /* Try to match with specified preference */
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000157 struct cfcnfg_phyinfo *phy;
158
159 list_for_each_entry_rcu(phy, &cnfg->phys, node) {
160 if (phy->up && phy->pref == phy_pref &&
161 phy->frm_layer != NULL)
162
163 return &phy->dev_info;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000164 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000165
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000166 /* Otherwise just return something */
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000167 list_for_each_entry_rcu(phy, &cnfg->phys, node)
168 if (phy->up)
169 return &phy->dev_info;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000170
171 return NULL;
172}
173
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000174static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000175{
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000176 struct cfcnfg_phyinfo *phy;
177
178 list_for_each_entry_rcu(phy, &cnfg->phys, node)
179 if (phy->ifindex == ifi && phy->up)
180 return phy->id;
André Carvalho de Matosf2527ec2010-11-01 11:52:47 +0000181 return -ENODEV;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000182}
183
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000184int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000185{
186 u8 channel_id = 0;
187 int ret = 0;
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000188 struct cflayer *servl = NULL;
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000189 struct cfcnfg *cfg = get_cfcnfg(net);
Dan Carpenter01a85902011-01-15 03:06:39 +0000190
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000191 caif_assert(adap_layer != NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000192
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000193 channel_id = adap_layer->id;
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000194 if (adap_layer->dn == NULL || channel_id == 0) {
Sjur Braendelandb04367d2010-09-21 11:44:44 +0000195 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000196 ret = -ENOTCONN;
197 goto end;
198 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000199
200 servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000201 if (servl == NULL) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000202 pr_err("PROTOCOL ERROR - "
203 "Error removing service_layer Channel_Id(%d)",
204 channel_id);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000205 ret = -EINVAL;
206 goto end;
207 }
208
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000209 ret = cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
210
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000211end:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000212 cfctrl_cancel_req(cfg->ctrl, adap_layer);
213
214 /* Do RCU sync before initiating cleanup */
215 synchronize_rcu();
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000216 if (adap_layer->ctrlcmd != NULL)
217 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000218 return ret;
219
220}
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000221EXPORT_SYMBOL(caif_disconnect_client);
Sjur Braendeland5b208652010-04-28 08:54:36 +0000222
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000223static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000224{
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000225}
226
Stephen Hemminger73d6ac62011-04-11 10:43:50 +0000227static const int protohead[CFCTRL_SRV_MASK] = {
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000228 [CFCTRL_SRV_VEI] = 4,
229 [CFCTRL_SRV_DATAGRAM] = 7,
230 [CFCTRL_SRV_UTIL] = 4,
231 [CFCTRL_SRV_RFM] = 3,
232 [CFCTRL_SRV_DBG] = 3,
233};
234
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000235
236static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
237 struct caif_connect_request *s,
238 struct cfctrl_link_param *l)
239{
240 struct dev_info *dev_info;
241 enum cfcnfg_phy_preference pref;
242 int res;
243
244 memset(l, 0, sizeof(*l));
245 /* In caif protocol low value is high priority */
246 l->priority = CAIF_PRIO_MAX - s->priority + 1;
247
248 if (s->ifindex != 0) {
249 res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
250 if (res < 0)
251 return res;
252 l->phyid = res;
253 } else {
254 switch (s->link_selector) {
255 case CAIF_LINK_HIGH_BANDW:
256 pref = CFPHYPREF_HIGH_BW;
257 break;
258 case CAIF_LINK_LOW_LATENCY:
259 pref = CFPHYPREF_LOW_LAT;
260 break;
261 default:
262 return -EINVAL;
263 }
264 dev_info = cfcnfg_get_phyid(cnfg, pref);
265 if (dev_info == NULL)
266 return -ENODEV;
267 l->phyid = dev_info->id;
268 }
269 switch (s->protocol) {
270 case CAIFPROTO_AT:
271 l->linktype = CFCTRL_SRV_VEI;
272 l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
273 l->chtype = s->sockaddr.u.at.type & 0x3;
274 break;
275 case CAIFPROTO_DATAGRAM:
276 l->linktype = CFCTRL_SRV_DATAGRAM;
277 l->chtype = 0x00;
278 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
279 break;
280 case CAIFPROTO_DATAGRAM_LOOP:
281 l->linktype = CFCTRL_SRV_DATAGRAM;
282 l->chtype = 0x03;
283 l->endpoint = 0x00;
284 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
285 break;
286 case CAIFPROTO_RFM:
287 l->linktype = CFCTRL_SRV_RFM;
288 l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
289 strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
290 sizeof(l->u.rfm.volume)-1);
291 l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0;
292 break;
293 case CAIFPROTO_UTIL:
294 l->linktype = CFCTRL_SRV_UTIL;
295 l->endpoint = 0x00;
296 l->chtype = 0x00;
297 strncpy(l->u.utility.name, s->sockaddr.u.util.service,
298 sizeof(l->u.utility.name)-1);
299 l->u.utility.name[sizeof(l->u.utility.name)-1] = 0;
300 caif_assert(sizeof(l->u.utility.name) > 10);
301 l->u.utility.paramlen = s->param.size;
302 if (l->u.utility.paramlen > sizeof(l->u.utility.params))
303 l->u.utility.paramlen = sizeof(l->u.utility.params);
304
305 memcpy(l->u.utility.params, s->param.data,
306 l->u.utility.paramlen);
307
308 break;
309 case CAIFPROTO_DEBUG:
310 l->linktype = CFCTRL_SRV_DBG;
311 l->endpoint = s->sockaddr.u.dbg.service;
312 l->chtype = s->sockaddr.u.dbg.type;
313 break;
314 default:
315 return -EINVAL;
316 }
317 return 0;
318}
319
320int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
321 struct cflayer *adap_layer, int *ifindex,
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000322 int *proto_head,
323 int *proto_tail)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000324{
325 struct cflayer *frml;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000326 struct cfcnfg_phyinfo *phy;
327 int err;
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000328 struct cfctrl_link_param param;
329 struct cfcnfg *cfg = get_cfcnfg(net);
330 caif_assert(cfg != NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000331
332 rcu_read_lock();
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000333 err = caif_connect_req_to_link_param(cfg, conn_req, &param);
334 if (err)
335 goto unlock;
336
337 phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000338 if (!phy) {
339 err = -ENODEV;
340 goto unlock;
341 }
342 err = -EINVAL;
343
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000344 if (adap_layer == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000345 pr_err("adap_layer is zero\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000346 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000347 }
348 if (adap_layer->receive == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000349 pr_err("adap_layer->receive is NULL\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000350 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000351 }
352 if (adap_layer->ctrlcmd == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000353 pr_err("adap_layer->ctrlcmd == NULL\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000354 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000355 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000356
357 err = -ENODEV;
358 frml = phy->frm_layer;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000359 if (frml == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000360 pr_err("Specified PHY type does not exist!\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000361 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000362 }
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000363 caif_assert(param.phyid == phy->id);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000364 caif_assert(phy->frm_layer->id ==
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000365 param.phyid);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000366 caif_assert(phy->phy_layer->id ==
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000367 param.phyid);
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000368
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000369 *ifindex = phy->ifindex;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000370 *proto_tail = 2;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000371 *proto_head =
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000372
373 protohead[param.linktype] + (phy->use_stx ? 1 : 0);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000374
375 rcu_read_unlock();
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000376
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000377 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000378 cfctrl_enum_req(cfg->ctrl, param.phyid);
379 return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000380
381unlock:
382 rcu_read_unlock();
383 return err;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000384}
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000385EXPORT_SYMBOL(caif_connect_client);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000386
Sjur Braendelande539d832010-04-28 08:54:35 +0000387static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000388 struct cflayer *adapt_layer)
389{
390 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
391 adapt_layer->ctrlcmd(adapt_layer,
392 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
393}
394
395static void
Sjur Braendelande539d832010-04-28 08:54:35 +0000396cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000397 u8 phyid, struct cflayer *adapt_layer)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000398{
399 struct cfcnfg *cnfg = container_obj(layer);
400 struct cflayer *servicel = NULL;
401 struct cfcnfg_phyinfo *phyinfo;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000402 struct net_device *netdev;
403
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000404 rcu_read_lock();
405
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000406 if (adapt_layer == NULL) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000407 pr_debug("link setup response but no client exist,"
408 "send linkdown back\n");
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000409 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000410 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000411 }
412
413 caif_assert(cnfg != NULL);
414 caif_assert(phyid != 0);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000415
416 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
417 if (phyinfo == NULL) {
418 pr_err("ERROR: Link Layer Device dissapeared"
419 "while connecting\n");
420 goto unlock;
421 }
422
423 caif_assert(phyinfo != NULL);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000424 caif_assert(phyinfo->id == phyid);
425 caif_assert(phyinfo->phy_layer != NULL);
426 caif_assert(phyinfo->phy_layer->id == phyid);
427
Sjur Braendelande539d832010-04-28 08:54:35 +0000428 adapt_layer->id = channel_id;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000429
430 switch (serv) {
431 case CFCTRL_SRV_VEI:
Sjur Braendelande539d832010-04-28 08:54:35 +0000432 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000433 break;
434 case CFCTRL_SRV_DATAGRAM:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000435 servicel = cfdgml_create(channel_id,
436 &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000437 break;
438 case CFCTRL_SRV_RFM:
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000439 netdev = phyinfo->dev_info.dev;
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000440 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000441 netdev->mtu);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000442 break;
443 case CFCTRL_SRV_UTIL:
Sjur Braendelande539d832010-04-28 08:54:35 +0000444 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000445 break;
446 case CFCTRL_SRV_VIDEO:
Sjur Braendelande539d832010-04-28 08:54:35 +0000447 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000448 break;
449 case CFCTRL_SRV_DBG:
Sjur Braendelande539d832010-04-28 08:54:35 +0000450 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000451 break;
452 default:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000453 pr_err("Protocol error. Link setup response "
454 "- unknown channel type\n");
455 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000456 }
457 if (!servicel) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000458 pr_warn("Out of memory\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000459 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000460 }
461 layer_set_dn(servicel, cnfg->mux);
Sjur Braendelande539d832010-04-28 08:54:35 +0000462 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000463 layer_set_up(servicel, adapt_layer);
464 layer_set_dn(adapt_layer, servicel);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000465
466 rcu_read_unlock();
467
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000468 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000469 return;
470unlock:
471 rcu_read_unlock();
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000472}
473
474void
475cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000476 struct net_device *dev, struct cflayer *phy_layer,
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000477 enum cfcnfg_phy_preference pref,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000478 bool fcs, bool stx)
479{
480 struct cflayer *frml;
481 struct cflayer *phy_driver = NULL;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000482 struct cfcnfg_phyinfo *phyinfo;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000483 int i;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000484 u8 phyid;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000485
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000486 mutex_lock(&cnfg->lock);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000487
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000488 /* CAIF protocol allow maximum 6 link-layers */
489 for (i = 0; i < 7; i++) {
490 phyid = (dev->ifindex + i) & 0x7;
491 if (phyid == 0)
492 continue;
493 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
494 goto got_phyid;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000495 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000496 pr_warn("Too many CAIF Link Layers (max 6)\n");
497 goto out;
498
499got_phyid:
500 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000501
502 switch (phy_type) {
503 case CFPHYTYPE_FRAG:
504 phy_driver =
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000505 cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000506 if (!phy_driver) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000507 pr_warn("Out of memory\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000508 goto out;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000509 }
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000510 break;
511 case CFPHYTYPE_CAIF:
512 phy_driver = NULL;
513 break;
514 default:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000515 goto out;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000516 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000517 phy_layer->id = phyid;
518 phyinfo->pref = pref;
519 phyinfo->id = phyid;
520 phyinfo->dev_info.id = phyid;
521 phyinfo->dev_info.dev = dev;
522 phyinfo->phy_layer = phy_layer;
523 phyinfo->ifindex = dev->ifindex;
524 phyinfo->use_stx = stx;
525 phyinfo->use_fcs = fcs;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000526
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000527 phy_layer->type = phy_type;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000528 frml = cffrml_create(phyid, fcs);
529
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000530 if (!frml) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000531 pr_warn("Out of memory\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000532 kfree(phyinfo);
533 goto out;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000534 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000535 phyinfo->frm_layer = frml;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000536 layer_set_up(frml, cnfg->mux);
537
538 if (phy_driver != NULL) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000539 phy_driver->id = phyid;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000540 layer_set_dn(frml, phy_driver);
541 layer_set_up(phy_driver, frml);
542 layer_set_dn(phy_driver, phy_layer);
543 layer_set_up(phy_layer, phy_driver);
544 } else {
545 layer_set_dn(frml, phy_layer);
546 layer_set_up(phy_layer, frml);
547 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000548
549 list_add_rcu(&phyinfo->node, &cnfg->phys);
550out:
551 mutex_unlock(&cnfg->lock);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000552}
553EXPORT_SYMBOL(cfcnfg_add_phy_layer);
554
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000555int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
556 bool up)
557{
558 struct cfcnfg_phyinfo *phyinfo;
559
560 rcu_read_lock();
561 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
562 if (phyinfo == NULL) {
563 rcu_read_unlock();
564 return -ENODEV;
565 }
566
567 if (phyinfo->up == up) {
568 rcu_read_unlock();
569 return 0;
570 }
571 phyinfo->up = up;
572
573 if (up) {
574 cffrml_hold(phyinfo->frm_layer);
575 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
576 phy_layer->id);
577 } else {
578 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
579 cffrml_put(phyinfo->frm_layer);
580 }
581
582 rcu_read_unlock();
583 return 0;
584}
585EXPORT_SYMBOL(cfcnfg_set_phy_state);
586
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000587int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
588{
589 struct cflayer *frml, *frml_dn;
590 u16 phyid;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000591 struct cfcnfg_phyinfo *phyinfo;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000592
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000593 might_sleep();
594
595 mutex_lock(&cnfg->lock);
596
597 phyid = phy_layer->id;
598 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
599
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000600 if (phyinfo == NULL) {
601 mutex_unlock(&cnfg->lock);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000602 return 0;
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000603 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000604 caif_assert(phyid == phyinfo->id);
605 caif_assert(phy_layer == phyinfo->phy_layer);
606 caif_assert(phy_layer->id == phyid);
607 caif_assert(phyinfo->frm_layer->id == phyid);
608
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000609 list_del_rcu(&phyinfo->node);
610 synchronize_rcu();
611
sjur.brandeland@stericsson.comcb3cb422011-05-13 02:44:02 +0000612 /* Fail if reference count is not zero */
613 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
614 pr_info("Wait for device inuse\n");
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000615 list_add_rcu(&phyinfo->node, &cnfg->phys);
sjur.brandeland@stericsson.comcb3cb422011-05-13 02:44:02 +0000616 mutex_unlock(&cnfg->lock);
617 return -EAGAIN;
618 }
619
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000620 frml = phyinfo->frm_layer;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000621 frml_dn = frml->dn;
622 cffrml_set_uplayer(frml, NULL);
623 cffrml_set_dnlayer(frml, NULL);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000624 if (phy_layer != frml_dn) {
625 layer_set_up(frml_dn, NULL);
626 layer_set_dn(frml_dn, NULL);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000627 }
628 layer_set_up(phy_layer, NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000629
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000630 if (phyinfo->phy_layer != frml_dn)
631 kfree(frml_dn);
632
sjur.brandeland@stericsson.comcb3cb422011-05-13 02:44:02 +0000633 cffrml_free(frml);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000634 kfree(phyinfo);
635 mutex_unlock(&cnfg->lock);
636
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000637 return 0;
638}
639EXPORT_SYMBOL(cfcnfg_del_phy_layer);