net/dcb/dcbnl.c

   1 /*
   2  * Copyright (c) 2008-2011, Intel Corporation.
   3  *
   4  * This program is free software; you can redistribute it and/or modify it
   5  * under the terms and conditions of the GNU General Public License,
   6  * version 2, as published by the Free Software Foundation.
   7  *
   8  * This program is distributed in the hope it will be useful, but WITHOUT
   9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  11  * more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along with
  14  * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  15  * Place - Suite 330, Boston, MA 02111-1307 USA.
  16  *
  17  * Author: Lucy Liu <lucy.liu@intel.com>
  18  */
  19
  20 #include <linux/netdevice.h>
  21 #include <linux/netlink.h>
  22 #include <linux/slab.h>
  23 #include <net/netlink.h>
  24 #include <net/rtnetlink.h>
  25 #include <linux/dcbnl.h>
  26 #include <net/dcbevent.h>
  27 #include <linux/rtnetlink.h>
  28 #include <linux/module.h>
  29 #include <net/sock.h>
  30
  31 /**
  32  * Data Center Bridging (DCB) is a collection of Ethernet enhancements
  33  * intended to allow network traffic with differing requirements
  34  * (highly reliable, no drops vs. best effort vs. low latency) to operate
  35  * and co-exist on Ethernet.  Current DCB features are:
  36  *
  37  * Enhanced Transmission Selection (aka Priority Grouping [PG]) - provides a
  38  *   framework for assigning bandwidth guarantees to traffic classes.
  39  *
  40  * Priority-based Flow Control (PFC) - provides a flow control mechanism which
  41  *   can work independently for each 802.1p priority.
  42  *
  43  * Congestion Notification - provides a mechanism for end-to-end congestion
  44  *   control for protocols which do not have built-in congestion management.
  45  *
  46  * More information about the emerging standards for these Ethernet features
  47  * can be found at: http://www.ieee802.org/1/pages/dcbridges.html
  48  *
  49  * This file implements an rtnetlink interface to allow configuration of DCB
  50  * features for capable devices.
  51  */
  52
  53 MODULE_AUTHOR("Lucy Liu, <lucy.liu@intel.com>");
  54 MODULE_DESCRIPTION("Data Center Bridging netlink interface");
  55 MODULE_LICENSE("GPL");
  56
  57 /**************** DCB attribute policies *************************************/
  58
  59 /* DCB netlink attributes policy */
  60 static const struct nla_policy dcbnl_rtnl_policy[DCB_ATTR_MAX + 1] = {
  61         [DCB_ATTR_IFNAME]      = {.type = NLA_NUL_STRING, .len = IFNAMSIZ - 1},
  62         [DCB_ATTR_STATE]       = {.type = NLA_U8},
  63         [DCB_ATTR_PFC_CFG]     = {.type = NLA_NESTED},
  64         [DCB_ATTR_PG_CFG]      = {.type = NLA_NESTED},
  65         [DCB_ATTR_SET_ALL]     = {.type = NLA_U8},
  66         [DCB_ATTR_PERM_HWADDR] = {.type = NLA_FLAG},
  67         [DCB_ATTR_CAP]         = {.type = NLA_NESTED},
  68         [DCB_ATTR_PFC_STATE]   = {.type = NLA_U8},
  69         [DCB_ATTR_BCN]         = {.type = NLA_NESTED},
  70         [DCB_ATTR_APP]         = {.type = NLA_NESTED},
  71         [DCB_ATTR_IEEE]        = {.type = NLA_NESTED},
  72         [DCB_ATTR_DCBX]        = {.type = NLA_U8},
  73         [DCB_ATTR_FEATCFG]     = {.type = NLA_NESTED},
  74 };
  75
  76 /* DCB priority flow control to User Priority nested attributes */
  77 static const struct nla_policy dcbnl_pfc_up_nest[DCB_PFC_UP_ATTR_MAX + 1] = {
  78         [DCB_PFC_UP_ATTR_0]   = {.type = NLA_U8},
  79         [DCB_PFC_UP_ATTR_1]   = {.type = NLA_U8},
  80         [DCB_PFC_UP_ATTR_2]   = {.type = NLA_U8},
  81         [DCB_PFC_UP_ATTR_3]   = {.type = NLA_U8},
  82         [DCB_PFC_UP_ATTR_4]   = {.type = NLA_U8},
  83         [DCB_PFC_UP_ATTR_5]   = {.type = NLA_U8},
  84         [DCB_PFC_UP_ATTR_6]   = {.type = NLA_U8},
  85         [DCB_PFC_UP_ATTR_7]   = {.type = NLA_U8},
  86         [DCB_PFC_UP_ATTR_ALL] = {.type = NLA_FLAG},
  87 };
  88
  89 /* DCB priority grouping nested attributes */
  90 static const struct nla_policy dcbnl_pg_nest[DCB_PG_ATTR_MAX + 1] = {
  91         [DCB_PG_ATTR_TC_0]      = {.type = NLA_NESTED},
  92         [DCB_PG_ATTR_TC_1]      = {.type = NLA_NESTED},
  93         [DCB_PG_ATTR_TC_2]      = {.type = NLA_NESTED},
  94         [DCB_PG_ATTR_TC_3]      = {.type = NLA_NESTED},
  95         [DCB_PG_ATTR_TC_4]      = {.type = NLA_NESTED},
  96         [DCB_PG_ATTR_TC_5]      = {.type = NLA_NESTED},
  97         [DCB_PG_ATTR_TC_6]      = {.type = NLA_NESTED},
  98         [DCB_PG_ATTR_TC_7]      = {.type = NLA_NESTED},
  99         [DCB_PG_ATTR_TC_ALL]    = {.type = NLA_NESTED},
 100         [DCB_PG_ATTR_BW_ID_0]   = {.type = NLA_U8},
 101         [DCB_PG_ATTR_BW_ID_1]   = {.type = NLA_U8},
 102         [DCB_PG_ATTR_BW_ID_2]   = {.type = NLA_U8},
 103         [DCB_PG_ATTR_BW_ID_3]   = {.type = NLA_U8},
 104         [DCB_PG_ATTR_BW_ID_4]   = {.type = NLA_U8},
 105         [DCB_PG_ATTR_BW_ID_5]   = {.type = NLA_U8},
 106         [DCB_PG_ATTR_BW_ID_6]   = {.type = NLA_U8},
 107         [DCB_PG_ATTR_BW_ID_7]   = {.type = NLA_U8},
 108         [DCB_PG_ATTR_BW_ID_ALL] = {.type = NLA_FLAG},
 109 };
 110
 111 /* DCB traffic class nested attributes. */
 112 static const struct nla_policy dcbnl_tc_param_nest[DCB_TC_ATTR_PARAM_MAX + 1] = {
 113         [DCB_TC_ATTR_PARAM_PGID]            = {.type = NLA_U8},
 114         [DCB_TC_ATTR_PARAM_UP_MAPPING]      = {.type = NLA_U8},
 115         [DCB_TC_ATTR_PARAM_STRICT_PRIO]     = {.type = NLA_U8},
 116         [DCB_TC_ATTR_PARAM_BW_PCT]          = {.type = NLA_U8},
 117         [DCB_TC_ATTR_PARAM_ALL]             = {.type = NLA_FLAG},
 118 };
 119
 120 /* DCB capabilities nested attributes. */
 121 static const struct nla_policy dcbnl_cap_nest[DCB_CAP_ATTR_MAX + 1] = {
 122         [DCB_CAP_ATTR_ALL]     = {.type = NLA_FLAG},
 123         [DCB_CAP_ATTR_PG]      = {.type = NLA_U8},
 124         [DCB_CAP_ATTR_PFC]     = {.type = NLA_U8},
 125         [DCB_CAP_ATTR_UP2TC]   = {.type = NLA_U8},
 126         [DCB_CAP_ATTR_PG_TCS]  = {.type = NLA_U8},
 127         [DCB_CAP_ATTR_PFC_TCS] = {.type = NLA_U8},
 128         [DCB_CAP_ATTR_GSP]     = {.type = NLA_U8},
 129         [DCB_CAP_ATTR_BCN]     = {.type = NLA_U8},
 130         [DCB_CAP_ATTR_DCBX]    = {.type = NLA_U8},
 131 };
 132
 133 /* DCB capabilities nested attributes. */
 134 static const struct nla_policy dcbnl_numtcs_nest[DCB_NUMTCS_ATTR_MAX + 1] = {
 135         [DCB_NUMTCS_ATTR_ALL]     = {.type = NLA_FLAG},
 136         [DCB_NUMTCS_ATTR_PG]      = {.type = NLA_U8},
 137         [DCB_NUMTCS_ATTR_PFC]     = {.type = NLA_U8},
 138 };
 139
 140 /* DCB BCN nested attributes. */
 141 static const struct nla_policy dcbnl_bcn_nest[DCB_BCN_ATTR_MAX + 1] = {
 142         [DCB_BCN_ATTR_RP_0]         = {.type = NLA_U8},
 143         [DCB_BCN_ATTR_RP_1]         = {.type = NLA_U8},
 144         [DCB_BCN_ATTR_RP_2]         = {.type = NLA_U8},
 145         [DCB_BCN_ATTR_RP_3]         = {.type = NLA_U8},
 146         [DCB_BCN_ATTR_RP_4]         = {.type = NLA_U8},
 147         [DCB_BCN_ATTR_RP_5]         = {.type = NLA_U8},
 148         [DCB_BCN_ATTR_RP_6]         = {.type = NLA_U8},
 149         [DCB_BCN_ATTR_RP_7]         = {.type = NLA_U8},
 150         [DCB_BCN_ATTR_RP_ALL]       = {.type = NLA_FLAG},
 151         [DCB_BCN_ATTR_BCNA_0]       = {.type = NLA_U32},
 152         [DCB_BCN_ATTR_BCNA_1]       = {.type = NLA_U32},
 153         [DCB_BCN_ATTR_ALPHA]        = {.type = NLA_U32},
 154         [DCB_BCN_ATTR_BETA]         = {.type = NLA_U32},
 155         [DCB_BCN_ATTR_GD]           = {.type = NLA_U32},
 156         [DCB_BCN_ATTR_GI]           = {.type = NLA_U32},
 157         [DCB_BCN_ATTR_TMAX]         = {.type = NLA_U32},
 158         [DCB_BCN_ATTR_TD]           = {.type = NLA_U32},
 159         [DCB_BCN_ATTR_RMIN]         = {.type = NLA_U32},
 160         [DCB_BCN_ATTR_W]            = {.type = NLA_U32},
 161         [DCB_BCN_ATTR_RD]           = {.type = NLA_U32},
 162         [DCB_BCN_ATTR_RU]           = {.type = NLA_U32},
 163         [DCB_BCN_ATTR_WRTT]         = {.type = NLA_U32},
 164         [DCB_BCN_ATTR_RI]           = {.type = NLA_U32},
 165         [DCB_BCN_ATTR_C]            = {.type = NLA_U32},
 166         [DCB_BCN_ATTR_ALL]          = {.type = NLA_FLAG},
 167 };
 168
 169 /* DCB APP nested attributes. */
 170 static const struct nla_policy dcbnl_app_nest[DCB_APP_ATTR_MAX + 1] = {
 171         [DCB_APP_ATTR_IDTYPE]       = {.type = NLA_U8},
 172         [DCB_APP_ATTR_ID]           = {.type = NLA_U16},
 173         [DCB_APP_ATTR_PRIORITY]     = {.type = NLA_U8},
 174 };
 175
 176 /* IEEE 802.1Qaz nested attributes. */
 177 static const struct nla_policy dcbnl_ieee_policy[DCB_ATTR_IEEE_MAX + 1] = {
 178         [DCB_ATTR_IEEE_ETS]         = {.len = sizeof(struct ieee_ets)},
 179         [DCB_ATTR_IEEE_PFC]         = {.len = sizeof(struct ieee_pfc)},
 180         [DCB_ATTR_IEEE_APP_TABLE]   = {.type = NLA_NESTED},
 181         [DCB_ATTR_IEEE_MAXRATE]   = {.len = sizeof(struct ieee_maxrate)},
 182 };
 183
 184 static const struct nla_policy dcbnl_ieee_app[DCB_ATTR_IEEE_APP_MAX + 1] = {
 185         [DCB_ATTR_IEEE_APP]         = {.len = sizeof(struct dcb_app)},
 186 };
 187
 188 /* DCB number of traffic classes nested attributes. */
 189 static const struct nla_policy dcbnl_featcfg_nest[DCB_FEATCFG_ATTR_MAX + 1] = {
 190         [DCB_FEATCFG_ATTR_ALL]      = {.type = NLA_FLAG},
 191         [DCB_FEATCFG_ATTR_PG]       = {.type = NLA_U8},
 192         [DCB_FEATCFG_ATTR_PFC]      = {.type = NLA_U8},
 193         [DCB_FEATCFG_ATTR_APP]      = {.type = NLA_U8},
 194 };
 195
 196 static LIST_HEAD(dcb_app_list);
 197 static DEFINE_SPINLOCK(dcb_lock);
 198
 199 static struct sk_buff *dcbnl_newmsg(int type, u8 cmd, u32 port, u32 seq,
 200                                     u32 flags, struct nlmsghdr **nlhp)
 201 {
 202         struct sk_buff *skb;
 203         struct dcbmsg *dcb;
 204         struct nlmsghdr *nlh;
 205
 206         skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 207         if (!skb)
 208                 return NULL;
 209
 210         nlh = nlmsg_put(skb, port, seq, type, sizeof(*dcb), flags);
 211         BUG_ON(!nlh);
 212
 213         dcb = nlmsg_data(nlh);
 214         dcb->dcb_family = AF_UNSPEC;
 215         dcb->cmd = cmd;
 216         dcb->dcb_pad = 0;
 217
 218         if (nlhp)
 219                 *nlhp = nlh;
 220
 221         return skb;
 222 }
 223
 224 static int dcbnl_getstate(struct net_device *netdev, struct nlmsghdr *nlh,
 225                           u32 seq, struct nlattr **tb, struct sk_buff *skb)
 226 {
 227         /* if (!tb[DCB_ATTR_STATE] || !netdev->dcbnl_ops->getstate) */
 228         if (!netdev->dcbnl_ops->getstate)
 229                 return -EOPNOTSUPP;
 230
 231         return nla_put_u8(skb, DCB_ATTR_STATE,
 232                           netdev->dcbnl_ops->getstate(netdev));
 233 }
 234
 235 static int dcbnl_getpfccfg(struct net_device *netdev, struct nlmsghdr *nlh,
 236                            u32 seq, struct nlattr **tb, struct sk_buff *skb)
 237 {
 238         struct nlattr *data[DCB_PFC_UP_ATTR_MAX + 1], *nest;
 239         u8 value;
 240         int ret;
 241         int i;
 242         int getall = 0;
 243
 244         if (!tb[DCB_ATTR_PFC_CFG])
 245                 return -EINVAL;
 246
 247         if (!netdev->dcbnl_ops->getpfccfg)
 248                 return -EOPNOTSUPP;
 249
 250         ret = nla_parse_nested(data, DCB_PFC_UP_ATTR_MAX,
 251                                tb[DCB_ATTR_PFC_CFG],
 252                                dcbnl_pfc_up_nest);
 253         if (ret)
 254                 return ret;
 255
 256         nest = nla_nest_start(skb, DCB_ATTR_PFC_CFG);
 257         if (!nest)
 258                 return -EMSGSIZE;
 259
 260         if (data[DCB_PFC_UP_ATTR_ALL])
 261                 getall = 1;
 262
 263         for (i = DCB_PFC_UP_ATTR_0; i <= DCB_PFC_UP_ATTR_7; i++) {
 264                 if (!getall && !data[i])
 265                         continue;
 266
 267                 netdev->dcbnl_ops->getpfccfg(netdev, i - DCB_PFC_UP_ATTR_0,
 268                                              &value);
 269                 ret = nla_put_u8(skb, i, value);
 270                 if (ret) {
 271                         nla_nest_cancel(skb, nest);
 272                         return ret;
 273                 }
 274         }
 275         nla_nest_end(skb, nest);
 276
 277         return 0;
 278 }
 279
 280 static int dcbnl_getperm_hwaddr(struct net_device *netdev, struct nlmsghdr *nlh,
 281                                 u32 seq, struct nlattr **tb, struct sk_buff *skb)
 282 {
 283         u8 perm_addr[MAX_ADDR_LEN];
 284
 285         if (!netdev->dcbnl_ops->getpermhwaddr)
 286                 return -EOPNOTSUPP;
 287
 288         netdev->dcbnl_ops->getpermhwaddr(netdev, perm_addr);
 289
 290         return nla_put(skb, DCB_ATTR_PERM_HWADDR, sizeof(perm_addr), perm_addr);
 291 }
 292
 293 static int dcbnl_getcap(struct net_device *netdev, struct nlmsghdr *nlh,
 294                         u32 seq, struct nlattr **tb, struct sk_buff *skb)
 295 {
 296         struct nlattr *data[DCB_CAP_ATTR_MAX + 1], *nest;
 297         u8 value;
 298         int ret;
 299         int i;
 300         int getall = 0;
 301
 302         if (!tb[DCB_ATTR_CAP])
 303                 return -EINVAL;
 304
 305         if (!netdev->dcbnl_ops->getcap)
 306                 return -EOPNOTSUPP;
 307
 308         ret = nla_parse_nested(data, DCB_CAP_ATTR_MAX, tb[DCB_ATTR_CAP],
 309                                dcbnl_cap_nest);
 310         if (ret)
 311                 return ret;
 312
 313         nest = nla_nest_start(skb, DCB_ATTR_CAP);
 314         if (!nest)
 315                 return -EMSGSIZE;
 316
 317         if (data[DCB_CAP_ATTR_ALL])
 318                 getall = 1;
 319
 320         for (i = DCB_CAP_ATTR_ALL+1; i <= DCB_CAP_ATTR_MAX; i++) {
 321                 if (!getall && !data[i])
 322                         continue;
 323
 324                 if (!netdev->dcbnl_ops->getcap(netdev, i, &value)) {
 325                         ret = nla_put_u8(skb, i, value);
 326                         if (ret) {
 327                                 nla_nest_cancel(skb, nest);
 328                                 return ret;
 329                         }
 330                 }
 331         }
 332         nla_nest_end(skb, nest);
 333
 334         return 0;
 335 }
 336
 337 static int dcbnl_getnumtcs(struct net_device *netdev, struct nlmsghdr *nlh,
 338                            u32 seq, struct nlattr **tb, struct sk_buff *skb)
 339 {
 340         struct nlattr *data[DCB_NUMTCS_ATTR_MAX + 1], *nest;
 341         u8 value;
 342         int ret;
 343         int i;
 344         int getall = 0;
 345
 346         if (!tb[DCB_ATTR_NUMTCS])
 347                 return -EINVAL;
 348
 349         if (!netdev->dcbnl_ops->getnumtcs)
 350                 return -EOPNOTSUPP;
 351
 352         ret = nla_parse_nested(data, DCB_NUMTCS_ATTR_MAX, tb[DCB_ATTR_NUMTCS],
 353                                dcbnl_numtcs_nest);
 354         if (ret)
 355                 return ret;
 356
 357         nest = nla_nest_start(skb, DCB_ATTR_NUMTCS);
 358         if (!nest)
 359                 return -EMSGSIZE;
 360
 361         if (data[DCB_NUMTCS_ATTR_ALL])
 362                 getall = 1;
 363
 364         for (i = DCB_NUMTCS_ATTR_ALL+1; i <= DCB_NUMTCS_ATTR_MAX; i++) {
 365                 if (!getall && !data[i])
 366                         continue;
 367
 368                 ret = netdev->dcbnl_ops->getnumtcs(netdev, i, &value);
 369                 if (!ret) {
 370                         ret = nla_put_u8(skb, i, value);
 371                         if (ret) {
 372                                 nla_nest_cancel(skb, nest);
 373                                 return ret;
 374                         }
 375                 } else
 376                         return -EINVAL;
 377         }
 378         nla_nest_end(skb, nest);
 379
 380         return 0;
 381 }
 382
 383 static int dcbnl_setnumtcs(struct net_device *netdev, struct nlmsghdr *nlh,
 384                            u32 seq, struct nlattr **tb, struct sk_buff *skb)
 385 {
 386         struct nlattr *data[DCB_NUMTCS_ATTR_MAX + 1];
 387         int ret;
 388         u8 value;
 389         int i;
 390
 391         if (!tb[DCB_ATTR_NUMTCS])
 392                 return -EINVAL;
 393
 394         if (!netdev->dcbnl_ops->setnumtcs)
 395                 return -EOPNOTSUPP;
 396
 397         ret = nla_parse_nested(data, DCB_NUMTCS_ATTR_MAX, tb[DCB_ATTR_NUMTCS],
 398                                dcbnl_numtcs_nest);
 399         if (ret)
 400                 return ret;
 401
 402         for (i = DCB_NUMTCS_ATTR_ALL+1; i <= DCB_NUMTCS_ATTR_MAX; i++) {
 403                 if (data[i] == NULL)
 404                         continue;
 405
 406                 value = nla_get_u8(data[i]);
 407
 408                 ret = netdev->dcbnl_ops->setnumtcs(netdev, i, value);
 409                 if (ret)
 410                         break;
 411         }
 412
 413         return nla_put_u8(skb, DCB_ATTR_NUMTCS, !!ret);
 414 }
 415
 416 static int dcbnl_getpfcstate(struct net_device *netdev, struct nlmsghdr *nlh,
 417                              u32 seq, struct nlattr **tb, struct sk_buff *skb)
 418 {
 419         if (!netdev->dcbnl_ops->getpfcstate)
 420                 return -EOPNOTSUPP;
 421
 422         return nla_put_u8(skb, DCB_ATTR_PFC_STATE,
 423                           netdev->dcbnl_ops->getpfcstate(netdev));
 424 }
 425
 426 static int dcbnl_setpfcstate(struct net_device *netdev, struct nlmsghdr *nlh,
 427                              u32 seq, struct nlattr **tb, struct sk_buff *skb)
 428 {
 429         u8 value;
 430
 431         if (!tb[DCB_ATTR_PFC_STATE])
 432                 return -EINVAL;
 433
 434         if (!netdev->dcbnl_ops->setpfcstate)
 435                 return -EOPNOTSUPP;
 436
 437         value = nla_get_u8(tb[DCB_ATTR_PFC_STATE]);
 438
 439         netdev->dcbnl_ops->setpfcstate(netdev, value);
 440
 441         return nla_put_u8(skb, DCB_ATTR_PFC_STATE, 0);
 442 }
 443
 444 static int dcbnl_getapp(struct net_device *netdev, struct nlmsghdr *nlh,
 445                         u32 seq, struct nlattr **tb, struct sk_buff *skb)
 446 {
 447         struct nlattr *app_nest;
 448         struct nlattr *app_tb[DCB_APP_ATTR_MAX + 1];
 449         u16 id;
 450         u8 up, idtype;
 451         int ret;
 452
 453         if (!tb[DCB_ATTR_APP])
 454                 return -EINVAL;
 455
 456         ret = nla_parse_nested(app_tb, DCB_APP_ATTR_MAX, tb[DCB_ATTR_APP],
 457                                dcbnl_app_nest);
 458         if (ret)
 459                 return ret;
 460
 461         /* all must be non-null */
 462         if ((!app_tb[DCB_APP_ATTR_IDTYPE]) ||
 463             (!app_tb[DCB_APP_ATTR_ID]))
 464                 return -EINVAL;
 465
 466         /* either by eth type or by socket number */
 467         idtype = nla_get_u8(app_tb[DCB_APP_ATTR_IDTYPE]);
 468         if ((idtype != DCB_APP_IDTYPE_ETHTYPE) &&
 469             (idtype != DCB_APP_IDTYPE_PORTNUM))
 470                 return -EINVAL;
 471
 472         id = nla_get_u16(app_tb[DCB_APP_ATTR_ID]);
 473
 474         if (netdev->dcbnl_ops->getapp) {
 475                 up = netdev->dcbnl_ops->getapp(netdev, idtype, id);
 476         } else {
 477                 struct dcb_app app = {
 478                                         .selector = idtype,
 479                                         .protocol = id,
 480                                      };
 481                 up = dcb_getapp(netdev, &app);
 482         }
 483
 484         app_nest = nla_nest_start(skb, DCB_ATTR_APP);
 485         if (!app_nest)
 486                 return -EMSGSIZE;
 487
 488         ret = nla_put_u8(skb, DCB_APP_ATTR_IDTYPE, idtype);
 489         if (ret)
 490                 goto out_cancel;
 491
 492         ret = nla_put_u16(skb, DCB_APP_ATTR_ID, id);
 493         if (ret)
 494                 goto out_cancel;
 495
 496         ret = nla_put_u8(skb, DCB_APP_ATTR_PRIORITY, up);
 497         if (ret)
 498                 goto out_cancel;
 499
 500         nla_nest_end(skb, app_nest);
 501
 502         return 0;
 503
 504 out_cancel:
 505         nla_nest_cancel(skb, app_nest);
 506         return ret;
 507 }
 508
 509 static int dcbnl_setapp(struct net_device *netdev, struct nlmsghdr *nlh,
 510                         u32 seq, struct nlattr **tb, struct sk_buff *skb)
 511 {
 512         int ret;
 513         u16 id;
 514         u8 up, idtype;
 515         struct nlattr *app_tb[DCB_APP_ATTR_MAX + 1];
 516
 517         if (!tb[DCB_ATTR_APP])
 518                 return -EINVAL;
 519
 520         ret = nla_parse_nested(app_tb, DCB_APP_ATTR_MAX, tb[DCB_ATTR_APP],
 521                                dcbnl_app_nest);
 522         if (ret)
 523                 return ret;
 524
 525         /* all must be non-null */
 526         if ((!app_tb[DCB_APP_ATTR_IDTYPE]) ||
 527             (!app_tb[DCB_APP_ATTR_ID]) ||
 528             (!app_tb[DCB_APP_ATTR_PRIORITY]))
 529                 return -EINVAL;
 530
 531         /* either by eth type or by socket number */
 532         idtype = nla_get_u8(app_tb[DCB_APP_ATTR_IDTYPE]);
 533         if ((idtype != DCB_APP_IDTYPE_ETHTYPE) &&
 534             (idtype != DCB_APP_IDTYPE_PORTNUM))
 535                 return -EINVAL;
 536
 537         id = nla_get_u16(app_tb[DCB_APP_ATTR_ID]);
 538         up = nla_get_u8(app_tb[DCB_APP_ATTR_PRIORITY]);
 539
 540         if (netdev->dcbnl_ops->setapp) {
 541                 ret = netdev->dcbnl_ops->setapp(netdev, idtype, id, up);
 542         } else {
 543                 struct dcb_app app;
 544                 app.selector = idtype;
 545                 app.protocol = id;
 546                 app.priority = up;
 547                 ret = dcb_setapp(netdev, &app);
 548         }
 549
 550         ret = nla_put_u8(skb, DCB_ATTR_APP, ret);
 551         dcbnl_cee_notify(netdev, RTM_SETDCB, DCB_CMD_SAPP, seq, 0);
 552
 553         return ret;
 554 }
 555
 556 static int __dcbnl_pg_getcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 557                              struct nlattr **tb, struct sk_buff *skb, int dir)
 558 {
 559         struct nlattr *pg_nest, *param_nest, *data;
 560         struct nlattr *pg_tb[DCB_PG_ATTR_MAX + 1];
 561         struct nlattr *param_tb[DCB_TC_ATTR_PARAM_MAX + 1];
 562         u8 prio, pgid, tc_pct, up_map;
 563         int ret;
 564         int getall = 0;
 565         int i;
 566
 567         if (!tb[DCB_ATTR_PG_CFG])
 568                 return -EINVAL;
 569
 570         if (!netdev->dcbnl_ops->getpgtccfgtx ||
 571             !netdev->dcbnl_ops->getpgtccfgrx ||
 572             !netdev->dcbnl_ops->getpgbwgcfgtx ||
 573             !netdev->dcbnl_ops->getpgbwgcfgrx)
 574                 return -EOPNOTSUPP;
 575
 576         ret = nla_parse_nested(pg_tb, DCB_PG_ATTR_MAX,
 577                                tb[DCB_ATTR_PG_CFG], dcbnl_pg_nest);
 578         if (ret)
 579                 return ret;
 580
 581         pg_nest = nla_nest_start(skb, DCB_ATTR_PG_CFG);
 582         if (!pg_nest)
 583                 return -EMSGSIZE;
 584
 585         if (pg_tb[DCB_PG_ATTR_TC_ALL])
 586                 getall = 1;
 587
 588         for (i = DCB_PG_ATTR_TC_0; i <= DCB_PG_ATTR_TC_7; i++) {
 589                 if (!getall && !pg_tb[i])
 590                         continue;
 591
 592                 if (pg_tb[DCB_PG_ATTR_TC_ALL])
 593                         data = pg_tb[DCB_PG_ATTR_TC_ALL];
 594                 else
 595                         data = pg_tb[i];
 596                 ret = nla_parse_nested(param_tb, DCB_TC_ATTR_PARAM_MAX,
 597                                        data, dcbnl_tc_param_nest);
 598                 if (ret)
 599                         goto err_pg;
 600
 601                 param_nest = nla_nest_start(skb, i);
 602                 if (!param_nest)
 603                         goto err_pg;
 604
 605                 pgid = DCB_ATTR_VALUE_UNDEFINED;
 606                 prio = DCB_ATTR_VALUE_UNDEFINED;
 607                 tc_pct = DCB_ATTR_VALUE_UNDEFINED;
 608                 up_map = DCB_ATTR_VALUE_UNDEFINED;
 609
 610                 if (dir) {
 611                         /* Rx */
 612                         netdev->dcbnl_ops->getpgtccfgrx(netdev,
 613                                                 i - DCB_PG_ATTR_TC_0, &prio,
 614                                                 &pgid, &tc_pct, &up_map);
 615                 } else {
 616                         /* Tx */
 617                         netdev->dcbnl_ops->getpgtccfgtx(netdev,
 618                                                 i - DCB_PG_ATTR_TC_0, &prio,
 619                                                 &pgid, &tc_pct, &up_map);
 620                 }
 621
 622                 if (param_tb[DCB_TC_ATTR_PARAM_PGID] ||
 623                     param_tb[DCB_TC_ATTR_PARAM_ALL]) {
 624                         ret = nla_put_u8(skb,
 625                                          DCB_TC_ATTR_PARAM_PGID, pgid);
 626                         if (ret)
 627                                 goto err_param;
 628                 }
 629                 if (param_tb[DCB_TC_ATTR_PARAM_UP_MAPPING] ||
 630                     param_tb[DCB_TC_ATTR_PARAM_ALL]) {
 631                         ret = nla_put_u8(skb,
 632                                          DCB_TC_ATTR_PARAM_UP_MAPPING, up_map);
 633                         if (ret)
 634                                 goto err_param;
 635                 }
 636                 if (param_tb[DCB_TC_ATTR_PARAM_STRICT_PRIO] ||
 637                     param_tb[DCB_TC_ATTR_PARAM_ALL]) {
 638                         ret = nla_put_u8(skb,
 639                                          DCB_TC_ATTR_PARAM_STRICT_PRIO, prio);
 640                         if (ret)
 641                                 goto err_param;
 642                 }
 643                 if (param_tb[DCB_TC_ATTR_PARAM_BW_PCT] ||
 644                     param_tb[DCB_TC_ATTR_PARAM_ALL]) {
 645                         ret = nla_put_u8(skb, DCB_TC_ATTR_PARAM_BW_PCT,
 646                                          tc_pct);
 647                         if (ret)
 648                                 goto err_param;
 649                 }
 650                 nla_nest_end(skb, param_nest);
 651         }
 652
 653         if (pg_tb[DCB_PG_ATTR_BW_ID_ALL])
 654                 getall = 1;
 655         else
 656                 getall = 0;
 657
 658         for (i = DCB_PG_ATTR_BW_ID_0; i <= DCB_PG_ATTR_BW_ID_7; i++) {
 659                 if (!getall && !pg_tb[i])
 660                         continue;
 661
 662                 tc_pct = DCB_ATTR_VALUE_UNDEFINED;
 663
 664                 if (dir) {
 665                         /* Rx */
 666                         netdev->dcbnl_ops->getpgbwgcfgrx(netdev,
 667                                         i - DCB_PG_ATTR_BW_ID_0, &tc_pct);
 668                 } else {
 669                         /* Tx */
 670                         netdev->dcbnl_ops->getpgbwgcfgtx(netdev,
 671                                         i - DCB_PG_ATTR_BW_ID_0, &tc_pct);
 672                 }
 673                 ret = nla_put_u8(skb, i, tc_pct);
 674                 if (ret)
 675                         goto err_pg;
 676         }
 677
 678         nla_nest_end(skb, pg_nest);
 679
 680         return 0;
 681
 682 err_param:
 683         nla_nest_cancel(skb, param_nest);
 684 err_pg:
 685         nla_nest_cancel(skb, pg_nest);
 686
 687         return -EMSGSIZE;
 688 }
 689
 690 static int dcbnl_pgtx_getcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 691                              u32 seq, struct nlattr **tb, struct sk_buff *skb)
 692 {
 693         return __dcbnl_pg_getcfg(netdev, nlh, tb, skb, 0);
 694 }
 695
 696 static int dcbnl_pgrx_getcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 697                              u32 seq, struct nlattr **tb, struct sk_buff *skb)
 698 {
 699         return __dcbnl_pg_getcfg(netdev, nlh, tb, skb, 1);
 700 }
 701
 702 static int dcbnl_setstate(struct net_device *netdev, struct nlmsghdr *nlh,
 703                           u32 seq, struct nlattr **tb, struct sk_buff *skb)
 704 {
 705         u8 value;
 706
 707         if (!tb[DCB_ATTR_STATE])
 708                 return -EINVAL;
 709
 710         if (!netdev->dcbnl_ops->setstate)
 711                 return -EOPNOTSUPP;
 712
 713         value = nla_get_u8(tb[DCB_ATTR_STATE]);
 714
 715         return nla_put_u8(skb, DCB_ATTR_STATE,
 716                           netdev->dcbnl_ops->setstate(netdev, value));
 717 }
 718
 719 static int dcbnl_setpfccfg(struct net_device *netdev, struct nlmsghdr *nlh,
 720                            u32 seq, struct nlattr **tb, struct sk_buff *skb)
 721 {
 722         struct nlattr *data[DCB_PFC_UP_ATTR_MAX + 1];
 723         int i;
 724         int ret;
 725         u8 value;
 726
 727         if (!tb[DCB_ATTR_PFC_CFG])
 728                 return -EINVAL;
 729
 730         if (!netdev->dcbnl_ops->setpfccfg)
 731                 return -EOPNOTSUPP;
 732
 733         ret = nla_parse_nested(data, DCB_PFC_UP_ATTR_MAX,
 734                                tb[DCB_ATTR_PFC_CFG],
 735                                dcbnl_pfc_up_nest);
 736         if (ret)
 737                 return ret;
 738
 739         for (i = DCB_PFC_UP_ATTR_0; i <= DCB_PFC_UP_ATTR_7; i++) {
 740                 if (data[i] == NULL)
 741                         continue;
 742                 value = nla_get_u8(data[i]);
 743                 netdev->dcbnl_ops->setpfccfg(netdev,
 744                         data[i]->nla_type - DCB_PFC_UP_ATTR_0, value);
 745         }
 746
 747         return nla_put_u8(skb, DCB_ATTR_PFC_CFG, 0);
 748 }
 749
 750 static int dcbnl_setall(struct net_device *netdev, struct nlmsghdr *nlh,
 751                         u32 seq, struct nlattr **tb, struct sk_buff *skb)
 752 {
 753         int ret;
 754
 755         if (!tb[DCB_ATTR_SET_ALL])
 756                 return -EINVAL;
 757
 758         if (!netdev->dcbnl_ops->setall)
 759                 return -EOPNOTSUPP;
 760
 761         ret = nla_put_u8(skb, DCB_ATTR_SET_ALL,
 762                          netdev->dcbnl_ops->setall(netdev));
 763         dcbnl_cee_notify(netdev, RTM_SETDCB, DCB_CMD_SET_ALL, seq, 0);
 764
 765         return ret;
 766 }
 767
 768 static int __dcbnl_pg_setcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 769                              u32 seq, struct nlattr **tb, struct sk_buff *skb,
 770                              int dir)
 771 {
 772         struct nlattr *pg_tb[DCB_PG_ATTR_MAX + 1];
 773         struct nlattr *param_tb[DCB_TC_ATTR_PARAM_MAX + 1];
 774         int ret;
 775         int i;
 776         u8 pgid;
 777         u8 up_map;
 778         u8 prio;
 779         u8 tc_pct;
 780
 781         if (!tb[DCB_ATTR_PG_CFG])
 782                 return -EINVAL;
 783
 784         if (!netdev->dcbnl_ops->setpgtccfgtx ||
 785             !netdev->dcbnl_ops->setpgtccfgrx ||
 786             !netdev->dcbnl_ops->setpgbwgcfgtx ||
 787             !netdev->dcbnl_ops->setpgbwgcfgrx)
 788                 return -EOPNOTSUPP;
 789
 790         ret = nla_parse_nested(pg_tb, DCB_PG_ATTR_MAX,
 791                                tb[DCB_ATTR_PG_CFG], dcbnl_pg_nest);
 792         if (ret)
 793                 return ret;
 794
 795         for (i = DCB_PG_ATTR_TC_0; i <= DCB_PG_ATTR_TC_7; i++) {
 796                 if (!pg_tb[i])
 797                         continue;
 798
 799                 ret = nla_parse_nested(param_tb, DCB_TC_ATTR_PARAM_MAX,
 800                                        pg_tb[i], dcbnl_tc_param_nest);
 801                 if (ret)
 802                         return ret;
 803
 804                 pgid = DCB_ATTR_VALUE_UNDEFINED;
 805                 prio = DCB_ATTR_VALUE_UNDEFINED;
 806                 tc_pct = DCB_ATTR_VALUE_UNDEFINED;
 807                 up_map = DCB_ATTR_VALUE_UNDEFINED;
 808
 809                 if (param_tb[DCB_TC_ATTR_PARAM_STRICT_PRIO])
 810                         prio =
 811                             nla_get_u8(param_tb[DCB_TC_ATTR_PARAM_STRICT_PRIO]);
 812
 813                 if (param_tb[DCB_TC_ATTR_PARAM_PGID])
 814                         pgid = nla_get_u8(param_tb[DCB_TC_ATTR_PARAM_PGID]);
 815
 816                 if (param_tb[DCB_TC_ATTR_PARAM_BW_PCT])
 817                         tc_pct = nla_get_u8(param_tb[DCB_TC_ATTR_PARAM_BW_PCT]);
 818
 819                 if (param_tb[DCB_TC_ATTR_PARAM_UP_MAPPING])
 820                         up_map =
 821                              nla_get_u8(param_tb[DCB_TC_ATTR_PARAM_UP_MAPPING]);
 822
 823                 /* dir: Tx = 0, Rx = 1 */
 824                 if (dir) {
 825                         /* Rx */
 826                         netdev->dcbnl_ops->setpgtccfgrx(netdev,
 827                                 i - DCB_PG_ATTR_TC_0,
 828                                 prio, pgid, tc_pct, up_map);
 829                 } else {
 830                         /* Tx */
 831                         netdev->dcbnl_ops->setpgtccfgtx(netdev,
 832                                 i - DCB_PG_ATTR_TC_0,
 833                                 prio, pgid, tc_pct, up_map);
 834                 }
 835         }
 836
 837         for (i = DCB_PG_ATTR_BW_ID_0; i <= DCB_PG_ATTR_BW_ID_7; i++) {
 838                 if (!pg_tb[i])
 839                         continue;
 840
 841                 tc_pct = nla_get_u8(pg_tb[i]);
 842
 843                 /* dir: Tx = 0, Rx = 1 */
 844                 if (dir) {
 845                         /* Rx */
 846                         netdev->dcbnl_ops->setpgbwgcfgrx(netdev,
 847                                          i - DCB_PG_ATTR_BW_ID_0, tc_pct);
 848                 } else {
 849                         /* Tx */
 850                         netdev->dcbnl_ops->setpgbwgcfgtx(netdev,
 851                                          i - DCB_PG_ATTR_BW_ID_0, tc_pct);
 852                 }
 853         }
 854
 855         return nla_put_u8(skb, DCB_ATTR_PG_CFG, 0);
 856 }
 857
 858 static int dcbnl_pgtx_setcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 859                              u32 seq, struct nlattr **tb, struct sk_buff *skb)
 860 {
 861         return __dcbnl_pg_setcfg(netdev, nlh, seq, tb, skb, 0);
 862 }
 863
 864 static int dcbnl_pgrx_setcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 865                              u32 seq, struct nlattr **tb, struct sk_buff *skb)
 866 {
 867         return __dcbnl_pg_setcfg(netdev, nlh, seq, tb, skb, 1);
 868 }
 869
 870 static int dcbnl_bcn_getcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 871                             u32 seq, struct nlattr **tb, struct sk_buff *skb)
 872 {
 873         struct nlattr *bcn_nest;
 874         struct nlattr *bcn_tb[DCB_BCN_ATTR_MAX + 1];
 875         u8 value_byte;
 876         u32 value_integer;
 877         int ret;
 878         bool getall = false;
 879         int i;
 880
 881         if (!tb[DCB_ATTR_BCN])
 882                 return -EINVAL;
 883
 884         if (!netdev->dcbnl_ops->getbcnrp ||
 885             !netdev->dcbnl_ops->getbcncfg)
 886                 return -EOPNOTSUPP;
 887
 888         ret = nla_parse_nested(bcn_tb, DCB_BCN_ATTR_MAX,
 889                                tb[DCB_ATTR_BCN], dcbnl_bcn_nest);
 890         if (ret)
 891                 return ret;
 892
 893         bcn_nest = nla_nest_start(skb, DCB_ATTR_BCN);
 894         if (!bcn_nest)
 895                 return -EMSGSIZE;
 896
 897         if (bcn_tb[DCB_BCN_ATTR_ALL])
 898                 getall = true;
 899
 900         for (i = DCB_BCN_ATTR_RP_0; i <= DCB_BCN_ATTR_RP_7; i++) {
 901                 if (!getall && !bcn_tb[i])
 902                         continue;
 903
 904                 netdev->dcbnl_ops->getbcnrp(netdev, i - DCB_BCN_ATTR_RP_0,
 905                                             &value_byte);
 906                 ret = nla_put_u8(skb, i, value_byte);
 907                 if (ret)
 908                         goto err_bcn;
 909         }
 910
 911         for (i = DCB_BCN_ATTR_BCNA_0; i <= DCB_BCN_ATTR_RI; i++) {
 912                 if (!getall && !bcn_tb[i])
 913                         continue;
 914
 915                 netdev->dcbnl_ops->getbcncfg(netdev, i,
 916                                              &value_integer);
 917                 ret = nla_put_u32(skb, i, value_integer);
 918                 if (ret)
 919                         goto err_bcn;
 920         }
 921
 922         nla_nest_end(skb, bcn_nest);
 923
 924         return 0;
 925
 926 err_bcn:
 927         nla_nest_cancel(skb, bcn_nest);
 928         return ret;
 929 }
 930
 931 static int dcbnl_bcn_setcfg(struct net_device *netdev, struct nlmsghdr *nlh,
 932                             u32 seq, struct nlattr **tb, struct sk_buff *skb)
 933 {
 934         struct nlattr *data[DCB_BCN_ATTR_MAX + 1];
 935         int i;
 936         int ret;
 937         u8 value_byte;
 938         u32 value_int;
 939
 940         if (!tb[DCB_ATTR_BCN])
 941                 return -EINVAL;
 942
 943         if (!netdev->dcbnl_ops->setbcncfg ||
 944             !netdev->dcbnl_ops->setbcnrp)
 945                 return -EOPNOTSUPP;
 946
 947         ret = nla_parse_nested(data, DCB_BCN_ATTR_MAX,
 948                                tb[DCB_ATTR_BCN],
 949                                dcbnl_pfc_up_nest);
 950         if (ret)
 951                 return ret;
 952
 953         for (i = DCB_BCN_ATTR_RP_0; i <= DCB_BCN_ATTR_RP_7; i++) {
 954                 if (data[i] == NULL)
 955                         continue;
 956                 value_byte = nla_get_u8(data[i]);
 957                 netdev->dcbnl_ops->setbcnrp(netdev,
 958                         data[i]->nla_type - DCB_BCN_ATTR_RP_0, value_byte);
 959         }
 960
 961         for (i = DCB_BCN_ATTR_BCNA_0; i <= DCB_BCN_ATTR_RI; i++) {
 962                 if (data[i] == NULL)
 963                         continue;
 964                 value_int = nla_get_u32(data[i]);
 965                 netdev->dcbnl_ops->setbcncfg(netdev,
 966                                              i, value_int);
 967         }
 968
 969         return nla_put_u8(skb, DCB_ATTR_BCN, 0);
 970 }
 971
 972 static int dcbnl_build_peer_app(struct net_device *netdev, struct sk_buff* skb,
 973                                 int app_nested_type, int app_info_type,
 974                                 int app_entry_type)
 975 {
 976         struct dcb_peer_app_info info;
 977         struct dcb_app *table = NULL;
 978         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
 979         u16 app_count;
 980         int err;
 981
 982
 983         /**
 984          * retrieve the peer app configuration form the driver. If the driver
 985          * handlers fail exit without doing anything
 986          */
 987         err = ops->peer_getappinfo(netdev, &info, &app_count);
 988         if (!err && app_count) {
 989                 table = kmalloc(sizeof(struct dcb_app) * app_count, GFP_KERNEL);
 990                 if (!table)
 991                         return -ENOMEM;
 992
 993                 err = ops->peer_getapptable(netdev, table);
 994         }
 995
 996         if (!err) {
 997                 u16 i;
 998                 struct nlattr *app;
 999
1000                 /**
1001                  * build the message, from here on the only possible failure
1002                  * is due to the skb size
1003                  */
1004                 err = -EMSGSIZE;
1005
1006                 app = nla_nest_start(skb, app_nested_type);
1007                 if (!app)
1008                         goto nla_put_failure;
1009
1010                 if (app_info_type &&
1011                     nla_put(skb, app_info_type, sizeof(info), &info))
1012                         goto nla_put_failure;
1013
1014                 for (i = 0; i < app_count; i++) {
1015                         if (nla_put(skb, app_entry_type, sizeof(struct dcb_app),
1016                                     &table[i]))
1017                                 goto nla_put_failure;
1018                 }
1019                 nla_nest_end(skb, app);
1020         }
1021         err = 0;
1022
1023 nla_put_failure:
1024         kfree(table);
1025         return err;
1026 }
1027
1028 /* Handle IEEE 802.1Qaz GET commands. */
1029 static int dcbnl_ieee_fill(struct sk_buff *skb, struct net_device *netdev)
1030 {
1031         struct nlattr *ieee, *app;
1032         struct dcb_app_type *itr;
1033         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
1034         int dcbx;
1035         int err;
1036
1037         if (nla_put_string(skb, DCB_ATTR_IFNAME, netdev->name))
1038                 return -EMSGSIZE;
1039
1040         ieee = nla_nest_start(skb, DCB_ATTR_IEEE);
1041         if (!ieee)
1042                 return -EMSGSIZE;
1043
1044         if (ops->ieee_getets) {
1045                 struct ieee_ets ets;
1046                 err = ops->ieee_getets(netdev, &ets);
1047                 if (!err &&
1048                     nla_put(skb, DCB_ATTR_IEEE_ETS, sizeof(ets), &ets))
1049                         return -EMSGSIZE;
1050         }
1051
1052         if (ops->ieee_getmaxrate) {
1053                 struct ieee_maxrate maxrate;
1054                 err = ops->ieee_getmaxrate(netdev, &maxrate);
1055                 if (!err) {
1056                         err = nla_put(skb, DCB_ATTR_IEEE_MAXRATE,
1057                                       sizeof(maxrate), &maxrate);
1058                         if (err)
1059                                 return -EMSGSIZE;
1060                 }
1061         }
1062
1063         if (ops->ieee_getpfc) {
1064                 struct ieee_pfc pfc;
1065                 err = ops->ieee_getpfc(netdev, &pfc);
1066                 if (!err &&
1067                     nla_put(skb, DCB_ATTR_IEEE_PFC, sizeof(pfc), &pfc))
1068                         return -EMSGSIZE;
1069         }
1070
1071         app = nla_nest_start(skb, DCB_ATTR_IEEE_APP_TABLE);
1072         if (!app)
1073                 return -EMSGSIZE;
1074
1075         spin_lock(&dcb_lock);
1076         list_for_each_entry(itr, &dcb_app_list, list) {
1077                 if (itr->ifindex == netdev->ifindex) {
1078                         err = nla_put(skb, DCB_ATTR_IEEE_APP, sizeof(itr->app),
1079                                          &itr->app);
1080                         if (err) {
1081                                 spin_unlock(&dcb_lock);
1082                                 return -EMSGSIZE;
1083                         }
1084                 }
1085         }
1086
1087         if (netdev->dcbnl_ops->getdcbx)
1088                 dcbx = netdev->dcbnl_ops->getdcbx(netdev);
1089         else
1090                 dcbx = -EOPNOTSUPP;
1091
1092         spin_unlock(&dcb_lock);
1093         nla_nest_end(skb, app);
1094
1095         /* get peer info if available */
1096         if (ops->ieee_peer_getets) {
1097                 struct ieee_ets ets;
1098                 err = ops->ieee_peer_getets(netdev, &ets);
1099                 if (!err &&
1100                     nla_put(skb, DCB_ATTR_IEEE_PEER_ETS, sizeof(ets), &ets))
1101                         return -EMSGSIZE;
1102         }
1103
1104         if (ops->ieee_peer_getpfc) {
1105                 struct ieee_pfc pfc;
1106                 err = ops->ieee_peer_getpfc(netdev, &pfc);
1107                 if (!err &&
1108                     nla_put(skb, DCB_ATTR_IEEE_PEER_PFC, sizeof(pfc), &pfc))
1109                         return -EMSGSIZE;
1110         }
1111
1112         if (ops->peer_getappinfo && ops->peer_getapptable) {
1113                 err = dcbnl_build_peer_app(netdev, skb,
1114                                            DCB_ATTR_IEEE_PEER_APP,
1115                                            DCB_ATTR_IEEE_APP_UNSPEC,
1116                                            DCB_ATTR_IEEE_APP);
1117                 if (err)
1118                         return -EMSGSIZE;
1119         }
1120
1121         nla_nest_end(skb, ieee);
1122         if (dcbx >= 0) {
1123                 err = nla_put_u8(skb, DCB_ATTR_DCBX, dcbx);
1124                 if (err)
1125                         return -EMSGSIZE;
1126         }
1127
1128         return 0;
1129 }
1130
1131 static int dcbnl_cee_pg_fill(struct sk_buff *skb, struct net_device *dev,
1132                              int dir)
1133 {
1134         u8 pgid, up_map, prio, tc_pct;
1135         const struct dcbnl_rtnl_ops *ops = dev->dcbnl_ops;
1136         int i = dir ? DCB_ATTR_CEE_TX_PG : DCB_ATTR_CEE_RX_PG;
1137         struct nlattr *pg = nla_nest_start(skb, i);
1138
1139         if (!pg)
1140                 return -EMSGSIZE;
1141
1142         for (i = DCB_PG_ATTR_TC_0; i <= DCB_PG_ATTR_TC_7; i++) {
1143                 struct nlattr *tc_nest = nla_nest_start(skb, i);
1144
1145                 if (!tc_nest)
1146                         return -EMSGSIZE;
1147
1148                 pgid = DCB_ATTR_VALUE_UNDEFINED;
1149                 prio = DCB_ATTR_VALUE_UNDEFINED;
1150                 tc_pct = DCB_ATTR_VALUE_UNDEFINED;
1151                 up_map = DCB_ATTR_VALUE_UNDEFINED;
1152
1153                 if (!dir)
1154                         ops->getpgtccfgrx(dev, i - DCB_PG_ATTR_TC_0,
1155                                           &prio, &pgid, &tc_pct, &up_map);
1156                 else
1157                         ops->getpgtccfgtx(dev, i - DCB_PG_ATTR_TC_0,
1158                                           &prio, &pgid, &tc_pct, &up_map);
1159
1160                 if (nla_put_u8(skb, DCB_TC_ATTR_PARAM_PGID, pgid) ||
1161                     nla_put_u8(skb, DCB_TC_ATTR_PARAM_UP_MAPPING, up_map) ||
1162                     nla_put_u8(skb, DCB_TC_ATTR_PARAM_STRICT_PRIO, prio) ||
1163                     nla_put_u8(skb, DCB_TC_ATTR_PARAM_BW_PCT, tc_pct))
1164                         return -EMSGSIZE;
1165                 nla_nest_end(skb, tc_nest);
1166         }
1167
1168         for (i = DCB_PG_ATTR_BW_ID_0; i <= DCB_PG_ATTR_BW_ID_7; i++) {
1169                 tc_pct = DCB_ATTR_VALUE_UNDEFINED;
1170
1171                 if (!dir)
1172                         ops->getpgbwgcfgrx(dev, i - DCB_PG_ATTR_BW_ID_0,
1173                                            &tc_pct);
1174                 else
1175                         ops->getpgbwgcfgtx(dev, i - DCB_PG_ATTR_BW_ID_0,
1176                                            &tc_pct);
1177                 if (nla_put_u8(skb, i, tc_pct))
1178                         return -EMSGSIZE;
1179         }
1180         nla_nest_end(skb, pg);
1181         return 0;
1182 }
1183
1184 static int dcbnl_cee_fill(struct sk_buff *skb, struct net_device *netdev)
1185 {
1186         struct nlattr *cee, *app;
1187         struct dcb_app_type *itr;
1188         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
1189         int dcbx, i, err = -EMSGSIZE;
1190         u8 value;
1191
1192         if (nla_put_string(skb, DCB_ATTR_IFNAME, netdev->name))
1193                 goto nla_put_failure;
1194         cee = nla_nest_start(skb, DCB_ATTR_CEE);
1195         if (!cee)
1196                 goto nla_put_failure;
1197
1198         /* local pg */
1199         if (ops->getpgtccfgtx && ops->getpgbwgcfgtx) {
1200                 err = dcbnl_cee_pg_fill(skb, netdev, 1);
1201                 if (err)
1202                         goto nla_put_failure;
1203         }
1204
1205         if (ops->getpgtccfgrx && ops->getpgbwgcfgrx) {
1206                 err = dcbnl_cee_pg_fill(skb, netdev, 0);
1207                 if (err)
1208                         goto nla_put_failure;
1209         }
1210
1211         /* local pfc */
1212         if (ops->getpfccfg) {
1213                 struct nlattr *pfc_nest = nla_nest_start(skb, DCB_ATTR_CEE_PFC);
1214
1215                 if (!pfc_nest)
1216                         goto nla_put_failure;
1217
1218                 for (i = DCB_PFC_UP_ATTR_0; i <= DCB_PFC_UP_ATTR_7; i++) {
1219                         ops->getpfccfg(netdev, i - DCB_PFC_UP_ATTR_0, &value);
1220                         if (nla_put_u8(skb, i, value))
1221                                 goto nla_put_failure;
1222                 }
1223                 nla_nest_end(skb, pfc_nest);
1224         }
1225
1226         /* local app */
1227         spin_lock(&dcb_lock);
1228         app = nla_nest_start(skb, DCB_ATTR_CEE_APP_TABLE);
1229         if (!app)
1230                 goto dcb_unlock;
1231
1232         list_for_each_entry(itr, &dcb_app_list, list) {
1233                 if (itr->ifindex == netdev->ifindex) {
1234                         struct nlattr *app_nest = nla_nest_start(skb,
1235                                                                  DCB_ATTR_APP);
1236                         if (!app_nest)
1237                                 goto dcb_unlock;
1238
1239                         err = nla_put_u8(skb, DCB_APP_ATTR_IDTYPE,
1240                                          itr->app.selector);
1241                         if (err)
1242                                 goto dcb_unlock;
1243
1244                         err = nla_put_u16(skb, DCB_APP_ATTR_ID,
1245                                           itr->app.protocol);
1246                         if (err)
1247                                 goto dcb_unlock;
1248
1249                         err = nla_put_u8(skb, DCB_APP_ATTR_PRIORITY,
1250                                          itr->app.priority);
1251                         if (err)
1252                                 goto dcb_unlock;
1253
1254                         nla_nest_end(skb, app_nest);
1255                 }
1256         }
1257         nla_nest_end(skb, app);
1258
1259         if (netdev->dcbnl_ops->getdcbx)
1260                 dcbx = netdev->dcbnl_ops->getdcbx(netdev);
1261         else
1262                 dcbx = -EOPNOTSUPP;
1263
1264         spin_unlock(&dcb_lock);
1265
1266         /* features flags */
1267         if (ops->getfeatcfg) {
1268                 struct nlattr *feat = nla_nest_start(skb, DCB_ATTR_CEE_FEAT);
1269                 if (!feat)
1270                         goto nla_put_failure;
1271
1272                 for (i = DCB_FEATCFG_ATTR_ALL + 1; i <= DCB_FEATCFG_ATTR_MAX;
1273                      i++)
1274                         if (!ops->getfeatcfg(netdev, i, &value) &&
1275                             nla_put_u8(skb, i, value))
1276                                 goto nla_put_failure;
1277
1278                 nla_nest_end(skb, feat);
1279         }
1280
1281         /* peer info if available */
1282         if (ops->cee_peer_getpg) {
1283                 struct cee_pg pg;
1284                 err = ops->cee_peer_getpg(netdev, &pg);
1285                 if (!err &&
1286                     nla_put(skb, DCB_ATTR_CEE_PEER_PG, sizeof(pg), &pg))
1287                         goto nla_put_failure;
1288         }
1289
1290         if (ops->cee_peer_getpfc) {
1291                 struct cee_pfc pfc;
1292                 err = ops->cee_peer_getpfc(netdev, &pfc);
1293                 if (!err &&
1294                     nla_put(skb, DCB_ATTR_CEE_PEER_PFC, sizeof(pfc), &pfc))
1295                         goto nla_put_failure;
1296         }
1297
1298         if (ops->peer_getappinfo && ops->peer_getapptable) {
1299                 err = dcbnl_build_peer_app(netdev, skb,
1300                                            DCB_ATTR_CEE_PEER_APP_TABLE,
1301                                            DCB_ATTR_CEE_PEER_APP_INFO,
1302                                            DCB_ATTR_CEE_PEER_APP);
1303                 if (err)
1304                         goto nla_put_failure;
1305         }
1306         nla_nest_end(skb, cee);
1307
1308         /* DCBX state */
1309         if (dcbx >= 0) {
1310                 err = nla_put_u8(skb, DCB_ATTR_DCBX, dcbx);
1311                 if (err)
1312                         goto nla_put_failure;
1313         }
1314         return 0;
1315
1316 dcb_unlock:
1317         spin_unlock(&dcb_lock);
1318 nla_put_failure:
1319         return err;
1320 }
1321
1322 static int dcbnl_notify(struct net_device *dev, int event, int cmd,
1323                         u32 seq, u32 pid, int dcbx_ver)
1324 {
1325         struct net *net = dev_net(dev);
1326         struct sk_buff *skb;
1327         struct nlmsghdr *nlh;
1328         const struct dcbnl_rtnl_ops *ops = dev->dcbnl_ops;
1329         int err;
1330
1331         if (!ops)
1332                 return -EOPNOTSUPP;
1333
1334         skb = dcbnl_newmsg(event, cmd, pid, seq, 0, &nlh);
1335         if (!skb)
1336                 return -ENOBUFS;
1337
1338         if (dcbx_ver == DCB_CAP_DCBX_VER_IEEE)
1339                 err = dcbnl_ieee_fill(skb, dev);
1340         else
1341                 err = dcbnl_cee_fill(skb, dev);
1342
1343         if (err < 0) {
1344                 /* Report error to broadcast listeners */
1345                 nlmsg_free(skb);
1346                 rtnl_set_sk_err(net, RTNLGRP_DCB, err);
1347         } else {
1348                 /* End nlmsg and notify broadcast listeners */
1349                 nlmsg_end(skb, nlh);
1350                 rtnl_notify(skb, net, 0, RTNLGRP_DCB, NULL, GFP_KERNEL);
1351         }
1352
1353         return err;
1354 }
1355
1356 int dcbnl_ieee_notify(struct net_device *dev, int event, int cmd,
1357                       u32 seq, u32 pid)
1358 {
1359         return dcbnl_notify(dev, event, cmd, seq, pid, DCB_CAP_DCBX_VER_IEEE);
1360 }
1361 EXPORT_SYMBOL(dcbnl_ieee_notify);
1362
1363 int dcbnl_cee_notify(struct net_device *dev, int event, int cmd,
1364                      u32 seq, u32 pid)
1365 {
1366         return dcbnl_notify(dev, event, cmd, seq, pid, DCB_CAP_DCBX_VER_CEE);
1367 }
1368 EXPORT_SYMBOL(dcbnl_cee_notify);
1369
1370 /* Handle IEEE 802.1Qaz SET commands. If any requested operation can not
1371  * be completed the entire msg is aborted and error value is returned.
1372  * No attempt is made to reconcile the case where only part of the
1373  * cmd can be completed.
1374  */
1375 static int dcbnl_ieee_set(struct net_device *netdev, struct nlmsghdr *nlh,
1376                           u32 seq, struct nlattr **tb, struct sk_buff *skb)
1377 {
1378         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
1379         struct nlattr *ieee[DCB_ATTR_IEEE_MAX + 1];
1380         int err;
1381
1382         if (!ops)
1383                 return -EOPNOTSUPP;
1384
1385         if (!tb[DCB_ATTR_IEEE])
1386                 return -EINVAL;
1387
1388         err = nla_parse_nested(ieee, DCB_ATTR_IEEE_MAX,
1389                                tb[DCB_ATTR_IEEE], dcbnl_ieee_policy);
1390         if (err)
1391                 return err;
1392
1393         if (ieee[DCB_ATTR_IEEE_ETS] && ops->ieee_setets) {
1394                 struct ieee_ets *ets = nla_data(ieee[DCB_ATTR_IEEE_ETS]);
1395                 err = ops->ieee_setets(netdev, ets);
1396                 if (err)
1397                         goto err;
1398         }
1399
1400         if (ieee[DCB_ATTR_IEEE_MAXRATE] && ops->ieee_setmaxrate) {
1401                 struct ieee_maxrate *maxrate =
1402                         nla_data(ieee[DCB_ATTR_IEEE_MAXRATE]);
1403                 err = ops->ieee_setmaxrate(netdev, maxrate);
1404                 if (err)
1405                         goto err;
1406         }
1407
1408         if (ieee[DCB_ATTR_IEEE_PFC] && ops->ieee_setpfc) {
1409                 struct ieee_pfc *pfc = nla_data(ieee[DCB_ATTR_IEEE_PFC]);
1410                 err = ops->ieee_setpfc(netdev, pfc);
1411                 if (err)
1412                         goto err;
1413         }
1414
1415         if (ieee[DCB_ATTR_IEEE_APP_TABLE]) {
1416                 struct nlattr *attr;
1417                 int rem;
1418
1419                 nla_for_each_nested(attr, ieee[DCB_ATTR_IEEE_APP_TABLE], rem) {
1420                         struct dcb_app *app_data;
1421                         if (nla_type(attr) != DCB_ATTR_IEEE_APP)
1422                                 continue;
1423                         app_data = nla_data(attr);
1424                         if (ops->ieee_setapp)
1425                                 err = ops->ieee_setapp(netdev, app_data);
1426                         else
1427                                 err = dcb_ieee_setapp(netdev, app_data);
1428                         if (err)
1429                                 goto err;
1430                 }
1431         }
1432
1433 err:
1434         err = nla_put_u8(skb, DCB_ATTR_IEEE, err);
1435         dcbnl_ieee_notify(netdev, RTM_SETDCB, DCB_CMD_IEEE_SET, seq, 0);
1436         return err;
1437 }
1438
1439 static int dcbnl_ieee_get(struct net_device *netdev, struct nlmsghdr *nlh,
1440                           u32 seq, struct nlattr **tb, struct sk_buff *skb)
1441 {
1442         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
1443
1444         if (!ops)
1445                 return -EOPNOTSUPP;
1446
1447         return dcbnl_ieee_fill(skb, netdev);
1448 }
1449
1450 static int dcbnl_ieee_del(struct net_device *netdev, struct nlmsghdr *nlh,
1451                           u32 seq, struct nlattr **tb, struct sk_buff *skb)
1452 {
1453         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
1454         struct nlattr *ieee[DCB_ATTR_IEEE_MAX + 1];
1455         int err;
1456
1457         if (!ops)
1458                 return -EOPNOTSUPP;
1459
1460         if (!tb[DCB_ATTR_IEEE])
1461                 return -EINVAL;
1462
1463         err = nla_parse_nested(ieee, DCB_ATTR_IEEE_MAX,
1464                                tb[DCB_ATTR_IEEE], dcbnl_ieee_policy);
1465         if (err)
1466                 return err;
1467
1468         if (ieee[DCB_ATTR_IEEE_APP_TABLE]) {
1469                 struct nlattr *attr;
1470                 int rem;
1471
1472                 nla_for_each_nested(attr, ieee[DCB_ATTR_IEEE_APP_TABLE], rem) {
1473                         struct dcb_app *app_data;
1474
1475                         if (nla_type(attr) != DCB_ATTR_IEEE_APP)
1476                                 continue;
1477                         app_data = nla_data(attr);
1478                         if (ops->ieee_delapp)
1479                                 err = ops->ieee_delapp(netdev, app_data);
1480                         else
1481                                 err = dcb_ieee_delapp(netdev, app_data);
1482                         if (err)
1483                                 goto err;
1484                 }
1485         }
1486
1487 err:
1488         err = nla_put_u8(skb, DCB_ATTR_IEEE, err);
1489         dcbnl_ieee_notify(netdev, RTM_SETDCB, DCB_CMD_IEEE_DEL, seq, 0);
1490         return err;
1491 }
1492
1493
1494 /* DCBX configuration */
1495 static int dcbnl_getdcbx(struct net_device *netdev, struct nlmsghdr *nlh,
1496                          u32 seq, struct nlattr **tb, struct sk_buff *skb)
1497 {
1498         if (!netdev->dcbnl_ops->getdcbx)
1499                 return -EOPNOTSUPP;
1500
1501         return nla_put_u8(skb, DCB_ATTR_DCBX,
1502                           netdev->dcbnl_ops->getdcbx(netdev));
1503 }
1504
1505 static int dcbnl_setdcbx(struct net_device *netdev, struct nlmsghdr *nlh,
1506                          u32 seq, struct nlattr **tb, struct sk_buff *skb)
1507 {
1508         u8 value;
1509
1510         if (!netdev->dcbnl_ops->setdcbx)
1511                 return -EOPNOTSUPP;
1512
1513         if (!tb[DCB_ATTR_DCBX])
1514                 return -EINVAL;
1515
1516         value = nla_get_u8(tb[DCB_ATTR_DCBX]);
1517
1518         return nla_put_u8(skb, DCB_ATTR_DCBX,
1519                           netdev->dcbnl_ops->setdcbx(netdev, value));
1520 }
1521
1522 static int dcbnl_getfeatcfg(struct net_device *netdev, struct nlmsghdr *nlh,
1523                             u32 seq, struct nlattr **tb, struct sk_buff *skb)
1524 {
1525         struct nlattr *data[DCB_FEATCFG_ATTR_MAX + 1], *nest;
1526         u8 value;
1527         int ret, i;
1528         int getall = 0;
1529
1530         if (!netdev->dcbnl_ops->getfeatcfg)
1531                 return -EOPNOTSUPP;
1532
1533         if (!tb[DCB_ATTR_FEATCFG])
1534                 return -EINVAL;
1535
1536         ret = nla_parse_nested(data, DCB_FEATCFG_ATTR_MAX, tb[DCB_ATTR_FEATCFG],
1537                                dcbnl_featcfg_nest);
1538         if (ret)
1539                 return ret;
1540
1541         nest = nla_nest_start(skb, DCB_ATTR_FEATCFG);
1542         if (!nest)
1543                 return -EMSGSIZE;
1544
1545         if (data[DCB_FEATCFG_ATTR_ALL])
1546                 getall = 1;
1547
1548         for (i = DCB_FEATCFG_ATTR_ALL+1; i <= DCB_FEATCFG_ATTR_MAX; i++) {
1549                 if (!getall && !data[i])
1550                         continue;
1551
1552                 ret = netdev->dcbnl_ops->getfeatcfg(netdev, i, &value);
1553                 if (!ret)
1554                         ret = nla_put_u8(skb, i, value);
1555
1556                 if (ret) {
1557                         nla_nest_cancel(skb, nest);
1558                         goto nla_put_failure;
1559                 }
1560         }
1561         nla_nest_end(skb, nest);
1562
1563 nla_put_failure:
1564         return ret;
1565 }
1566
1567 static int dcbnl_setfeatcfg(struct net_device *netdev, struct nlmsghdr *nlh,
1568                             u32 seq, struct nlattr **tb, struct sk_buff *skb)
1569 {
1570         struct nlattr *data[DCB_FEATCFG_ATTR_MAX + 1];
1571         int ret, i;
1572         u8 value;
1573
1574         if (!netdev->dcbnl_ops->setfeatcfg)
1575                 return -ENOTSUPP;
1576
1577         if (!tb[DCB_ATTR_FEATCFG])
1578                 return -EINVAL;
1579
1580         ret = nla_parse_nested(data, DCB_FEATCFG_ATTR_MAX, tb[DCB_ATTR_FEATCFG],
1581                                dcbnl_featcfg_nest);
1582
1583         if (ret)
1584                 goto err;
1585
1586         for (i = DCB_FEATCFG_ATTR_ALL+1; i <= DCB_FEATCFG_ATTR_MAX; i++) {
1587                 if (data[i] == NULL)
1588                         continue;
1589
1590                 value = nla_get_u8(data[i]);
1591
1592                 ret = netdev->dcbnl_ops->setfeatcfg(netdev, i, value);
1593
1594                 if (ret)
1595                         goto err;
1596         }
1597 err:
1598         ret = nla_put_u8(skb, DCB_ATTR_FEATCFG, ret);
1599
1600         return ret;
1601 }
1602
1603 /* Handle CEE DCBX GET commands. */
1604 static int dcbnl_cee_get(struct net_device *netdev, struct nlmsghdr *nlh,
1605                          u32 seq, struct nlattr **tb, struct sk_buff *skb)
1606 {
1607         const struct dcbnl_rtnl_ops *ops = netdev->dcbnl_ops;
1608
1609         if (!ops)
1610                 return -EOPNOTSUPP;
1611
1612         return dcbnl_cee_fill(skb, netdev);
1613 }
1614
1615 struct reply_func {
1616         /* reply netlink message type */
1617         int     type;
1618
1619         /* function to fill message contents */
1620         int   (*cb)(struct net_device *, struct nlmsghdr *, u32,
1621                     struct nlattr **, struct sk_buff *);
1622 };
1623
1624 static const struct reply_func reply_funcs[DCB_CMD_MAX+1] = {
1625         [DCB_CMD_GSTATE]        = { RTM_GETDCB, dcbnl_getstate },
1626         [DCB_CMD_SSTATE]        = { RTM_SETDCB, dcbnl_setstate },
1627         [DCB_CMD_PFC_GCFG]      = { RTM_GETDCB, dcbnl_getpfccfg },
1628         [DCB_CMD_PFC_SCFG]      = { RTM_SETDCB, dcbnl_setpfccfg },
1629         [DCB_CMD_GPERM_HWADDR]  = { RTM_GETDCB, dcbnl_getperm_hwaddr },
1630         [DCB_CMD_GCAP]          = { RTM_GETDCB, dcbnl_getcap },
1631         [DCB_CMD_GNUMTCS]       = { RTM_GETDCB, dcbnl_getnumtcs },
1632         [DCB_CMD_SNUMTCS]       = { RTM_SETDCB, dcbnl_setnumtcs },
1633         [DCB_CMD_PFC_GSTATE]    = { RTM_GETDCB, dcbnl_getpfcstate },
1634         [DCB_CMD_PFC_SSTATE]    = { RTM_SETDCB, dcbnl_setpfcstate },
1635         [DCB_CMD_GAPP]          = { RTM_GETDCB, dcbnl_getapp },
1636         [DCB_CMD_SAPP]          = { RTM_SETDCB, dcbnl_setapp },
1637         [DCB_CMD_PGTX_GCFG]     = { RTM_GETDCB, dcbnl_pgtx_getcfg },
1638         [DCB_CMD_PGTX_SCFG]     = { RTM_SETDCB, dcbnl_pgtx_setcfg },
1639         [DCB_CMD_PGRX_GCFG]     = { RTM_GETDCB, dcbnl_pgrx_getcfg },
1640         [DCB_CMD_PGRX_SCFG]     = { RTM_SETDCB, dcbnl_pgrx_setcfg },
1641         [DCB_CMD_SET_ALL]       = { RTM_SETDCB, dcbnl_setall },
1642         [DCB_CMD_BCN_GCFG]      = { RTM_GETDCB, dcbnl_bcn_getcfg },
1643         [DCB_CMD_BCN_SCFG]      = { RTM_SETDCB, dcbnl_bcn_setcfg },
1644         [DCB_CMD_IEEE_GET]      = { RTM_GETDCB, dcbnl_ieee_get },
1645         [DCB_CMD_IEEE_SET]      = { RTM_SETDCB, dcbnl_ieee_set },
1646         [DCB_CMD_IEEE_DEL]      = { RTM_SETDCB, dcbnl_ieee_del },
1647         [DCB_CMD_GDCBX]         = { RTM_GETDCB, dcbnl_getdcbx },
1648         [DCB_CMD_SDCBX]         = { RTM_SETDCB, dcbnl_setdcbx },
1649         [DCB_CMD_GFEATCFG]      = { RTM_GETDCB, dcbnl_getfeatcfg },
1650         [DCB_CMD_SFEATCFG]      = { RTM_SETDCB, dcbnl_setfeatcfg },
1651         [DCB_CMD_CEE_GET]       = { RTM_GETDCB, dcbnl_cee_get },
1652 };
1653
1654 static int dcb_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
1655 {
1656         struct net *net = sock_net(skb->sk);
1657         struct net_device *netdev;
1658         struct dcbmsg *dcb = nlmsg_data(nlh);
1659         struct nlattr *tb[DCB_ATTR_MAX + 1];
1660         u32 pid = skb ? NETLINK_CB(skb).pid : 0;
1661         int ret = -EINVAL;
1662         struct sk_buff *reply_skb;
1663         struct nlmsghdr *reply_nlh = NULL;
1664         const struct reply_func *fn;
1665
1666         if (!net_eq(net, &init_net))
1667                 return -EINVAL;
1668
1669         ret = nlmsg_parse(nlh, sizeof(*dcb), tb, DCB_ATTR_MAX,
1670                           dcbnl_rtnl_policy);
1671         if (ret < 0)
1672                 return ret;
1673
1674         if (dcb->cmd > DCB_CMD_MAX)
1675                 return -EINVAL;
1676
1677         /* check if a reply function has been defined for the command */
1678         fn = &reply_funcs[dcb->cmd];
1679         if (!fn->cb)
1680                 return -EOPNOTSUPP;
1681
1682         if (!tb[DCB_ATTR_IFNAME])
1683                 return -EINVAL;
1684
1685         netdev = dev_get_by_name(&init_net, nla_data(tb[DCB_ATTR_IFNAME]));
1686         if (!netdev)
1687                 return -ENODEV;
1688
1689         if (!netdev->dcbnl_ops) {
1690                 ret = -EOPNOTSUPP;
1691                 goto out;
1692         }
1693
1694         reply_skb = dcbnl_newmsg(fn->type, dcb->cmd, pid, nlh->nlmsg_seq,
1695                                  nlh->nlmsg_flags, &reply_nlh);
1696         if (!reply_skb) {
1697                 ret = -ENOBUFS;
1698                 goto out;
1699         }
1700
1701         ret = fn->cb(netdev, nlh, nlh->nlmsg_seq, tb, reply_skb);
1702         if (ret < 0) {
1703                 nlmsg_free(reply_skb);
1704                 goto out;
1705         }
1706
1707         nlmsg_end(reply_skb, reply_nlh);
1708
1709         ret = rtnl_unicast(reply_skb, &init_net, pid);
1710 out:
1711         dev_put(netdev);
1712         return ret;
1713 }
1714
1715 static struct dcb_app_type *dcb_app_lookup(const struct dcb_app *app,
1716                                            int ifindex, int prio)
1717 {
1718         struct dcb_app_type *itr;
1719
1720         list_for_each_entry(itr, &dcb_app_list, list) {
1721                 if (itr->app.selector == app->selector &&
1722                     itr->app.protocol == app->protocol &&
1723                     itr->ifindex == ifindex &&
1724                     (!prio || itr->app.priority == prio))
1725                         return itr;
1726         }
1727
1728         return NULL;
1729 }
1730
1731 static int dcb_app_add(const struct dcb_app *app, int ifindex)
1732 {
1733         struct dcb_app_type *entry;
1734
1735         entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
1736         if (!entry)
1737                 return -ENOMEM;
1738
1739         memcpy(&entry->app, app, sizeof(*app));
1740         entry->ifindex = ifindex;
1741         list_add(&entry->list, &dcb_app_list);
1742
1743         return 0;
1744 }
1745
1746 /**
1747  * dcb_getapp - retrieve the DCBX application user priority
1748  *
1749  * On success returns a non-zero 802.1p user priority bitmap
1750  * otherwise returns 0 as the invalid user priority bitmap to
1751  * indicate an error.
1752  */
1753 u8 dcb_getapp(struct net_device *dev, struct dcb_app *app)
1754 {
1755         struct dcb_app_type *itr;
1756         u8 prio = 0;
1757
1758         spin_lock(&dcb_lock);
1759         if ((itr = dcb_app_lookup(app, dev->ifindex, 0)))
1760                 prio = itr->app.priority;
1761         spin_unlock(&dcb_lock);
1762
1763         return prio;
1764 }
1765 EXPORT_SYMBOL(dcb_getapp);
1766
1767 /**
1768  * dcb_setapp - add CEE dcb application data to app list
1769  *
1770  * Priority 0 is an invalid priority in CEE spec. This routine
1771  * removes applications from the app list if the priority is
1772  * set to zero.
1773  */
1774 int dcb_setapp(struct net_device *dev, struct dcb_app *new)
1775 {
1776         struct dcb_app_type *itr;
1777         struct dcb_app_type event;
1778         int err = 0;
1779
1780         event.ifindex = dev->ifindex;
1781         memcpy(&event.app, new, sizeof(event.app));
1782         if (dev->dcbnl_ops->getdcbx)
1783                 event.dcbx = dev->dcbnl_ops->getdcbx(dev);
1784
1785         spin_lock(&dcb_lock);
1786         /* Search for existing match and replace */
1787         if ((itr = dcb_app_lookup(new, dev->ifindex, 0))) {
1788                 if (new->priority)
1789                         itr->app.priority = new->priority;
1790                 else {
1791                         list_del(&itr->list);
1792                         kfree(itr);
1793                 }
1794                 goto out;
1795         }
1796         /* App type does not exist add new application type */
1797         if (new->priority)
1798                 err = dcb_app_add(new, dev->ifindex);
1799 out:
1800         spin_unlock(&dcb_lock);
1801         if (!err)
1802                 call_dcbevent_notifiers(DCB_APP_EVENT, &event);
1803         return err;
1804 }
1805 EXPORT_SYMBOL(dcb_setapp);
1806
1807 /**
1808  * dcb_ieee_getapp_mask - retrieve the IEEE DCB application priority
1809  *
1810  * Helper routine which on success returns a non-zero 802.1Qaz user
1811  * priority bitmap otherwise returns 0 to indicate the dcb_app was
1812  * not found in APP list.
1813  */
1814 u8 dcb_ieee_getapp_mask(struct net_device *dev, struct dcb_app *app)
1815 {
1816         struct dcb_app_type *itr;
1817         u8 prio = 0;
1818
1819         spin_lock(&dcb_lock);
1820         if ((itr = dcb_app_lookup(app, dev->ifindex, 0)))
1821                 prio |= 1 << itr->app.priority;
1822         spin_unlock(&dcb_lock);
1823
1824         return prio;
1825 }
1826 EXPORT_SYMBOL(dcb_ieee_getapp_mask);
1827
1828 /**
1829  * dcb_ieee_setapp - add IEEE dcb application data to app list
1830  *
1831  * This adds Application data to the list. Multiple application
1832  * entries may exists for the same selector and protocol as long
1833  * as the priorities are different.
1834  */
1835 int dcb_ieee_setapp(struct net_device *dev, struct dcb_app *new)
1836 {
1837         struct dcb_app_type event;
1838         int err = 0;
1839
1840         event.ifindex = dev->ifindex;
1841         memcpy(&event.app, new, sizeof(event.app));
1842         if (dev->dcbnl_ops->getdcbx)
1843                 event.dcbx = dev->dcbnl_ops->getdcbx(dev);
1844
1845         spin_lock(&dcb_lock);
1846         /* Search for existing match and abort if found */
1847         if (dcb_app_lookup(new, dev->ifindex, new->priority)) {
1848                 err = -EEXIST;
1849                 goto out;
1850         }
1851
1852         err = dcb_app_add(new, dev->ifindex);
1853 out:
1854         spin_unlock(&dcb_lock);
1855         if (!err)
1856                 call_dcbevent_notifiers(DCB_APP_EVENT, &event);
1857         return err;
1858 }
1859 EXPORT_SYMBOL(dcb_ieee_setapp);
1860
1861 /**
1862  * dcb_ieee_delapp - delete IEEE dcb application data from list
1863  *
1864  * This removes a matching APP data from the APP list
1865  */
1866 int dcb_ieee_delapp(struct net_device *dev, struct dcb_app *del)
1867 {
1868         struct dcb_app_type *itr;
1869         struct dcb_app_type event;
1870         int err = -ENOENT;
1871
1872         event.ifindex = dev->ifindex;
1873         memcpy(&event.app, del, sizeof(event.app));
1874         if (dev->dcbnl_ops->getdcbx)
1875                 event.dcbx = dev->dcbnl_ops->getdcbx(dev);
1876
1877         spin_lock(&dcb_lock);
1878         /* Search for existing match and remove it. */
1879         if ((itr = dcb_app_lookup(del, dev->ifindex, del->priority))) {
1880                 list_del(&itr->list);
1881                 kfree(itr);
1882                 err = 0;
1883         }
1884
1885         spin_unlock(&dcb_lock);
1886         if (!err)
1887                 call_dcbevent_notifiers(DCB_APP_EVENT, &event);
1888         return err;
1889 }
1890 EXPORT_SYMBOL(dcb_ieee_delapp);
1891
1892 static void dcb_flushapp(void)
1893 {
1894         struct dcb_app_type *app;
1895         struct dcb_app_type *tmp;
1896
1897         spin_lock(&dcb_lock);
1898         list_for_each_entry_safe(app, tmp, &dcb_app_list, list) {
1899                 list_del(&app->list);
1900                 kfree(app);
1901         }
1902         spin_unlock(&dcb_lock);
1903 }
1904
1905 static int __init dcbnl_init(void)
1906 {
1907         INIT_LIST_HEAD(&dcb_app_list);
1908
1909         rtnl_register(PF_UNSPEC, RTM_GETDCB, dcb_doit, NULL, NULL);
1910         rtnl_register(PF_UNSPEC, RTM_SETDCB, dcb_doit, NULL, NULL);
1911
1912         return 0;
1913 }
1914 module_init(dcbnl_init);
1915
1916 static void __exit dcbnl_exit(void)
1917 {
1918         rtnl_unregister(PF_UNSPEC, RTM_GETDCB);
1919         rtnl_unregister(PF_UNSPEC, RTM_SETDCB);
1920         dcb_flushapp();
1921 }
1922 module_exit(dcbnl_exit);