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                 return NULL;
  83         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;
  93         resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
  94         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;
  98         resp->linksetup_rsp = cfcnfg_linkup_rsp;
  99         resp->reject_rsp = cfcnfg_reject_rsp;
 100         INIT_LIST_HEAD(&this->phys);
 101
 102         cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
 103         layer_set_dn(this->ctrl, this->mux);
 104         layer_set_up(this->ctrl, this);
 105         mutex_init(&this->lock);
 106
 107         return this;
 108 out_of_mem:
 109         synchronize_rcu();
 110
 111         kfree(this->mux);
 112         kfree(this->ctrl);
 113         kfree(this);
 114         return NULL;
 115 }
 116
 117 void cfcnfg_remove(struct cfcnfg *cfg)
 118 {
 119         might_sleep();
 120         if (cfg) {
 121                 synchronize_rcu();
 122
 123                 kfree(cfg->mux);
 124                 cfctrl_remove(cfg->ctrl);
 125                 kfree(cfg);
 126         }
 127 }
 128
 129 static void cfctrl_resp_func(void)
 130 {
 131 }
 132
 133 static 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
 144 static void cfctrl_enum_resp(void)
 145 {
 146 }
 147
 148 static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
 149                                   enum cfcnfg_phy_preference phy_pref)
 150 {
 151         /* Try to match with specified preference */
 152         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;
 159         }
 160
 161         /* Otherwise just return something */
 162         list_for_each_entry_rcu(phy, &cnfg->phys, node)
 163                 if (phy->up)
 164                         return &phy->dev_info;
 165
 166         return NULL;
 167 }
 168
 169 static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
 170 {
 171         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;
 176         return -ENODEV;
 177 }
 178
 179 int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
 180 {
 181         u8 channel_id;
 182         struct cfcnfg *cfg = get_cfcnfg(net);
 183
 184         caif_assert(adap_layer != NULL);
 185         cfctrl_cancel_req(cfg->ctrl, adap_layer);
 186         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);
 195
 196         /* Do RCU sync before initiating cleanup */
 197         synchronize_rcu();
 198         if (adap_layer->ctrlcmd != NULL)
 199                 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
 200         return 0;
 201
 202 }
 203 EXPORT_SYMBOL(caif_disconnect_client);
 204
 205 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
 206 {
 207 }
 208
 209 static const int protohead[CFCTRL_SRV_MASK] = {
 210         [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
 217
 218 static 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
 302 int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
 303                         struct cflayer *adap_layer, int *ifindex,
 304                                 int *proto_head,
 305                                 int *proto_tail)
 306 {
 307         struct cflayer *frml;
 308         struct cfcnfg_phyinfo *phy;
 309         int err;
 310         struct cfctrl_link_param param;
 311         struct cfcnfg *cfg = get_cfcnfg(net);
 312         caif_assert(cfg != NULL);
 313
 314         rcu_read_lock();
 315         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);
 320         if (!phy) {
 321                 err = -ENODEV;
 322                 goto unlock;
 323         }
 324         err = -EINVAL;
 325
 326         if (adap_layer == NULL) {
 327                 pr_err("adap_layer is zero\n");
 328                 goto unlock;
 329         }
 330         if (adap_layer->receive == NULL) {
 331                 pr_err("adap_layer->receive is NULL\n");
 332                 goto unlock;
 333         }
 334         if (adap_layer->ctrlcmd == NULL) {
 335                 pr_err("adap_layer->ctrlcmd == NULL\n");
 336                 goto unlock;
 337         }
 338
 339         err = -ENODEV;
 340         frml = phy->frm_layer;
 341         if (frml == NULL) {
 342                 pr_err("Specified PHY type does not exist!\n");
 343                 goto unlock;
 344         }
 345         caif_assert(param.phyid == phy->id);
 346         caif_assert(phy->frm_layer->id ==
 347                      param.phyid);
 348         caif_assert(phy->phy_layer->id ==
 349                      param.phyid);
 350
 351         *ifindex = phy->ifindex;
 352         *proto_tail = 2;
 353         *proto_head =
 354
 355         protohead[param.linktype] + (phy->use_stx ? 1 : 0);
 356
 357         rcu_read_unlock();
 358
 359         /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
 360         cfctrl_enum_req(cfg->ctrl, param.phyid);
 361         return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
 362
 363 unlock:
 364         rcu_read_unlock();
 365         return err;
 366 }
 367 EXPORT_SYMBOL(caif_connect_client);
 368
 369 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
 370                              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
 377 static void
 378 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
 379                   u8 phyid, struct cflayer *adapt_layer)
 380 {
 381         struct cfcnfg *cnfg = container_obj(layer);
 382         struct cflayer *servicel = NULL;
 383         struct cfcnfg_phyinfo *phyinfo;
 384         struct net_device *netdev;
 385
 386         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
 394         rcu_read_lock();
 395
 396         if (adapt_layer == NULL) {
 397                 pr_debug("link setup response but no client exist,"
 398                                 "send linkdown back\n");
 399                 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
 400                 goto unlock;
 401         }
 402
 403         caif_assert(cnfg != NULL);
 404         caif_assert(phyid != 0);
 405
 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);
 414         caif_assert(phyinfo->id == phyid);
 415         caif_assert(phyinfo->phy_layer != NULL);
 416         caif_assert(phyinfo->phy_layer->id == phyid);
 417
 418         adapt_layer->id = channel_id;
 419
 420         switch (serv) {
 421         case CFCTRL_SRV_VEI:
 422                 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
 423                 break;
 424         case CFCTRL_SRV_DATAGRAM:
 425                 servicel = cfdgml_create(channel_id,
 426                                         &phyinfo->dev_info);
 427                 break;
 428         case CFCTRL_SRV_RFM:
 429                 netdev = phyinfo->dev_info.dev;
 430                 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
 431                                                 netdev->mtu);
 432                 break;
 433         case CFCTRL_SRV_UTIL:
 434                 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
 435                 break;
 436         case CFCTRL_SRV_VIDEO:
 437                 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
 438                 break;
 439         case CFCTRL_SRV_DBG:
 440                 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
 441                 break;
 442         default:
 443                 pr_err("Protocol error. Link setup response "
 444                                 "- unknown channel type\n");
 445                 goto unlock;
 446         }
 447         if (!servicel)
 448                 goto unlock;
 449         layer_set_dn(servicel, cnfg->mux);
 450         cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
 451         layer_set_up(servicel, adapt_layer);
 452         layer_set_dn(adapt_layer, servicel);
 453
 454         rcu_read_unlock();
 455
 456         servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
 457         return;
 458 unlock:
 459         rcu_read_unlock();
 460 }
 461
 462 void
 463 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
 464                      struct net_device *dev, struct cflayer *phy_layer,
 465                      enum cfcnfg_phy_preference pref,
 466                      bool fcs, bool stx)
 467 {
 468         struct cflayer *frml;
 469         struct cflayer *phy_driver = NULL;
 470         struct cfcnfg_phyinfo *phyinfo = NULL;
 471         int i;
 472         u8 phyid;
 473
 474         mutex_lock(&cnfg->lock);
 475
 476         /* 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;
 483         }
 484         pr_warn("Too many CAIF Link Layers (max 6)\n");
 485         goto out_err;
 486
 487 got_phyid:
 488         phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
 489         if (!phyinfo)
 490                 goto out_err;
 491
 492         switch (phy_type) {
 493         case CFPHYTYPE_FRAG:
 494                 phy_driver =
 495                     cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
 496                 if (!phy_driver)
 497                         goto out_err;
 498                 break;
 499         case CFPHYTYPE_CAIF:
 500                 phy_driver = NULL;
 501                 break;
 502         default:
 503                 goto out_err;
 504         }
 505         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;
 514
 515         frml = cffrml_create(phyid, fcs);
 516
 517         if (!frml)
 518                 goto out_err;
 519         phyinfo->frm_layer = frml;
 520         layer_set_up(frml, cnfg->mux);
 521
 522         if (phy_driver != NULL) {
 523                 phy_driver->id = phyid;
 524                 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         }
 532
 533         list_add_rcu(&phyinfo->node, &cnfg->phys);
 534         mutex_unlock(&cnfg->lock);
 535         return;
 536
 537 out_err:
 538         kfree(phy_driver);
 539         kfree(phyinfo);
 540         mutex_unlock(&cnfg->lock);
 541 }
 542 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
 543
 544 int 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 }
 574 EXPORT_SYMBOL(cfcnfg_set_phy_state);
 575
 576 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
 577 {
 578         struct cflayer *frml, *frml_dn;
 579         u16 phyid;
 580         struct cfcnfg_phyinfo *phyinfo;
 581
 582         might_sleep();
 583
 584         mutex_lock(&cnfg->lock);
 585
 586         phyid = phy_layer->id;
 587         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
 588
 589         if (phyinfo == NULL) {
 590                 mutex_unlock(&cnfg->lock);
 591                 return 0;
 592         }
 593         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
 598         list_del_rcu(&phyinfo->node);
 599         synchronize_rcu();
 600
 601         /* Fail if reference count is not zero */
 602         if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
 603                 pr_info("Wait for device inuse\n");
 604                 list_add_rcu(&phyinfo->node, &cnfg->phys);
 605                 mutex_unlock(&cnfg->lock);
 606                 return -EAGAIN;
 607         }
 608
 609         frml = phyinfo->frm_layer;
 610         frml_dn = frml->dn;
 611         cffrml_set_uplayer(frml, NULL);
 612         cffrml_set_dnlayer(frml, NULL);
 613         if (phy_layer != frml_dn) {
 614                 layer_set_up(frml_dn, NULL);
 615                 layer_set_dn(frml_dn, NULL);
 616         }
 617         layer_set_up(phy_layer, NULL);
 618
 619         if (phyinfo->phy_layer != frml_dn)
 620                 kfree(frml_dn);
 621
 622         cffrml_free(frml);
 623         kfree(phyinfo);
 624         mutex_unlock(&cnfg->lock);
 625
 626         return 0;
 627 }
 628 EXPORT_SYMBOL(cfcnfg_del_phy_layer);