net/caif/cfcnfg.c

   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 <linux/module.h>
  14 #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>
  22 #include <net/caif/caif_dev.h>
  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         struct list_head node;
  31         bool up;
  32
  33         /* 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
  42         /* Information about the physical device */
  43         struct dev_info dev_info;
  44
  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;
  53 };
  54
  55 struct cfcnfg {
  56         struct cflayer layer;
  57         struct cflayer *ctrl;
  58         struct cflayer *mux;
  59         struct list_head phys;
  60         struct mutex lock;
  61 };
  62
  63 static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
  64                              enum cfctrl_srv serv, u8 phyid,
  65                              struct cflayer *adapt_layer);
  66 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
  67 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
  68                              struct cflayer *adapt_layer);
  69 static void cfctrl_resp_func(void);
  70 static void cfctrl_enum_resp(void);
  71
  72 struct cfcnfg *cfcnfg_create(void)
  73 {
  74         struct cfcnfg *this;
  75         struct cfctrl_rsp *resp;
  76
  77         might_sleep();
  78
  79         /* Initiate this layer */
  80         this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
  81         if (!this) {
  82                 pr_warn("Out of memory\n");
  83                 return NULL;
  84         }
  85         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;
  95         resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
  96         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;
 100         resp->linksetup_rsp = cfcnfg_linkup_rsp;
 101         resp->reject_rsp = cfcnfg_reject_rsp;
 102         INIT_LIST_HEAD(&this->phys);
 103
 104         cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
 105         layer_set_dn(this->ctrl, this->mux);
 106         layer_set_up(this->ctrl, this);
 107         mutex_init(&this->lock);
 108
 109         return this;
 110 out_of_mem:
 111         pr_warn("Out of memory\n");
 112
 113         synchronize_rcu();
 114
 115         kfree(this->mux);
 116         kfree(this->ctrl);
 117         kfree(this);
 118         return NULL;
 119 }
 120 EXPORT_SYMBOL(cfcnfg_create);
 121
 122 void cfcnfg_remove(struct cfcnfg *cfg)
 123 {
 124         might_sleep();
 125         if (cfg) {
 126                 synchronize_rcu();
 127
 128                 kfree(cfg->mux);
 129                 kfree(cfg->ctrl);
 130                 kfree(cfg);
 131         }
 132 }
 133
 134 static void cfctrl_resp_func(void)
 135 {
 136 }
 137
 138 static 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
 149 static void cfctrl_enum_resp(void)
 150 {
 151 }
 152
 153 struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
 154                                   enum cfcnfg_phy_preference phy_pref)
 155 {
 156         /* Try to match with specified preference */
 157         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;
 164         }
 165
 166         /* Otherwise just return something */
 167         list_for_each_entry_rcu(phy, &cnfg->phys, node)
 168                 if (phy->up)
 169                         return &phy->dev_info;
 170
 171         return NULL;
 172 }
 173
 174 int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
 175 {
 176         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;
 181         return -ENODEV;
 182 }
 183
 184 int cfcnfg_disconn_adapt_layer(struct cfcnfg *cfg, struct cflayer *adap_layer)
 185 {
 186         u8 channel_id = 0;
 187         int ret = 0;
 188         struct cflayer *servl = NULL;
 189
 190         caif_assert(adap_layer != NULL);
 191
 192         channel_id = adap_layer->id;
 193         if (adap_layer->dn == NULL || channel_id == 0) {
 194                 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n");
 195                 ret = -ENOTCONN;
 196                 goto end;
 197         }
 198
 199         servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
 200         if (servl == NULL) {
 201                 pr_err("PROTOCOL ERROR - "
 202                                 "Error removing service_layer Channel_Id(%d)",
 203                                 channel_id);
 204                 ret = -EINVAL;
 205                 goto end;
 206         }
 207
 208         ret = cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
 209
 210 end:
 211         cfctrl_cancel_req(cfg->ctrl, adap_layer);
 212
 213         /* Do RCU sync before initiating cleanup */
 214         synchronize_rcu();
 215         if (adap_layer->ctrlcmd != NULL)
 216                 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
 217         return ret;
 218
 219 }
 220 EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer);
 221
 222 void cfcnfg_release_adap_layer(struct cflayer *adap_layer)
 223 {
 224         if (adap_layer->dn)
 225                 cfsrvl_put(adap_layer->dn);
 226 }
 227 EXPORT_SYMBOL(cfcnfg_release_adap_layer);
 228
 229 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
 230 {
 231 }
 232
 233 static const int protohead[CFCTRL_SRV_MASK] = {
 234         [CFCTRL_SRV_VEI] = 4,
 235         [CFCTRL_SRV_DATAGRAM] = 7,
 236         [CFCTRL_SRV_UTIL] = 4,
 237         [CFCTRL_SRV_RFM] = 3,
 238         [CFCTRL_SRV_DBG] = 3,
 239 };
 240
 241 int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg,
 242                                 struct cfctrl_link_param *param,
 243                                 struct cflayer *adap_layer,
 244                                 int *ifindex,
 245                                 int *proto_head,
 246                                 int *proto_tail)
 247 {
 248         struct cflayer *frml;
 249         struct cfcnfg_phyinfo *phy;
 250         int err;
 251
 252         rcu_read_lock();
 253         phy = cfcnfg_get_phyinfo_rcu(cnfg, param->phyid);
 254         if (!phy) {
 255                 err = -ENODEV;
 256                 goto unlock;
 257         }
 258         err = -EINVAL;
 259
 260         if (adap_layer == NULL) {
 261                 pr_err("adap_layer is zero\n");
 262                 goto unlock;
 263         }
 264         if (adap_layer->receive == NULL) {
 265                 pr_err("adap_layer->receive is NULL\n");
 266                 goto unlock;
 267         }
 268         if (adap_layer->ctrlcmd == NULL) {
 269                 pr_err("adap_layer->ctrlcmd == NULL\n");
 270                 goto unlock;
 271         }
 272
 273         err = -ENODEV;
 274         frml = phy->frm_layer;
 275         if (frml == NULL) {
 276                 pr_err("Specified PHY type does not exist!\n");
 277                 goto unlock;
 278         }
 279         caif_assert(param->phyid == phy->id);
 280         caif_assert(phy->frm_layer->id ==
 281                      param->phyid);
 282         caif_assert(phy->phy_layer->id ==
 283                      param->phyid);
 284
 285         *ifindex = phy->ifindex;
 286         *proto_tail = 2;
 287         *proto_head =
 288                 protohead[param->linktype] + (phy->use_stx ? 1 : 0);
 289
 290         rcu_read_unlock();
 291
 292         /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
 293         cfctrl_enum_req(cnfg->ctrl, param->phyid);
 294         return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer);
 295
 296 unlock:
 297         rcu_read_unlock();
 298         return err;
 299 }
 300 EXPORT_SYMBOL(cfcnfg_add_adaptation_layer);
 301
 302 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
 303                              struct cflayer *adapt_layer)
 304 {
 305         if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
 306                 adapt_layer->ctrlcmd(adapt_layer,
 307                                      CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
 308 }
 309
 310 static void
 311 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
 312                   u8 phyid, struct cflayer *adapt_layer)
 313 {
 314         struct cfcnfg *cnfg = container_obj(layer);
 315         struct cflayer *servicel = NULL;
 316         struct cfcnfg_phyinfo *phyinfo;
 317         struct net_device *netdev;
 318
 319         rcu_read_lock();
 320
 321         if (adapt_layer == NULL) {
 322                 pr_debug("link setup response but no client exist,"
 323                                 "send linkdown back\n");
 324                 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
 325                 goto unlock;
 326         }
 327
 328         caif_assert(cnfg != NULL);
 329         caif_assert(phyid != 0);
 330
 331         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
 332         if (phyinfo == NULL) {
 333                 pr_err("ERROR: Link Layer Device dissapeared"
 334                                 "while connecting\n");
 335                 goto unlock;
 336         }
 337
 338         caif_assert(phyinfo != NULL);
 339         caif_assert(phyinfo->id == phyid);
 340         caif_assert(phyinfo->phy_layer != NULL);
 341         caif_assert(phyinfo->phy_layer->id == phyid);
 342
 343         adapt_layer->id = channel_id;
 344
 345         switch (serv) {
 346         case CFCTRL_SRV_VEI:
 347                 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
 348                 break;
 349         case CFCTRL_SRV_DATAGRAM:
 350                 servicel = cfdgml_create(channel_id,
 351                                         &phyinfo->dev_info);
 352                 break;
 353         case CFCTRL_SRV_RFM:
 354                 netdev = phyinfo->dev_info.dev;
 355                 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
 356                                                 netdev->mtu);
 357                 break;
 358         case CFCTRL_SRV_UTIL:
 359                 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
 360                 break;
 361         case CFCTRL_SRV_VIDEO:
 362                 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
 363                 break;
 364         case CFCTRL_SRV_DBG:
 365                 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
 366                 break;
 367         default:
 368                 pr_err("Protocol error. Link setup response "
 369                                 "- unknown channel type\n");
 370                 goto unlock;
 371         }
 372         if (!servicel) {
 373                 pr_warn("Out of memory\n");
 374                 goto unlock;
 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
 381         rcu_read_unlock();
 382
 383         servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
 384         return;
 385 unlock:
 386         rcu_read_unlock();
 387 }
 388
 389 void
 390 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
 391                      struct net_device *dev, struct cflayer *phy_layer,
 392                      u16 *phy_id, enum cfcnfg_phy_preference pref,
 393                      bool fcs, bool stx)
 394 {
 395         struct cflayer *frml;
 396         struct cflayer *phy_driver = NULL;
 397         struct cfcnfg_phyinfo *phyinfo;
 398         int i;
 399         u8 phyid;
 400
 401         mutex_lock(&cnfg->lock);
 402
 403         /* CAIF protocol allow maximum 6 link-layers */
 404         for (i = 0; i < 7; i++) {
 405                 phyid = (dev->ifindex + i) & 0x7;
 406                 if (phyid == 0)
 407                         continue;
 408                 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
 409                         goto got_phyid;
 410         }
 411         pr_warn("Too many CAIF Link Layers (max 6)\n");
 412         goto out;
 413
 414 got_phyid:
 415         phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
 416
 417         switch (phy_type) {
 418         case CFPHYTYPE_FRAG:
 419                 phy_driver =
 420                     cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
 421                 if (!phy_driver) {
 422                         pr_warn("Out of memory\n");
 423                         goto out;
 424                 }
 425                 break;
 426         case CFPHYTYPE_CAIF:
 427                 phy_driver = NULL;
 428                 break;
 429         default:
 430                 goto out;
 431         }
 432         phy_layer->id = phyid;
 433         phyinfo->pref = pref;
 434         phyinfo->id = phyid;
 435         phyinfo->dev_info.id = phyid;
 436         phyinfo->dev_info.dev = dev;
 437         phyinfo->phy_layer = phy_layer;
 438         phyinfo->ifindex = dev->ifindex;
 439         phyinfo->use_stx = stx;
 440         phyinfo->use_fcs = fcs;
 441
 442         phy_layer->type = phy_type;
 443         frml = cffrml_create(phyid, fcs);
 444
 445         if (!frml) {
 446                 pr_warn("Out of memory\n");
 447                 kfree(phyinfo);
 448                 goto out;
 449         }
 450         phyinfo->frm_layer = frml;
 451         layer_set_up(frml, cnfg->mux);
 452
 453         if (phy_driver != NULL) {
 454                 phy_driver->id = phyid;
 455                 layer_set_dn(frml, phy_driver);
 456                 layer_set_up(phy_driver, frml);
 457                 layer_set_dn(phy_driver, phy_layer);
 458                 layer_set_up(phy_layer, phy_driver);
 459         } else {
 460                 layer_set_dn(frml, phy_layer);
 461                 layer_set_up(phy_layer, frml);
 462         }
 463
 464         list_add_rcu(&phyinfo->node, &cnfg->phys);
 465 out:
 466         mutex_unlock(&cnfg->lock);
 467 }
 468 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
 469
 470 int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
 471                 bool up)
 472 {
 473         struct cfcnfg_phyinfo *phyinfo;
 474
 475         rcu_read_lock();
 476         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
 477         if (phyinfo == NULL) {
 478                 rcu_read_unlock();
 479                 return -ENODEV;
 480         }
 481
 482         if (phyinfo->up == up) {
 483                 rcu_read_unlock();
 484                 return 0;
 485         }
 486         phyinfo->up = up;
 487
 488         if (up) {
 489                 cffrml_hold(phyinfo->frm_layer);
 490                 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
 491                                         phy_layer->id);
 492         } else {
 493                 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
 494                 cffrml_put(phyinfo->frm_layer);
 495         }
 496
 497         rcu_read_unlock();
 498         return 0;
 499 }
 500 EXPORT_SYMBOL(cfcnfg_set_phy_state);
 501
 502 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
 503 {
 504         struct cflayer *frml, *frml_dn;
 505         u16 phyid;
 506         struct cfcnfg_phyinfo *phyinfo;
 507
 508         might_sleep();
 509
 510         mutex_lock(&cnfg->lock);
 511
 512         phyid = phy_layer->id;
 513         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
 514
 515         if (phyinfo == NULL)
 516                 return 0;
 517         caif_assert(phyid == phyinfo->id);
 518         caif_assert(phy_layer == phyinfo->phy_layer);
 519         caif_assert(phy_layer->id == phyid);
 520         caif_assert(phyinfo->frm_layer->id == phyid);
 521
 522         list_del_rcu(&phyinfo->node);
 523         synchronize_rcu();
 524
 525         frml = phyinfo->frm_layer;
 526         frml_dn = frml->dn;
 527         cffrml_set_uplayer(frml, NULL);
 528         cffrml_set_dnlayer(frml, NULL);
 529         if (phy_layer != frml_dn) {
 530                 layer_set_up(frml_dn, NULL);
 531                 layer_set_dn(frml_dn, NULL);
 532         }
 533         layer_set_up(phy_layer, NULL);
 534
 535
 536
 537         if (phyinfo->phy_layer != frml_dn)
 538                 kfree(frml_dn);
 539
 540         kfree(frml);
 541         kfree(phyinfo);
 542         mutex_unlock(&cnfg->lock);
 543
 544         return 0;
 545 }
 546 EXPORT_SYMBOL(cfcnfg_del_phy_layer);