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 int cfcnfg_get_named(struct cfcnfg *cnfg, char *name) 177 { 178 int i; 179 180 /* Try to match with specified name */ 181 for (i = 0; i < MAX_PHY_LAYERS; i++) { 182 if (cnfg->phy_layers[i].frm_layer != NULL 183 && strcmp(cnfg->phy_layers[i].phy_layer->name, 184 name) == 0) 185 return cnfg->phy_layers[i].frm_layer->id; 186 } 187 return 0; 188 } 189 190 int cfcnfg_disconn_adapt_layer(struct cfcnfg *cnfg, struct cflayer *adap_layer) 191 { 192 u8 channel_id = 0; 193 int ret = 0; 194 struct cflayer *servl = NULL; 195 struct cfcnfg_phyinfo *phyinfo = NULL; 196 u8 phyid = 0; 197 caif_assert(adap_layer != NULL); 198 channel_id = adap_layer->id; 199 if (adap_layer->dn == NULL || channel_id == 0) { 200 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n"); 201 ret = -ENOTCONN; 202 goto end; 203 } 204 servl = cfmuxl_remove_uplayer(cnfg->mux, channel_id); 205 if (servl == NULL) 206 goto end; 207 layer_set_up(servl, NULL); 208 ret = cfctrl_linkdown_req(cnfg->ctrl, channel_id, adap_layer); 209 if (servl == NULL) { 210 pr_err("PROTOCOL ERROR - Error removing service_layer Channel_Id(%d)", 211 channel_id); 212 ret = -EINVAL; 213 goto end; 214 } 215 caif_assert(channel_id == servl->id); 216 if (adap_layer->dn != NULL) { 217 phyid = cfsrvl_getphyid(adap_layer->dn); 218 219 phyinfo = cfcnfg_get_phyinfo(cnfg, phyid); 220 if (phyinfo == NULL) { 221 pr_warn("No interface to send disconnect to\n"); 222 ret = -ENODEV; 223 goto end; 224 } 225 if (phyinfo->id != phyid || 226 phyinfo->phy_layer->id != phyid || 227 phyinfo->frm_layer->id != phyid) { 228 pr_err("Inconsistency in phy registration\n"); 229 ret = -EINVAL; 230 goto end; 231 } 232 } 233 if (phyinfo != NULL && --phyinfo->phy_ref_count == 0 && 234 phyinfo->phy_layer != NULL && 235 phyinfo->phy_layer->modemcmd != NULL) { 236 phyinfo->phy_layer->modemcmd(phyinfo->phy_layer, 237 _CAIF_MODEMCMD_PHYIF_USELESS); 238 } 239 end: 240 cfsrvl_put(servl); 241 cfctrl_cancel_req(cnfg->ctrl, adap_layer); 242 if (adap_layer->ctrlcmd != NULL) 243 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0); 244 return ret; 245 246 } 247 EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer); 248 249 void cfcnfg_release_adap_layer(struct cflayer *adap_layer) 250 { 251 if (adap_layer->dn) 252 cfsrvl_put(adap_layer->dn); 253 } 254 EXPORT_SYMBOL(cfcnfg_release_adap_layer); 255 256 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id) 257 { 258 } 259 260 int protohead[CFCTRL_SRV_MASK] = { 261 [CFCTRL_SRV_VEI] = 4, 262 [CFCTRL_SRV_DATAGRAM] = 7, 263 [CFCTRL_SRV_UTIL] = 4, 264 [CFCTRL_SRV_RFM] = 3, 265 [CFCTRL_SRV_DBG] = 3, 266 }; 267 268 int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg, 269 struct cfctrl_link_param *param, 270 struct cflayer *adap_layer, 271 int *ifindex, 272 int *proto_head, 273 int *proto_tail) 274 { 275 struct cflayer *frml; 276 if (adap_layer == NULL) { 277 pr_err("adap_layer is zero\n"); 278 return -EINVAL; 279 } 280 if (adap_layer->receive == NULL) { 281 pr_err("adap_layer->receive is NULL\n"); 282 return -EINVAL; 283 } 284 if (adap_layer->ctrlcmd == NULL) { 285 pr_err("adap_layer->ctrlcmd == NULL\n"); 286 return -EINVAL; 287 } 288 frml = cnfg->phy_layers[param->phyid].frm_layer; 289 if (frml == NULL) { 290 pr_err("Specified PHY type does not exist!\n"); 291 return -ENODEV; 292 } 293 caif_assert(param->phyid == cnfg->phy_layers[param->phyid].id); 294 caif_assert(cnfg->phy_layers[param->phyid].frm_layer->id == 295 param->phyid); 296 caif_assert(cnfg->phy_layers[param->phyid].phy_layer->id == 297 param->phyid); 298 299 *ifindex = cnfg->phy_layers[param->phyid].ifindex; 300 *proto_head = 301 protohead[param->linktype]+ 302 (cnfg->phy_layers[param->phyid].use_stx ? 1 : 0); 303 304 *proto_tail = 2; 305 306 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */ 307 cfctrl_enum_req(cnfg->ctrl, param->phyid); 308 return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer); 309 } 310 EXPORT_SYMBOL(cfcnfg_add_adaptation_layer); 311 312 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, 313 struct cflayer *adapt_layer) 314 { 315 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL) 316 adapt_layer->ctrlcmd(adapt_layer, 317 CAIF_CTRLCMD_INIT_FAIL_RSP, 0); 318 } 319 320 static void 321 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv, 322 u8 phyid, struct cflayer *adapt_layer) 323 { 324 struct cfcnfg *cnfg = container_obj(layer); 325 struct cflayer *servicel = NULL; 326 struct cfcnfg_phyinfo *phyinfo; 327 struct net_device *netdev; 328 329 if (adapt_layer == NULL) { 330 pr_debug("link setup response but no client exist, send linkdown back\n"); 331 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL); 332 return; 333 } 334 335 caif_assert(cnfg != NULL); 336 caif_assert(phyid != 0); 337 phyinfo = &cnfg->phy_layers[phyid]; 338 caif_assert(phyinfo->id == phyid); 339 caif_assert(phyinfo->phy_layer != NULL); 340 caif_assert(phyinfo->phy_layer->id == phyid); 341 342 phyinfo->phy_ref_count++; 343 if (phyinfo->phy_ref_count == 1 && 344 phyinfo->phy_layer->modemcmd != NULL) { 345 phyinfo->phy_layer->modemcmd(phyinfo->phy_layer, 346 _CAIF_MODEMCMD_PHYIF_USEFULL); 347 } 348 adapt_layer->id = channel_id; 349 350 switch (serv) { 351 case CFCTRL_SRV_VEI: 352 servicel = cfvei_create(channel_id, &phyinfo->dev_info); 353 break; 354 case CFCTRL_SRV_DATAGRAM: 355 servicel = cfdgml_create(channel_id, &phyinfo->dev_info); 356 break; 357 case CFCTRL_SRV_RFM: 358 netdev = phyinfo->dev_info.dev; 359 servicel = cfrfml_create(channel_id, &phyinfo->dev_info, 360 netdev->mtu); 361 break; 362 case CFCTRL_SRV_UTIL: 363 servicel = cfutill_create(channel_id, &phyinfo->dev_info); 364 break; 365 case CFCTRL_SRV_VIDEO: 366 servicel = cfvidl_create(channel_id, &phyinfo->dev_info); 367 break; 368 case CFCTRL_SRV_DBG: 369 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info); 370 break; 371 default: 372 pr_err("Protocol error. Link setup response - unknown channel type\n"); 373 return; 374 } 375 if (!servicel) { 376 pr_warn("Out of memory\n"); 377 return; 378 } 379 layer_set_dn(servicel, cnfg->mux); 380 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id); 381 layer_set_up(servicel, adapt_layer); 382 layer_set_dn(adapt_layer, servicel); 383 cfsrvl_get(servicel); 384 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0); 385 } 386 387 void 388 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type, 389 struct net_device *dev, struct cflayer *phy_layer, 390 u16 *phyid, enum cfcnfg_phy_preference pref, 391 bool fcs, bool stx) 392 { 393 struct cflayer *frml; 394 struct cflayer *phy_driver = NULL; 395 int i; 396 397 398 if (cnfg->phy_layers[cnfg->last_phyid].frm_layer == NULL) { 399 *phyid = cnfg->last_phyid; 400 401 /* range: * 1..(MAX_PHY_LAYERS-1) */ 402 cnfg->last_phyid = 403 (cnfg->last_phyid % (MAX_PHY_LAYERS - 1)) + 1; 404 } else { 405 *phyid = 0; 406 for (i = 1; i < MAX_PHY_LAYERS; i++) { 407 if (cnfg->phy_layers[i].frm_layer == NULL) { 408 *phyid = i; 409 break; 410 } 411 } 412 } 413 if (*phyid == 0) { 414 pr_err("No Available PHY ID\n"); 415 return; 416 } 417 418 switch (phy_type) { 419 case CFPHYTYPE_FRAG: 420 phy_driver = 421 cfserl_create(CFPHYTYPE_FRAG, *phyid, stx); 422 if (!phy_driver) { 423 pr_warn("Out of memory\n"); 424 return; 425 } 426 427 break; 428 case CFPHYTYPE_CAIF: 429 phy_driver = NULL; 430 break; 431 default: 432 pr_err("%d\n", phy_type); 433 return; 434 break; 435 } 436 437 phy_layer->id = *phyid; 438 cnfg->phy_layers[*phyid].pref = pref; 439 cnfg->phy_layers[*phyid].id = *phyid; 440 cnfg->phy_layers[*phyid].dev_info.id = *phyid; 441 cnfg->phy_layers[*phyid].dev_info.dev = dev; 442 cnfg->phy_layers[*phyid].phy_layer = phy_layer; 443 cnfg->phy_layers[*phyid].phy_ref_count = 0; 444 cnfg->phy_layers[*phyid].ifindex = dev->ifindex; 445 cnfg->phy_layers[*phyid].use_stx = stx; 446 cnfg->phy_layers[*phyid].use_fcs = fcs; 447 448 phy_layer->type = phy_type; 449 frml = cffrml_create(*phyid, fcs); 450 if (!frml) { 451 pr_warn("Out of memory\n"); 452 return; 453 } 454 cnfg->phy_layers[*phyid].frm_layer = frml; 455 cfmuxl_set_dnlayer(cnfg->mux, frml, *phyid); 456 layer_set_up(frml, cnfg->mux); 457 458 if (phy_driver != NULL) { 459 phy_driver->id = *phyid; 460 layer_set_dn(frml, phy_driver); 461 layer_set_up(phy_driver, frml); 462 layer_set_dn(phy_driver, phy_layer); 463 layer_set_up(phy_layer, phy_driver); 464 } else { 465 layer_set_dn(frml, phy_layer); 466 layer_set_up(phy_layer, frml); 467 } 468 } 469 EXPORT_SYMBOL(cfcnfg_add_phy_layer); 470 471 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer) 472 { 473 struct cflayer *frml, *frml_dn; 474 u16 phyid; 475 phyid = phy_layer->id; 476 caif_assert(phyid == cnfg->phy_layers[phyid].id); 477 caif_assert(phy_layer == cnfg->phy_layers[phyid].phy_layer); 478 caif_assert(phy_layer->id == phyid); 479 caif_assert(cnfg->phy_layers[phyid].frm_layer->id == phyid); 480 481 memset(&cnfg->phy_layers[phy_layer->id], 0, 482 sizeof(struct cfcnfg_phyinfo)); 483 frml = cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id); 484 frml_dn = frml->dn; 485 cffrml_set_uplayer(frml, NULL); 486 cffrml_set_dnlayer(frml, NULL); 487 kfree(frml); 488 489 if (phy_layer != frml_dn) { 490 layer_set_up(frml_dn, NULL); 491 layer_set_dn(frml_dn, NULL); 492 kfree(frml_dn); 493 } 494 layer_set_up(phy_layer, NULL); 495 return 0; 496 } 497 EXPORT_SYMBOL(cfcnfg_del_phy_layer); 498