blob: 00523ecc4ced75ebaa1059c1eb3e0a324cae182e [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);
Joe Perches7ac2ed02011-08-25 13:22:24 +000081 if (!this)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000082 return NULL;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000083 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;
Sjur Braendelande539d832010-04-28 08:54:35 +000093 resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000094 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;
Sjur Braendelande539d832010-04-28 08:54:35 +000098 resp->linksetup_rsp = cfcnfg_linkup_rsp;
99 resp->reject_rsp = cfcnfg_reject_rsp;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000100 INIT_LIST_HEAD(&this->phys);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000101
102 cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
103 layer_set_dn(this->ctrl, this->mux);
104 layer_set_up(this->ctrl, this);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000105 mutex_init(&this->lock);
106
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000107 return this;
108out_of_mem:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000109 synchronize_rcu();
110
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000111 kfree(this->mux);
112 kfree(this->ctrl);
113 kfree(this);
114 return NULL;
115}
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000116
117void cfcnfg_remove(struct cfcnfg *cfg)
118{
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000119 might_sleep();
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000120 if (cfg) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000121 synchronize_rcu();
122
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000123 kfree(cfg->mux);
sjur.brandeland@stericsson.comc85c2952011-05-13 02:44:06 +0000124 cfctrl_remove(cfg->ctrl);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000125 kfree(cfg);
126 }
127}
128
129static void cfctrl_resp_func(void)
130{
131}
132
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000133static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
134 u8 phyid)
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
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000144static void cfctrl_enum_resp(void)
145{
146}
147
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000148static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000149 enum cfcnfg_phy_preference phy_pref)
150{
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000151 /* Try to match with specified preference */
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000152 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;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000159 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000160
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000161 /* Otherwise just return something */
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000162 list_for_each_entry_rcu(phy, &cnfg->phys, node)
163 if (phy->up)
164 return &phy->dev_info;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000165
166 return NULL;
167}
168
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000169static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000170{
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000171 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;
André Carvalho de Matosf2527ec2010-11-01 11:52:47 +0000176 return -ENODEV;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000177}
178
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000179int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000180{
sjur.brandeland@stericsson.com54e90fb2011-05-22 11:18:51 +0000181 u8 channel_id;
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000182 struct cfcnfg *cfg = get_cfcnfg(net);
Dan Carpenter01a85902011-01-15 03:06:39 +0000183
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000184 caif_assert(adap_layer != NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000185 cfctrl_cancel_req(cfg->ctrl, adap_layer);
sjur.brandeland@stericsson.com54e90fb2011-05-22 11:18:51 +0000186 channel_id = adap_layer->id;
187 if (channel_id != 0) {
188 struct cflayer *servl;
189 servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
190 if (servl != NULL)
191 layer_set_up(servl, NULL);
192 } else
193 pr_debug("nothing to disconnect\n");
194 cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000195
196 /* Do RCU sync before initiating cleanup */
197 synchronize_rcu();
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000198 if (adap_layer->ctrlcmd != NULL)
199 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
sjur.brandeland@stericsson.com54e90fb2011-05-22 11:18:51 +0000200 return 0;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000201
202}
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000203EXPORT_SYMBOL(caif_disconnect_client);
Sjur Braendeland5b208652010-04-28 08:54:36 +0000204
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000205static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000206{
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000207}
208
Stephen Hemminger73d6ac62011-04-11 10:43:50 +0000209static const int protohead[CFCTRL_SRV_MASK] = {
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000210 [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
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000217
218static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
219 struct caif_connect_request *s,
220 struct cfctrl_link_param *l)
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;
271 strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
272 sizeof(l->u.rfm.volume)-1);
273 l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0;
274 break;
275 case CAIFPROTO_UTIL:
276 l->linktype = CFCTRL_SRV_UTIL;
277 l->endpoint = 0x00;
278 l->chtype = 0x00;
279 strncpy(l->u.utility.name, s->sockaddr.u.util.service,
280 sizeof(l->u.utility.name)-1);
281 l->u.utility.name[sizeof(l->u.utility.name)-1] = 0;
282 caif_assert(sizeof(l->u.utility.name) > 10);
283 l->u.utility.paramlen = s->param.size;
284 if (l->u.utility.paramlen > sizeof(l->u.utility.params))
285 l->u.utility.paramlen = sizeof(l->u.utility.params);
286
287 memcpy(l->u.utility.params, s->param.data,
288 l->u.utility.paramlen);
289
290 break;
291 case CAIFPROTO_DEBUG:
292 l->linktype = CFCTRL_SRV_DBG;
293 l->endpoint = s->sockaddr.u.dbg.service;
294 l->chtype = s->sockaddr.u.dbg.type;
295 break;
296 default:
297 return -EINVAL;
298 }
299 return 0;
300}
301
302int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
303 struct cflayer *adap_layer, int *ifindex,
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000304 int *proto_head,
305 int *proto_tail)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000306{
307 struct cflayer *frml;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000308 struct cfcnfg_phyinfo *phy;
309 int err;
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000310 struct cfctrl_link_param param;
311 struct cfcnfg *cfg = get_cfcnfg(net);
312 caif_assert(cfg != NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000313
314 rcu_read_lock();
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000315 err = caif_connect_req_to_link_param(cfg, conn_req, &param);
316 if (err)
317 goto unlock;
318
319 phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000320 if (!phy) {
321 err = -ENODEV;
322 goto unlock;
323 }
324 err = -EINVAL;
325
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000326 if (adap_layer == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000327 pr_err("adap_layer is zero\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000328 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000329 }
330 if (adap_layer->receive == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000331 pr_err("adap_layer->receive is NULL\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000332 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000333 }
334 if (adap_layer->ctrlcmd == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000335 pr_err("adap_layer->ctrlcmd == NULL\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000336 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000337 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000338
339 err = -ENODEV;
340 frml = phy->frm_layer;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000341 if (frml == NULL) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000342 pr_err("Specified PHY type does not exist!\n");
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000343 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000344 }
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000345 caif_assert(param.phyid == phy->id);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000346 caif_assert(phy->frm_layer->id ==
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000347 param.phyid);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000348 caif_assert(phy->phy_layer->id ==
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000349 param.phyid);
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000350
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000351 *ifindex = phy->ifindex;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000352 *proto_tail = 2;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000353 *proto_head =
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000354
355 protohead[param.linktype] + (phy->use_stx ? 1 : 0);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000356
357 rcu_read_unlock();
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000358
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000359 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000360 cfctrl_enum_req(cfg->ctrl, param.phyid);
361 return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000362
363unlock:
364 rcu_read_unlock();
365 return err;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000366}
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000367EXPORT_SYMBOL(caif_connect_client);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000368
Sjur Braendelande539d832010-04-28 08:54:35 +0000369static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000370 struct cflayer *adapt_layer)
371{
372 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
373 adapt_layer->ctrlcmd(adapt_layer,
374 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
375}
376
377static void
Sjur Braendelande539d832010-04-28 08:54:35 +0000378cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000379 u8 phyid, struct cflayer *adapt_layer)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000380{
381 struct cfcnfg *cnfg = container_obj(layer);
382 struct cflayer *servicel = NULL;
383 struct cfcnfg_phyinfo *phyinfo;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000384 struct net_device *netdev;
385
sjur.brandeland@stericsson.com54e90fb2011-05-22 11:18:51 +0000386 if (channel_id == 0) {
387 pr_warn("received channel_id zero\n");
388 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
389 adapt_layer->ctrlcmd(adapt_layer,
390 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
391 return;
392 }
393
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000394 rcu_read_lock();
395
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000396 if (adapt_layer == NULL) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000397 pr_debug("link setup response but no client exist,"
398 "send linkdown back\n");
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000399 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000400 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000401 }
402
403 caif_assert(cnfg != NULL);
404 caif_assert(phyid != 0);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000405
406 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
407 if (phyinfo == NULL) {
408 pr_err("ERROR: Link Layer Device dissapeared"
409 "while connecting\n");
410 goto unlock;
411 }
412
413 caif_assert(phyinfo != NULL);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000414 caif_assert(phyinfo->id == phyid);
415 caif_assert(phyinfo->phy_layer != NULL);
416 caif_assert(phyinfo->phy_layer->id == phyid);
417
Sjur Braendelande539d832010-04-28 08:54:35 +0000418 adapt_layer->id = channel_id;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000419
420 switch (serv) {
421 case CFCTRL_SRV_VEI:
Sjur Braendelande539d832010-04-28 08:54:35 +0000422 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000423 break;
424 case CFCTRL_SRV_DATAGRAM:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000425 servicel = cfdgml_create(channel_id,
426 &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000427 break;
428 case CFCTRL_SRV_RFM:
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000429 netdev = phyinfo->dev_info.dev;
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000430 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000431 netdev->mtu);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000432 break;
433 case CFCTRL_SRV_UTIL:
Sjur Braendelande539d832010-04-28 08:54:35 +0000434 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000435 break;
436 case CFCTRL_SRV_VIDEO:
Sjur Braendelande539d832010-04-28 08:54:35 +0000437 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000438 break;
439 case CFCTRL_SRV_DBG:
Sjur Braendelande539d832010-04-28 08:54:35 +0000440 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000441 break;
442 default:
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000443 pr_err("Protocol error. Link setup response "
444 "- unknown channel type\n");
445 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000446 }
Joe Perches7ac2ed02011-08-25 13:22:24 +0000447 if (!servicel)
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000448 goto unlock;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000449 layer_set_dn(servicel, cnfg->mux);
Sjur Braendelande539d832010-04-28 08:54:35 +0000450 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000451 layer_set_up(servicel, adapt_layer);
452 layer_set_dn(adapt_layer, servicel);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000453
454 rcu_read_unlock();
455
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000456 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000457 return;
458unlock:
459 rcu_read_unlock();
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000460}
461
462void
463cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000464 struct net_device *dev, struct cflayer *phy_layer,
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000465 enum cfcnfg_phy_preference pref,
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000466 bool fcs, bool stx)
467{
468 struct cflayer *frml;
469 struct cflayer *phy_driver = NULL;
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000470 struct cfcnfg_phyinfo *phyinfo = NULL;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000471 int i;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000472 u8 phyid;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000473
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000474 mutex_lock(&cnfg->lock);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000475
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000476 /* CAIF protocol allow maximum 6 link-layers */
477 for (i = 0; i < 7; i++) {
478 phyid = (dev->ifindex + i) & 0x7;
479 if (phyid == 0)
480 continue;
481 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
482 goto got_phyid;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000483 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000484 pr_warn("Too many CAIF Link Layers (max 6)\n");
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000485 goto out_err;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000486
487got_phyid:
488 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000489 if (!phyinfo)
490 goto out_err;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000491
492 switch (phy_type) {
493 case CFPHYTYPE_FRAG:
494 phy_driver =
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000495 cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
Joe Perches7ac2ed02011-08-25 13:22:24 +0000496 if (!phy_driver)
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000497 goto out_err;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000498 break;
499 case CFPHYTYPE_CAIF:
500 phy_driver = NULL;
501 break;
502 default:
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000503 goto out_err;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000504 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000505 phy_layer->id = phyid;
506 phyinfo->pref = pref;
507 phyinfo->id = phyid;
508 phyinfo->dev_info.id = phyid;
509 phyinfo->dev_info.dev = dev;
510 phyinfo->phy_layer = phy_layer;
511 phyinfo->ifindex = dev->ifindex;
512 phyinfo->use_stx = stx;
513 phyinfo->use_fcs = fcs;
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000514
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000515 frml = cffrml_create(phyid, fcs);
516
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000517 if (!frml)
518 goto out_err;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000519 phyinfo->frm_layer = frml;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000520 layer_set_up(frml, cnfg->mux);
521
522 if (phy_driver != NULL) {
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000523 phy_driver->id = phyid;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000524 layer_set_dn(frml, phy_driver);
525 layer_set_up(phy_driver, frml);
526 layer_set_dn(phy_driver, phy_layer);
527 layer_set_up(phy_layer, phy_driver);
528 } else {
529 layer_set_dn(frml, phy_layer);
530 layer_set_up(phy_layer, frml);
531 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000532
533 list_add_rcu(&phyinfo->node, &cnfg->phys);
Dan Carpenter5bb20ed2011-09-20 21:21:59 +0000534 mutex_unlock(&cnfg->lock);
535 return;
536
537out_err:
538 kfree(phy_driver);
539 kfree(phyinfo);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000540 mutex_unlock(&cnfg->lock);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000541}
542EXPORT_SYMBOL(cfcnfg_add_phy_layer);
543
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000544int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
545 bool up)
546{
547 struct cfcnfg_phyinfo *phyinfo;
548
549 rcu_read_lock();
550 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
551 if (phyinfo == NULL) {
552 rcu_read_unlock();
553 return -ENODEV;
554 }
555
556 if (phyinfo->up == up) {
557 rcu_read_unlock();
558 return 0;
559 }
560 phyinfo->up = up;
561
562 if (up) {
563 cffrml_hold(phyinfo->frm_layer);
564 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
565 phy_layer->id);
566 } else {
567 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
568 cffrml_put(phyinfo->frm_layer);
569 }
570
571 rcu_read_unlock();
572 return 0;
573}
574EXPORT_SYMBOL(cfcnfg_set_phy_state);
575
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000576int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
577{
578 struct cflayer *frml, *frml_dn;
579 u16 phyid;
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000580 struct cfcnfg_phyinfo *phyinfo;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000581
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000582 might_sleep();
583
584 mutex_lock(&cnfg->lock);
585
586 phyid = phy_layer->id;
587 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
588
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000589 if (phyinfo == NULL) {
590 mutex_unlock(&cnfg->lock);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000591 return 0;
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000592 }
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000593 caif_assert(phyid == phyinfo->id);
594 caif_assert(phy_layer == phyinfo->phy_layer);
595 caif_assert(phy_layer->id == phyid);
596 caif_assert(phyinfo->frm_layer->id == phyid);
597
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000598 list_del_rcu(&phyinfo->node);
599 synchronize_rcu();
600
sjur.brandeland@stericsson.comcb3cb422011-05-13 02:44:02 +0000601 /* Fail if reference count is not zero */
602 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
603 pr_info("Wait for device inuse\n");
sjur.brandeland@stericsson.combee925d2011-05-13 02:44:05 +0000604 list_add_rcu(&phyinfo->node, &cnfg->phys);
sjur.brandeland@stericsson.comcb3cb422011-05-13 02:44:02 +0000605 mutex_unlock(&cnfg->lock);
606 return -EAGAIN;
607 }
608
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000609 frml = phyinfo->frm_layer;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000610 frml_dn = frml->dn;
611 cffrml_set_uplayer(frml, NULL);
612 cffrml_set_dnlayer(frml, NULL);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000613 if (phy_layer != frml_dn) {
614 layer_set_up(frml_dn, NULL);
615 layer_set_dn(frml_dn, NULL);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000616 }
617 layer_set_up(phy_layer, NULL);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000618
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000619 if (phyinfo->phy_layer != frml_dn)
620 kfree(frml_dn);
621
sjur.brandeland@stericsson.comcb3cb422011-05-13 02:44:02 +0000622 cffrml_free(frml);
sjur.brandeland@stericsson.comf3621442011-05-13 02:44:01 +0000623 kfree(phyinfo);
624 mutex_unlock(&cnfg->lock);
625
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000626 return 0;
627}
628EXPORT_SYMBOL(cfcnfg_del_phy_layer);