package/swconfig/src/swlib.c

   1 /*
   2  * swlib.c: Switch configuration API (user space part)
   3  *
   4  * Copyright (C) 2008 Felix Fietkau <nbd@openwrt.org>
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public License
   8  * version 2.1 as published by the Free Software Foundation.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  */
  15
  16 #include <stdio.h>
  17 #include <string.h>
  18 #include <stdlib.h>
  19 #include <inttypes.h>
  20 #include <errno.h>
  21 #include <stdint.h>
  22 #include <getopt.h>
  23 #include <sys/types.h>
  24 #include <sys/socket.h>
  25 #include <linux/switch.h>
  26 #include "swlib.h"
  27 #include <netlink/netlink.h>
  28 #include <netlink/genl/genl.h>
  29 #include <netlink/genl/family.h>
  30
  31 //#define DEBUG 1
  32 #ifdef DEBUG
  33 #define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__)
  34 #else
  35 #define DPRINTF(fmt, ...) do {} while (0)
  36 #endif
  37
  38 static struct nl_sock *handle;
  39 static struct nl_cache *cache;
  40 static struct genl_family *family;
  41 static struct nlattr *tb[SWITCH_ATTR_MAX];
  42 static int refcount = 0;
  43
  44 static struct nla_policy port_policy[] = {
  45         [SWITCH_PORT_ID] = { .type = NLA_U32 },
  46         [SWITCH_PORT_FLAG_TAGGED] = { .type = NLA_FLAG },
  47 };
  48
  49 static inline void *
  50 swlib_alloc(size_t size)
  51 {
  52         void *ptr;
  53
  54         ptr = malloc(size);
  55         if (!ptr)
  56                 goto done;
  57         memset(ptr, 0, size);
  58
  59 done:
  60         return ptr;
  61 }
  62
  63 static int
  64 wait_handler(struct nl_msg *msg, void *arg)
  65 {
  66         int *finished = arg;
  67
  68         *finished = 1;
  69         return NL_STOP;
  70 }
  71
  72 /* helper function for performing netlink requests */
  73 static int
  74 swlib_call(int cmd, int (*call)(struct nl_msg *, void *),
  75                 int (*data)(struct nl_msg *, void *), void *arg)
  76 {
  77         struct nl_msg *msg;
  78         struct nl_cb *cb = NULL;
  79         int finished;
  80         int flags = 0;
  81         int err;
  82
  83         msg = nlmsg_alloc();
  84         if (!msg) {
  85                 fprintf(stderr, "Out of memory!\n");
  86                 exit(1);
  87         }
  88
  89         if (!data)
  90                 flags |= NLM_F_DUMP;
  91
  92         genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, genl_family_get_id(family), 0, flags, cmd, 0);
  93         if (data) {
  94                 if (data(msg, arg) < 0)
  95                         goto nla_put_failure;
  96         }
  97
  98         cb = nl_cb_alloc(NL_CB_CUSTOM);
  99         if (!cb) {
 100                 fprintf(stderr, "nl_cb_alloc failed.\n");
 101                 exit(1);
 102         }
 103
 104         err = nl_send_auto_complete(handle, msg);
 105         if (err < 0) {
 106                 fprintf(stderr, "nl_send_auto_complete failed: %d\n", err);
 107                 goto out;
 108         }
 109
 110         finished = 0;
 111
 112         if (call)
 113                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, call, arg);
 114
 115         if (data)
 116                 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, wait_handler, &finished);
 117         else
 118                 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, wait_handler, &finished);
 119
 120         err = nl_recvmsgs(handle, cb);
 121         if (err < 0) {
 122                 goto out;
 123         }
 124
 125         if (!finished)
 126                 err = nl_wait_for_ack(handle);
 127
 128 out:
 129         if (cb)
 130                 nl_cb_put(cb);
 131 nla_put_failure:
 132         nlmsg_free(msg);
 133         return err;
 134 }
 135
 136 static int
 137 send_attr(struct nl_msg *msg, void *arg)
 138 {
 139         struct switch_val *val = arg;
 140         struct switch_attr *attr = val->attr;
 141
 142         NLA_PUT_U32(msg, SWITCH_ATTR_ID, attr->dev->id);
 143         NLA_PUT_U32(msg, SWITCH_ATTR_OP_ID, attr->id);
 144         switch(attr->atype) {
 145         case SWLIB_ATTR_GROUP_PORT:
 146                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_PORT, val->port_vlan);
 147                 break;
 148         case SWLIB_ATTR_GROUP_VLAN:
 149                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_VLAN, val->port_vlan);
 150                 break;
 151         default:
 152                 break;
 153         }
 154
 155         return 0;
 156
 157 nla_put_failure:
 158         return -1;
 159 }
 160
 161 static int
 162 store_port_val(struct nl_msg *msg, struct nlattr *nla, struct switch_val *val)
 163 {
 164         struct nlattr *p;
 165         int ports = val->attr->dev->ports;
 166         int err = 0;
 167         int remaining;
 168
 169         if (!val->value.ports)
 170                 val->value.ports = malloc(sizeof(struct switch_port) * ports);
 171
 172         nla_for_each_nested(p, nla, remaining) {
 173                 struct nlattr *tb[SWITCH_PORT_ATTR_MAX+1];
 174                 struct switch_port *port;
 175
 176                 if (val->len >= ports)
 177                         break;
 178
 179                 err = nla_parse_nested(tb, SWITCH_PORT_ATTR_MAX, p, port_policy);
 180                 if (err < 0)
 181                         goto out;
 182
 183                 if (!tb[SWITCH_PORT_ID])
 184                         continue;
 185
 186                 port = &val->value.ports[val->len];
 187                 port->id = nla_get_u32(tb[SWITCH_PORT_ID]);
 188                 port->flags = 0;
 189                 if (tb[SWITCH_PORT_FLAG_TAGGED])
 190                         port->flags |= SWLIB_PORT_FLAG_TAGGED;
 191
 192                 val->len++;
 193         }
 194
 195 out:
 196         return err;
 197 }
 198
 199 static int
 200 store_val(struct nl_msg *msg, void *arg)
 201 {
 202         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 203         struct switch_val *val = arg;
 204         struct switch_attr *attr = val->attr;
 205
 206         if (!val)
 207                 goto error;
 208
 209         if (nla_parse(tb, SWITCH_ATTR_MAX - 1, genlmsg_attrdata(gnlh, 0),
 210                         genlmsg_attrlen(gnlh, 0), NULL) < 0) {
 211                 goto error;
 212         }
 213
 214         if (tb[SWITCH_ATTR_OP_VALUE_INT])
 215                 val->value.i = nla_get_u32(tb[SWITCH_ATTR_OP_VALUE_INT]);
 216         else if (tb[SWITCH_ATTR_OP_VALUE_STR])
 217                 val->value.s = strdup(nla_get_string(tb[SWITCH_ATTR_OP_VALUE_STR]));
 218         else if (tb[SWITCH_ATTR_OP_VALUE_PORTS])
 219                 val->err = store_port_val(msg, tb[SWITCH_ATTR_OP_VALUE_PORTS], val);
 220
 221         val->err = 0;
 222         return 0;
 223
 224 error:
 225         return NL_SKIP;
 226 }
 227
 228 int
 229 swlib_get_attr(struct switch_dev *dev, struct switch_attr *attr, struct switch_val *val)
 230 {
 231         int cmd;
 232         int err;
 233
 234         switch(attr->atype) {
 235         case SWLIB_ATTR_GROUP_GLOBAL:
 236                 cmd = SWITCH_CMD_GET_GLOBAL;
 237                 break;
 238         case SWLIB_ATTR_GROUP_PORT:
 239                 cmd = SWITCH_CMD_GET_PORT;
 240                 break;
 241         case SWLIB_ATTR_GROUP_VLAN:
 242                 cmd = SWITCH_CMD_GET_VLAN;
 243                 break;
 244         default:
 245                 return -EINVAL;
 246         }
 247
 248         memset(&val->value, 0, sizeof(val->value));
 249         val->len = 0;
 250         val->attr = attr;
 251         val->err = -EINVAL;
 252         err = swlib_call(cmd, store_val, send_attr, val);
 253         if (!err)
 254                 err = val->err;
 255
 256         return err;
 257 }
 258
 259 static int
 260 send_attr_ports(struct nl_msg *msg, struct switch_val *val)
 261 {
 262         struct nlattr *n;
 263         int i;
 264
 265         /* TODO implement multipart? */
 266         if (val->len == 0)
 267                 goto done;
 268         n = nla_nest_start(msg, SWITCH_ATTR_OP_VALUE_PORTS);
 269         if (!n)
 270                 goto nla_put_failure;
 271         for (i = 0; i < val->len; i++) {
 272                 struct switch_port *port = &val->value.ports[i];
 273                 struct nlattr *np;
 274
 275                 np = nla_nest_start(msg, SWITCH_ATTR_PORT);
 276                 if (!np)
 277                         goto nla_put_failure;
 278
 279                 NLA_PUT_U32(msg, SWITCH_PORT_ID, port->id);
 280                 if (port->flags & SWLIB_PORT_FLAG_TAGGED)
 281                         NLA_PUT_FLAG(msg, SWITCH_PORT_FLAG_TAGGED);
 282
 283                 nla_nest_end(msg, np);
 284         }
 285         nla_nest_end(msg, n);
 286 done:
 287         return 0;
 288
 289 nla_put_failure:
 290         return -1;
 291 }
 292
 293 static int
 294 send_attr_val(struct nl_msg *msg, void *arg)
 295 {
 296         struct switch_val *val = arg;
 297         struct switch_attr *attr = val->attr;
 298
 299         if (send_attr(msg, arg))
 300                 goto nla_put_failure;
 301
 302         switch(attr->type) {
 303         case SWITCH_TYPE_NOVAL:
 304                 break;
 305         case SWITCH_TYPE_INT:
 306                 NLA_PUT_U32(msg, SWITCH_ATTR_OP_VALUE_INT, val->value.i);
 307                 break;
 308         case SWITCH_TYPE_STRING:
 309                 if (!val->value.s)
 310                         goto nla_put_failure;
 311                 NLA_PUT_STRING(msg, SWITCH_ATTR_OP_VALUE_STR, val->value.s);
 312                 break;
 313         case SWITCH_TYPE_PORTS:
 314                 if (send_attr_ports(msg, val) < 0)
 315                         goto nla_put_failure;
 316                 break;
 317         default:
 318                 goto nla_put_failure;
 319         }
 320         return 0;
 321
 322 nla_put_failure:
 323         return -1;
 324 }
 325
 326 int
 327 swlib_set_attr(struct switch_dev *dev, struct switch_attr *attr, struct switch_val *val)
 328 {
 329         int cmd;
 330
 331         switch(attr->atype) {
 332         case SWLIB_ATTR_GROUP_GLOBAL:
 333                 cmd = SWITCH_CMD_SET_GLOBAL;
 334                 break;
 335         case SWLIB_ATTR_GROUP_PORT:
 336                 cmd = SWITCH_CMD_SET_PORT;
 337                 break;
 338         case SWLIB_ATTR_GROUP_VLAN:
 339                 cmd = SWITCH_CMD_SET_VLAN;
 340                 break;
 341         default:
 342                 return -EINVAL;
 343         }
 344
 345         val->attr = attr;
 346         return swlib_call(cmd, NULL, send_attr_val, val);
 347 }
 348
 349 int swlib_set_attr_string(struct switch_dev *dev, struct switch_attr *a, int port_vlan, const char *str)
 350 {
 351         struct switch_port *ports;
 352         struct switch_val val;
 353         char *ptr;
 354
 355         memset(&val, 0, sizeof(val));
 356         val.port_vlan = port_vlan;
 357         switch(a->type) {
 358         case SWITCH_TYPE_INT:
 359                 val.value.i = atoi(str);
 360                 break;
 361         case SWITCH_TYPE_STRING:
 362                 val.value.s = str;
 363                 break;
 364         case SWITCH_TYPE_PORTS:
 365                 ports = alloca(sizeof(struct switch_port) * dev->ports);
 366                 memset(ports, 0, sizeof(struct switch_port) * dev->ports);
 367                 val.len = 0;
 368                 ptr = (char *)str;
 369                 while(ptr && *ptr)
 370                 {
 371                         while(*ptr && isspace(*ptr))
 372                                 ptr++;
 373
 374                         if (!*ptr)
 375                                 break;
 376
 377                         if (!isdigit(*ptr))
 378                                 return -1;
 379
 380                         if (val.len >= dev->ports)
 381                                 return -1;
 382
 383                         ports[val.len].flags = 0;
 384                         ports[val.len].id = strtoul(ptr, &ptr, 10);
 385                         while(*ptr && !isspace(*ptr)) {
 386                                 if (*ptr == 't')
 387                                         ports[val.len].flags |= SWLIB_PORT_FLAG_TAGGED;
 388                                 else
 389                                         return -1;
 390
 391                                 ptr++;
 392                         }
 393                         if (*ptr)
 394                                 ptr++;
 395                         val.len++;
 396                 }
 397                 val.value.ports = ports;
 398                 break;
 399         case SWITCH_TYPE_NOVAL:
 400                 break;
 401         default:
 402                 return -1;
 403         }
 404         return swlib_set_attr(dev, a, &val);
 405 }
 406
 407
 408 struct attrlist_arg {
 409         int id;
 410         int atype;
 411         struct switch_dev *dev;
 412         struct switch_attr *prev;
 413         struct switch_attr **head;
 414 };
 415
 416 static int
 417 add_id(struct nl_msg *msg, void *arg)
 418 {
 419         struct attrlist_arg *l = arg;
 420
 421         NLA_PUT_U32(msg, SWITCH_ATTR_ID, l->id);
 422
 423         return 0;
 424 nla_put_failure:
 425         return -1;
 426 }
 427
 428 static int
 429 add_attr(struct nl_msg *msg, void *ptr)
 430 {
 431         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 432         struct attrlist_arg *arg = ptr;
 433         struct switch_attr *new;
 434
 435         if (nla_parse(tb, SWITCH_ATTR_MAX - 1, genlmsg_attrdata(gnlh, 0),
 436                         genlmsg_attrlen(gnlh, 0), NULL) < 0)
 437                 goto done;
 438
 439         new = swlib_alloc(sizeof(struct switch_attr));
 440         if (!new)
 441                 goto done;
 442
 443         new->dev = arg->dev;
 444         new->atype = arg->atype;
 445         if (arg->prev) {
 446                 arg->prev->next = new;
 447         } else {
 448                 arg->prev = *arg->head;
 449         }
 450         *arg->head = new;
 451         arg->head = &new->next;
 452
 453         if (tb[SWITCH_ATTR_OP_ID])
 454                 new->id = nla_get_u32(tb[SWITCH_ATTR_OP_ID]);
 455         if (tb[SWITCH_ATTR_OP_TYPE])
 456                 new->type = nla_get_u32(tb[SWITCH_ATTR_OP_TYPE]);
 457         if (tb[SWITCH_ATTR_OP_NAME])
 458                 new->name = strdup(nla_get_string(tb[SWITCH_ATTR_OP_NAME]));
 459         if (tb[SWITCH_ATTR_OP_DESCRIPTION])
 460                 new->description = strdup(nla_get_string(tb[SWITCH_ATTR_OP_DESCRIPTION]));
 461
 462 done:
 463         return NL_SKIP;
 464 }
 465
 466 int
 467 swlib_scan(struct switch_dev *dev)
 468 {
 469         struct attrlist_arg arg;
 470
 471         if (dev->ops || dev->port_ops || dev->vlan_ops)
 472                 return 0;
 473
 474         arg.atype = SWLIB_ATTR_GROUP_GLOBAL;
 475         arg.dev = dev;
 476         arg.id = dev->id;
 477         arg.prev = NULL;
 478         arg.head = &dev->ops;
 479         swlib_call(SWITCH_CMD_LIST_GLOBAL, add_attr, add_id, &arg);
 480
 481         arg.atype = SWLIB_ATTR_GROUP_PORT;
 482         arg.prev = NULL;
 483         arg.head = &dev->port_ops;
 484         swlib_call(SWITCH_CMD_LIST_PORT, add_attr, add_id, &arg);
 485
 486         arg.atype = SWLIB_ATTR_GROUP_VLAN;
 487         arg.prev = NULL;
 488         arg.head = &dev->vlan_ops;
 489         swlib_call(SWITCH_CMD_LIST_VLAN, add_attr, add_id, &arg);
 490
 491         return 0;
 492 }
 493
 494 struct switch_attr *swlib_lookup_attr(struct switch_dev *dev,
 495                 enum swlib_attr_group atype, const char *name)
 496 {
 497         struct switch_attr *head;
 498
 499         if (!name || !dev)
 500                 return NULL;
 501
 502         switch(atype) {
 503         case SWLIB_ATTR_GROUP_GLOBAL:
 504                 head = dev->ops;
 505                 break;
 506         case SWLIB_ATTR_GROUP_PORT:
 507                 head = dev->port_ops;
 508                 break;
 509         case SWLIB_ATTR_GROUP_VLAN:
 510                 head = dev->vlan_ops;
 511                 break;
 512         }
 513         while(head) {
 514                 if (!strcmp(name, head->name))
 515                         return head;
 516                 head = head->next;
 517         }
 518
 519         return NULL;
 520 }
 521
 522 static void
 523 swlib_priv_free(void)
 524 {
 525         if (cache)
 526                 nl_cache_free(cache);
 527         if (handle)
 528                 nl_socket_free(handle);
 529         handle = NULL;
 530         cache = NULL;
 531 }
 532
 533 static int
 534 swlib_priv_init(void)
 535 {
 536         int ret;
 537
 538         handle = nl_socket_alloc();
 539         if (!handle) {
 540                 DPRINTF("Failed to create handle\n");
 541                 goto err;
 542         }
 543
 544         if (genl_connect(handle)) {
 545                 DPRINTF("Failed to connect to generic netlink\n");
 546                 goto err;
 547         }
 548
 549         ret = genl_ctrl_alloc_cache(handle, &cache);
 550         if (ret < 0) {
 551                 DPRINTF("Failed to allocate netlink cache\n");
 552                 goto err;
 553         }
 554
 555         family = genl_ctrl_search_by_name(cache, "switch");
 556         if (!family) {
 557                 DPRINTF("Switch API not present\n");
 558                 goto err;
 559         }
 560         return 0;
 561
 562 err:
 563         swlib_priv_free();
 564         return -EINVAL;
 565 }
 566
 567 struct swlib_scan_arg {
 568         const char *name;
 569         struct switch_dev *head;
 570         struct switch_dev *ptr;
 571 };
 572
 573 static int
 574 add_switch(struct nl_msg *msg, void *arg)
 575 {
 576         struct swlib_scan_arg *sa = arg;
 577         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 578         struct switch_dev *dev;
 579         const char *name;
 580
 581         if (nla_parse(tb, SWITCH_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL) < 0)
 582                 goto done;
 583
 584         if (!tb[SWITCH_ATTR_DEV_NAME])
 585                 goto done;
 586
 587         name = nla_get_string(tb[SWITCH_ATTR_DEV_NAME]);
 588         if (sa->name && (strcmp(name, sa->name) != 0))
 589                 goto done;
 590
 591         dev = swlib_alloc(sizeof(struct switch_dev));
 592         if (!dev)
 593                 goto done;
 594
 595         dev->dev_name = strdup(name);
 596         if (tb[SWITCH_ATTR_ID])
 597                 dev->id = nla_get_u32(tb[SWITCH_ATTR_ID]);
 598         if (tb[SWITCH_ATTR_NAME])
 599                 dev->name = strdup(nla_get_string(tb[SWITCH_ATTR_NAME]));
 600         if (tb[SWITCH_ATTR_PORTS])
 601                 dev->ports = nla_get_u32(tb[SWITCH_ATTR_PORTS]);
 602         if (tb[SWITCH_ATTR_VLANS])
 603                 dev->vlans = nla_get_u32(tb[SWITCH_ATTR_VLANS]);
 604         if (tb[SWITCH_ATTR_CPU_PORT])
 605                 dev->cpu_port = nla_get_u32(tb[SWITCH_ATTR_CPU_PORT]);
 606
 607         if (!sa->head) {
 608                 sa->head = dev;
 609                 sa->ptr = dev;
 610         } else {
 611                 sa->ptr->next = dev;
 612                 sa->ptr = dev;
 613         }
 614
 615         refcount++;
 616 done:
 617         return NL_SKIP;
 618 }
 619
 620
 621 struct switch_dev *
 622 swlib_connect(const char *name)
 623 {
 624         struct swlib_scan_arg arg;
 625         int err;
 626
 627         if (!refcount) {
 628                 if (swlib_priv_init() < 0)
 629                         return NULL;
 630         };
 631
 632         arg.head = NULL;
 633         arg.ptr = NULL;
 634         arg.name = name;
 635         swlib_call(SWITCH_CMD_GET_SWITCH, add_switch, NULL, &arg);
 636
 637         if (!refcount)
 638                 swlib_priv_free();
 639
 640         return arg.head;
 641 }
 642
 643 static void
 644 swlib_free_attributes(struct switch_attr **head)
 645 {
 646         struct switch_attr *a = *head;
 647         struct switch_attr *next;
 648
 649         while (a) {
 650                 next = a->next;
 651                 free(a);
 652                 a = next;
 653         }
 654         *head = NULL;
 655 }
 656
 657 void
 658 swlib_free(struct switch_dev *dev)
 659 {
 660         swlib_free_attributes(&dev->ops);
 661         swlib_free_attributes(&dev->port_ops);
 662         swlib_free_attributes(&dev->vlan_ops);
 663         free(dev);
 664
 665         if (--refcount == 0)
 666                 swlib_priv_free();
 667 }
 668
 669 void
 670 swlib_free_all(struct switch_dev *dev)
 671 {
 672         struct switch_dev *p;
 673
 674         while (dev) {
 675                 p = dev->next;
 676                 swlib_free(dev);
 677                 dev = p;
 678         }
 679 }