1 /* 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 */ 6 7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ 8 9 #include <linux/kernel.h> 10 #include <linux/stddef.h> 11 #include <linux/slab.h> 12 #include <linux/netdevice.h> 13 #include <net/caif/caif_layer.h> 14 #include <net/caif/cfpkt.h> 15 #include <net/caif/cfcnfg.h> 16 #include <net/caif/cfctrl.h> 17 #include <net/caif/cfmuxl.h> 18 #include <net/caif/cffrml.h> 19 #include <net/caif/cfserl.h> 20 #include <net/caif/cfsrvl.h> 21 22 #include <linux/module.h> 23 #include <asm/atomic.h> 24 25 #define MAX_PHY_LAYERS 7 26 #define PHY_NAME_LEN 20 27 28 #define container_obj(layr) container_of(layr, struct cfcnfg, layer) 29 #define RFM_FRAGMENT_SIZE 4030 30 31 /* Information about CAIF physical interfaces held by Config Module in order 32 * to manage physical interfaces 33 */ 34 struct cfcnfg_phyinfo { 35 /* Pointer to the layer below the MUX (framing layer) */ 36 struct cflayer *frm_layer; 37 /* Pointer to the lowest actual physical layer */ 38 struct cflayer *phy_layer; 39 /* Unique identifier of the physical interface */ 40 unsigned int id; 41 /* Preference of the physical in interface */ 42 enum cfcnfg_phy_preference pref; 43 44 /* Reference count, number of channels using the device */ 45 int phy_ref_count; 46 47 /* Information about the physical device */ 48 struct dev_info dev_info; 49 50 /* Interface index */ 51 int ifindex; 52 53 /* Use Start of frame extension */ 54 bool use_stx; 55 56 /* Use Start of frame checksum */ 57 bool use_fcs; 58 }; 59 60 struct cfcnfg { 61 struct cflayer layer; 62 struct cflayer *ctrl; 63 struct cflayer *mux; 64 u8 last_phyid; 65 struct cfcnfg_phyinfo phy_layers[MAX_PHY_LAYERS]; 66 }; 67 68 static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, 69 enum cfctrl_srv serv, u8 phyid, 70 struct cflayer *adapt_layer); 71 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id); 72 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, 73 struct cflayer *adapt_layer); 74 static void cfctrl_resp_func(void); 75 static void cfctrl_enum_resp(void); 76 77 struct cfcnfg *cfcnfg_create(void) 78 { 79 struct cfcnfg *this; 80 struct cfctrl_rsp *resp; 81 /* Initiate this layer */ 82 this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC); 83 if (!this) { 84 pr_warn("Out of memory\n"); 85 return NULL; 86 } 87 this->mux = cfmuxl_create(); 88 if (!this->mux) 89 goto out_of_mem; 90 this->ctrl = cfctrl_create(); 91 if (!this->ctrl) 92 goto out_of_mem; 93 /* Initiate response functions */ 94 resp = cfctrl_get_respfuncs(this->ctrl); 95 resp->enum_rsp = cfctrl_enum_resp; 96 resp->linkerror_ind = cfctrl_resp_func; 97 resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp; 98 resp->sleep_rsp = cfctrl_resp_func; 99 resp->wake_rsp = cfctrl_resp_func; 100 resp->restart_rsp = cfctrl_resp_func; 101 resp->radioset_rsp = cfctrl_resp_func; 102 resp->linksetup_rsp = cfcnfg_linkup_rsp; 103 resp->reject_rsp = cfcnfg_reject_rsp; 104 105 this->last_phyid = 1; 106 107 cfmuxl_set_uplayer(this->mux, this->ctrl, 0); 108 layer_set_dn(this->ctrl, this->mux); 109 layer_set_up(this->ctrl, this); 110 return this; 111 out_of_mem: 112 pr_warn("Out of memory\n"); 113 kfree(this->mux); 114 kfree(this->ctrl); 115 kfree(this); 116 return NULL; 117 } 118 EXPORT_SYMBOL(cfcnfg_create); 119 120 void cfcnfg_remove(struct cfcnfg *cfg) 121 { 122 if (cfg) { 123 kfree(cfg->mux); 124 kfree(cfg->ctrl); 125 kfree(cfg); 126 } 127 } 128 129 static void cfctrl_resp_func(void) 130 { 131 } 132 133 static void cfctrl_enum_resp(void) 134 { 135 } 136 137 struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg, 138 enum cfcnfg_phy_preference phy_pref) 139 { 140 u16 i; 141 142 /* Try to match with specified preference */ 143 for (i = 1; i < MAX_PHY_LAYERS; i++) { 144 if (cnfg->phy_layers[i].id == i && 145 cnfg->phy_layers[i].pref == phy_pref && 146 cnfg->phy_layers[i].frm_layer != NULL) { 147 caif_assert(cnfg->phy_layers != NULL); 148 caif_assert(cnfg->phy_layers[i].id == i); 149 return &cnfg->phy_layers[i].dev_info; 150 } 151 } 152 /* Otherwise just return something */ 153 for (i = 1; i < MAX_PHY_LAYERS; i++) { 154 if (cnfg->phy_layers[i].id == i) { 155 caif_assert(cnfg->phy_layers != NULL); 156 caif_assert(cnfg->phy_layers[i].id == i); 157 return &cnfg->phy_layers[i].dev_info; 158 } 159 } 160 161 return NULL; 162 } 163 164 static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo(struct cfcnfg *cnfg, 165 u8 phyid) 166 { 167 int i; 168 /* Try to match with specified preference */ 169 for (i = 0; i < MAX_PHY_LAYERS; i++) 170 if (cnfg->phy_layers[i].frm_layer != NULL && 171 cnfg->phy_layers[i].id == phyid) 172 return &cnfg->phy_layers[i]; 173 return NULL; 174 } 175 176 177 int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi) 178 { 179 int i; 180 for (i = 0; i < MAX_PHY_LAYERS; i++) 181 if (cnfg->phy_layers[i].frm_layer != NULL && 182 cnfg->phy_layers[i].ifindex == ifi) 183 return i; 184 return -ENODEV; 185 } 186 187 int cfcnfg_disconn_adapt_layer(struct cfcnfg *cnfg, struct cflayer *adap_layer) 188 { 189 u8 channel_id = 0; 190 int ret = 0; 191 struct cflayer *servl = NULL; 192 struct cfcnfg_phyinfo *phyinfo = NULL; 193 u8 phyid = 0; 194 caif_assert(adap_layer != NULL); 195 channel_id = adap_layer->id; 196 if (adap_layer->dn == NULL || channel_id == 0) { 197 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n"); 198 ret = -ENOTCONN; 199 goto end; 200 } 201 servl = cfmuxl_remove_uplayer(cnfg->mux, channel_id); 202 if (servl == NULL) 203 goto end; 204 layer_set_up(servl, NULL); 205 ret = cfctrl_linkdown_req(cnfg->ctrl, channel_id, adap_layer); 206 if (servl == NULL) { 207 pr_err("PROTOCOL ERROR - Error removing service_layer Channel_Id(%d)", 208 channel_id); 209 ret = -EINVAL; 210 goto end; 211 } 212 caif_assert(channel_id == servl->id); 213 if (adap_layer->dn != NULL) { 214 phyid = cfsrvl_getphyid(adap_layer->dn); 215 216 phyinfo = cfcnfg_get_phyinfo(cnfg, phyid); 217 if (phyinfo == NULL) { 218 pr_warn("No interface to send disconnect to\n"); 219 ret = -ENODEV; 220 goto end; 221 } 222 if (phyinfo->id != phyid || 223 phyinfo->phy_layer->id != phyid || 224 phyinfo->frm_layer->id != phyid) { 225 pr_err("Inconsistency in phy registration\n"); 226 ret = -EINVAL; 227 goto end; 228 } 229 } 230 if (phyinfo != NULL && --phyinfo->phy_ref_count == 0 && 231 phyinfo->phy_layer != NULL && 232 phyinfo->phy_layer->modemcmd != NULL) { 233 phyinfo->phy_layer->modemcmd(phyinfo->phy_layer, 234 _CAIF_MODEMCMD_PHYIF_USELESS); 235 } 236 end: 237 cfsrvl_put(servl); 238 cfctrl_cancel_req(cnfg->ctrl, adap_layer); 239 if (adap_layer->ctrlcmd != NULL) 240 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0); 241 return ret; 242 243 } 244 EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer); 245 246 void cfcnfg_release_adap_layer(struct cflayer *adap_layer) 247 { 248 if (adap_layer->dn) 249 cfsrvl_put(adap_layer->dn); 250 } 251 EXPORT_SYMBOL(cfcnfg_release_adap_layer); 252 253 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id) 254 { 255 } 256 257 int protohead[CFCTRL_SRV_MASK] = { 258 [CFCTRL_SRV_VEI] = 4, 259 [CFCTRL_SRV_DATAGRAM] = 7, 260 [CFCTRL_SRV_UTIL] = 4, 261 [CFCTRL_SRV_RFM] = 3, 262 [CFCTRL_SRV_DBG] = 3, 263 }; 264 265 int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg, 266 struct cfctrl_link_param *param, 267 struct cflayer *adap_layer, 268 int *ifindex, 269 int *proto_head, 270 int *proto_tail) 271 { 272 struct cflayer *frml; 273 if (adap_layer == NULL) { 274 pr_err("adap_layer is zero\n"); 275 return -EINVAL; 276 } 277 if (adap_layer->receive == NULL) { 278 pr_err("adap_layer->receive is NULL\n"); 279 return -EINVAL; 280 } 281 if (adap_layer->ctrlcmd == NULL) { 282 pr_err("adap_layer->ctrlcmd == NULL\n"); 283 return -EINVAL; 284 } 285 frml = cnfg->phy_layers[param->phyid].frm_layer; 286 if (frml == NULL) { 287 pr_err("Specified PHY type does not exist!\n"); 288 return -ENODEV; 289 } 290 caif_assert(param->phyid == cnfg->phy_layers[param->phyid].id); 291 caif_assert(cnfg->phy_layers[param->phyid].frm_layer->id == 292 param->phyid); 293 caif_assert(cnfg->phy_layers[param->phyid].phy_layer->id == 294 param->phyid); 295 296 *ifindex = cnfg->phy_layers[param->phyid].ifindex; 297 *proto_head = 298 protohead[param->linktype]+ 299 (cnfg->phy_layers[param->phyid].use_stx ? 1 : 0); 300 301 *proto_tail = 2; 302 303 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */ 304 cfctrl_enum_req(cnfg->ctrl, param->phyid); 305 return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer); 306 } 307 EXPORT_SYMBOL(cfcnfg_add_adaptation_layer); 308 309 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, 310 struct cflayer *adapt_layer) 311 { 312 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL) 313 adapt_layer->ctrlcmd(adapt_layer, 314 CAIF_CTRLCMD_INIT_FAIL_RSP, 0); 315 } 316 317 static void 318 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv, 319 u8 phyid, struct cflayer *adapt_layer) 320 { 321 struct cfcnfg *cnfg = container_obj(layer); 322 struct cflayer *servicel = NULL; 323 struct cfcnfg_phyinfo *phyinfo; 324 struct net_device *netdev; 325 326 if (adapt_layer == NULL) { 327 pr_debug("link setup response but no client exist, send linkdown back\n"); 328 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL); 329 return; 330 } 331 332 caif_assert(cnfg != NULL); 333 caif_assert(phyid != 0); 334 phyinfo = &cnfg->phy_layers[phyid]; 335 caif_assert(phyinfo->id == phyid); 336 caif_assert(phyinfo->phy_layer != NULL); 337 caif_assert(phyinfo->phy_layer->id == phyid); 338 339 phyinfo->phy_ref_count++; 340 if (phyinfo->phy_ref_count == 1 && 341 phyinfo->phy_layer->modemcmd != NULL) { 342 phyinfo->phy_layer->modemcmd(phyinfo->phy_layer, 343 _CAIF_MODEMCMD_PHYIF_USEFULL); 344 } 345 adapt_layer->id = channel_id; 346 347 switch (serv) { 348 case CFCTRL_SRV_VEI: 349 servicel = cfvei_create(channel_id, &phyinfo->dev_info); 350 break; 351 case CFCTRL_SRV_DATAGRAM: 352 servicel = cfdgml_create(channel_id, &phyinfo->dev_info); 353 break; 354 case CFCTRL_SRV_RFM: 355 netdev = phyinfo->dev_info.dev; 356 servicel = cfrfml_create(channel_id, &phyinfo->dev_info, 357 netdev->mtu); 358 break; 359 case CFCTRL_SRV_UTIL: 360 servicel = cfutill_create(channel_id, &phyinfo->dev_info); 361 break; 362 case CFCTRL_SRV_VIDEO: 363 servicel = cfvidl_create(channel_id, &phyinfo->dev_info); 364 break; 365 case CFCTRL_SRV_DBG: 366 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info); 367 break; 368 default: 369 pr_err("Protocol error. Link setup response - unknown channel type\n"); 370 return; 371 } 372 if (!servicel) { 373 pr_warn("Out of memory\n"); 374 return; 375 } 376 layer_set_dn(servicel, cnfg->mux); 377 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id); 378 layer_set_up(servicel, adapt_layer); 379 layer_set_dn(adapt_layer, servicel); 380 cfsrvl_get(servicel); 381 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0); 382 } 383 384 void 385 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type, 386 struct net_device *dev, struct cflayer *phy_layer, 387 u16 *phyid, enum cfcnfg_phy_preference pref, 388 bool fcs, bool stx) 389 { 390 struct cflayer *frml; 391 struct cflayer *phy_driver = NULL; 392 int i; 393 394 395 if (cnfg->phy_layers[cnfg->last_phyid].frm_layer == NULL) { 396 *phyid = cnfg->last_phyid; 397 398 /* range: * 1..(MAX_PHY_LAYERS-1) */ 399 cnfg->last_phyid = 400 (cnfg->last_phyid % (MAX_PHY_LAYERS - 1)) + 1; 401 } else { 402 *phyid = 0; 403 for (i = 1; i < MAX_PHY_LAYERS; i++) { 404 if (cnfg->phy_layers[i].frm_layer == NULL) { 405 *phyid = i; 406 break; 407 } 408 } 409 } 410 if (*phyid == 0) { 411 pr_err("No Available PHY ID\n"); 412 return; 413 } 414 415 switch (phy_type) { 416 case CFPHYTYPE_FRAG: 417 phy_driver = 418 cfserl_create(CFPHYTYPE_FRAG, *phyid, stx); 419 if (!phy_driver) { 420 pr_warn("Out of memory\n"); 421 return; 422 } 423 424 break; 425 case CFPHYTYPE_CAIF: 426 phy_driver = NULL; 427 break; 428 default: 429 pr_err("%d\n", phy_type); 430 return; 431 break; 432 } 433 434 phy_layer->id = *phyid; 435 cnfg->phy_layers[*phyid].pref = pref; 436 cnfg->phy_layers[*phyid].id = *phyid; 437 cnfg->phy_layers[*phyid].dev_info.id = *phyid; 438 cnfg->phy_layers[*phyid].dev_info.dev = dev; 439 cnfg->phy_layers[*phyid].phy_layer = phy_layer; 440 cnfg->phy_layers[*phyid].phy_ref_count = 0; 441 cnfg->phy_layers[*phyid].ifindex = dev->ifindex; 442 cnfg->phy_layers[*phyid].use_stx = stx; 443 cnfg->phy_layers[*phyid].use_fcs = fcs; 444 445 phy_layer->type = phy_type; 446 frml = cffrml_create(*phyid, fcs); 447 if (!frml) { 448 pr_warn("Out of memory\n"); 449 return; 450 } 451 cnfg->phy_layers[*phyid].frm_layer = frml; 452 cfmuxl_set_dnlayer(cnfg->mux, frml, *phyid); 453 layer_set_up(frml, cnfg->mux); 454 455 if (phy_driver != NULL) { 456 phy_driver->id = *phyid; 457 layer_set_dn(frml, phy_driver); 458 layer_set_up(phy_driver, frml); 459 layer_set_dn(phy_driver, phy_layer); 460 layer_set_up(phy_layer, phy_driver); 461 } else { 462 layer_set_dn(frml, phy_layer); 463 layer_set_up(phy_layer, frml); 464 } 465 } 466 EXPORT_SYMBOL(cfcnfg_add_phy_layer); 467 468 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer) 469 { 470 struct cflayer *frml, *frml_dn; 471 u16 phyid; 472 phyid = phy_layer->id; 473 caif_assert(phyid == cnfg->phy_layers[phyid].id); 474 caif_assert(phy_layer == cnfg->phy_layers[phyid].phy_layer); 475 caif_assert(phy_layer->id == phyid); 476 caif_assert(cnfg->phy_layers[phyid].frm_layer->id == phyid); 477 478 memset(&cnfg->phy_layers[phy_layer->id], 0, 479 sizeof(struct cfcnfg_phyinfo)); 480 frml = cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id); 481 frml_dn = frml->dn; 482 cffrml_set_uplayer(frml, NULL); 483 cffrml_set_dnlayer(frml, NULL); 484 kfree(frml); 485 486 if (phy_layer != frml_dn) { 487 layer_set_up(frml_dn, NULL); 488 layer_set_dn(frml_dn, NULL); 489 kfree(frml_dn); 490 } 491 layer_set_up(phy_layer, NULL); 492 return 0; 493 } 494 EXPORT_SYMBOL(cfcnfg_del_phy_layer); 495