linux/net/caif/cfcnfg.c
<<
>>
Prefs
   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 */
  29struct 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        /* Protocol head room added for CAIF link layer */
  49        int head_room;
  50
  51        /* Use Start of frame checksum */
  52        bool use_fcs;
  53};
  54
  55struct cfcnfg {
  56        struct cflayer layer;
  57        struct cflayer *ctrl;
  58        struct cflayer *mux;
  59        struct list_head phys;
  60        struct mutex lock;
  61};
  62
  63static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
  64                             enum cfctrl_srv serv, u8 phyid,
  65                             struct cflayer *adapt_layer);
  66static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
  67static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
  68                             struct cflayer *adapt_layer);
  69static void cfctrl_resp_func(void);
  70static void cfctrl_enum_resp(void);
  71
  72struct 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;
 108out_of_mem:
 109        synchronize_rcu();
 110
 111        kfree(this->mux);
 112        kfree(this->ctrl);
 113        kfree(this);
 114        return NULL;
 115}
 116
 117void 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
 129static void cfctrl_resp_func(void)
 130{
 131}
 132
 133static 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
 144static void cfctrl_enum_resp(void)
 145{
 146}
 147
 148static 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
 169static 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
 179int 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                cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
 191                if (servl != NULL)
 192                        layer_set_up(servl, NULL);
 193        } else
 194                pr_debug("nothing to disconnect\n");
 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}
 203EXPORT_SYMBOL(caif_disconnect_client);
 204
 205static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
 206{
 207}
 208
 209static 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
 218static 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
 302int 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
 313        rcu_read_lock();
 314        err = caif_connect_req_to_link_param(cfg, conn_req, &param);
 315        if (err)
 316                goto unlock;
 317
 318        phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
 319        if (!phy) {
 320                err = -ENODEV;
 321                goto unlock;
 322        }
 323        err = -EINVAL;
 324
 325        if (adap_layer == NULL) {
 326                pr_err("adap_layer is zero\n");
 327                goto unlock;
 328        }
 329        if (adap_layer->receive == NULL) {
 330                pr_err("adap_layer->receive is NULL\n");
 331                goto unlock;
 332        }
 333        if (adap_layer->ctrlcmd == NULL) {
 334                pr_err("adap_layer->ctrlcmd == NULL\n");
 335                goto unlock;
 336        }
 337
 338        err = -ENODEV;
 339        frml = phy->frm_layer;
 340        if (frml == NULL) {
 341                pr_err("Specified PHY type does not exist!\n");
 342                goto unlock;
 343        }
 344        caif_assert(param.phyid == phy->id);
 345        caif_assert(phy->frm_layer->id ==
 346                     param.phyid);
 347        caif_assert(phy->phy_layer->id ==
 348                     param.phyid);
 349
 350        *ifindex = phy->ifindex;
 351        *proto_tail = 2;
 352        *proto_head = protohead[param.linktype] + phy->head_room;
 353
 354        rcu_read_unlock();
 355
 356        /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
 357        cfctrl_enum_req(cfg->ctrl, param.phyid);
 358        return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
 359
 360unlock:
 361        rcu_read_unlock();
 362        return err;
 363}
 364EXPORT_SYMBOL(caif_connect_client);
 365
 366static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
 367                             struct cflayer *adapt_layer)
 368{
 369        if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
 370                adapt_layer->ctrlcmd(adapt_layer,
 371                                     CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
 372}
 373
 374static void
 375cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
 376                  u8 phyid, struct cflayer *adapt_layer)
 377{
 378        struct cfcnfg *cnfg = container_obj(layer);
 379        struct cflayer *servicel = NULL;
 380        struct cfcnfg_phyinfo *phyinfo;
 381        struct net_device *netdev;
 382
 383        if (channel_id == 0) {
 384                pr_warn("received channel_id zero\n");
 385                if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
 386                        adapt_layer->ctrlcmd(adapt_layer,
 387                                                CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
 388                return;
 389        }
 390
 391        rcu_read_lock();
 392
 393        if (adapt_layer == NULL) {
 394                pr_debug("link setup response but no client exist,"
 395                                "send linkdown back\n");
 396                cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
 397                goto unlock;
 398        }
 399
 400        caif_assert(cnfg != NULL);
 401        caif_assert(phyid != 0);
 402
 403        phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
 404        if (phyinfo == NULL) {
 405                pr_err("ERROR: Link Layer Device dissapeared"
 406                                "while connecting\n");
 407                goto unlock;
 408        }
 409
 410        caif_assert(phyinfo != NULL);
 411        caif_assert(phyinfo->id == phyid);
 412        caif_assert(phyinfo->phy_layer != NULL);
 413        caif_assert(phyinfo->phy_layer->id == phyid);
 414
 415        adapt_layer->id = channel_id;
 416
 417        switch (serv) {
 418        case CFCTRL_SRV_VEI:
 419                servicel = cfvei_create(channel_id, &phyinfo->dev_info);
 420                break;
 421        case CFCTRL_SRV_DATAGRAM:
 422                servicel = cfdgml_create(channel_id,
 423                                        &phyinfo->dev_info);
 424                break;
 425        case CFCTRL_SRV_RFM:
 426                netdev = phyinfo->dev_info.dev;
 427                servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
 428                                                netdev->mtu);
 429                break;
 430        case CFCTRL_SRV_UTIL:
 431                servicel = cfutill_create(channel_id, &phyinfo->dev_info);
 432                break;
 433        case CFCTRL_SRV_VIDEO:
 434                servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
 435                break;
 436        case CFCTRL_SRV_DBG:
 437                servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
 438                break;
 439        default:
 440                pr_err("Protocol error. Link setup response "
 441                                "- unknown channel type\n");
 442                goto unlock;
 443        }
 444        if (!servicel)
 445                goto unlock;
 446        layer_set_dn(servicel, cnfg->mux);
 447        cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
 448        layer_set_up(servicel, adapt_layer);
 449        layer_set_dn(adapt_layer, servicel);
 450
 451        rcu_read_unlock();
 452
 453        servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
 454        return;
 455unlock:
 456        rcu_read_unlock();
 457}
 458
 459void
 460cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
 461                     struct net_device *dev, struct cflayer *phy_layer,
 462                     enum cfcnfg_phy_preference pref,
 463                     struct cflayer *link_support,
 464                     bool fcs, int head_room)
 465{
 466        struct cflayer *frml;
 467        struct cfcnfg_phyinfo *phyinfo = NULL;
 468        int i;
 469        u8 phyid;
 470
 471        mutex_lock(&cnfg->lock);
 472
 473        /* CAIF protocol allow maximum 6 link-layers */
 474        for (i = 0; i < 7; i++) {
 475                phyid = (dev->ifindex + i) & 0x7;
 476                if (phyid == 0)
 477                        continue;
 478                if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
 479                        goto got_phyid;
 480        }
 481        pr_warn("Too many CAIF Link Layers (max 6)\n");
 482        goto out;
 483
 484got_phyid:
 485        phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
 486        if (!phyinfo)
 487                goto out_err;
 488
 489        phy_layer->id = phyid;
 490        phyinfo->pref = pref;
 491        phyinfo->id = phyid;
 492        phyinfo->dev_info.id = phyid;
 493        phyinfo->dev_info.dev = dev;
 494        phyinfo->phy_layer = phy_layer;
 495        phyinfo->ifindex = dev->ifindex;
 496        phyinfo->head_room = head_room;
 497        phyinfo->use_fcs = fcs;
 498
 499        frml = cffrml_create(phyid, fcs);
 500
 501        if (!frml)
 502                goto out_err;
 503        phyinfo->frm_layer = frml;
 504        layer_set_up(frml, cnfg->mux);
 505
 506        if (link_support != NULL) {
 507                link_support->id = phyid;
 508                layer_set_dn(frml, link_support);
 509                layer_set_up(link_support, frml);
 510                layer_set_dn(link_support, phy_layer);
 511                layer_set_up(phy_layer, link_support);
 512        } else {
 513                layer_set_dn(frml, phy_layer);
 514                layer_set_up(phy_layer, frml);
 515        }
 516
 517        list_add_rcu(&phyinfo->node, &cnfg->phys);
 518out:
 519        mutex_unlock(&cnfg->lock);
 520        return;
 521
 522out_err:
 523        kfree(phyinfo);
 524        mutex_unlock(&cnfg->lock);
 525}
 526EXPORT_SYMBOL(cfcnfg_add_phy_layer);
 527
 528int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
 529                bool up)
 530{
 531        struct cfcnfg_phyinfo *phyinfo;
 532
 533        rcu_read_lock();
 534        phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
 535        if (phyinfo == NULL) {
 536                rcu_read_unlock();
 537                return -ENODEV;
 538        }
 539
 540        if (phyinfo->up == up) {
 541                rcu_read_unlock();
 542                return 0;
 543        }
 544        phyinfo->up = up;
 545
 546        if (up) {
 547                cffrml_hold(phyinfo->frm_layer);
 548                cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
 549                                        phy_layer->id);
 550        } else {
 551                cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
 552                cffrml_put(phyinfo->frm_layer);
 553        }
 554
 555        rcu_read_unlock();
 556        return 0;
 557}
 558EXPORT_SYMBOL(cfcnfg_set_phy_state);
 559
 560int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
 561{
 562        struct cflayer *frml, *frml_dn;
 563        u16 phyid;
 564        struct cfcnfg_phyinfo *phyinfo;
 565
 566        might_sleep();
 567
 568        mutex_lock(&cnfg->lock);
 569
 570        phyid = phy_layer->id;
 571        phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
 572
 573        if (phyinfo == NULL) {
 574                mutex_unlock(&cnfg->lock);
 575                return 0;
 576        }
 577        caif_assert(phyid == phyinfo->id);
 578        caif_assert(phy_layer == phyinfo->phy_layer);
 579        caif_assert(phy_layer->id == phyid);
 580        caif_assert(phyinfo->frm_layer->id == phyid);
 581
 582        list_del_rcu(&phyinfo->node);
 583        synchronize_rcu();
 584
 585        /* Fail if reference count is not zero */
 586        if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
 587                pr_info("Wait for device inuse\n");
 588                list_add_rcu(&phyinfo->node, &cnfg->phys);
 589                mutex_unlock(&cnfg->lock);
 590                return -EAGAIN;
 591        }
 592
 593        frml = phyinfo->frm_layer;
 594        frml_dn = frml->dn;
 595        cffrml_set_uplayer(frml, NULL);
 596        cffrml_set_dnlayer(frml, NULL);
 597        if (phy_layer != frml_dn) {
 598                layer_set_up(frml_dn, NULL);
 599                layer_set_dn(frml_dn, NULL);
 600        }
 601        layer_set_up(phy_layer, NULL);
 602
 603        if (phyinfo->phy_layer != frml_dn)
 604                kfree(frml_dn);
 605
 606        cffrml_free(frml);
 607        kfree(phyinfo);
 608        mutex_unlock(&cnfg->lock);
 609
 610        return 0;
 611}
 612EXPORT_SYMBOL(cfcnfg_del_phy_layer);
 613
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.