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