src/ctl.c

   1 /*
   2  * Copyright (c) 2008 Vincent Bernat <bernat@luffy.cx>
   3  *
   4  * Permission to use, copy, modify, and distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
  11  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  13  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  14  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16
  17 #include "lldpd.h"
  18
  19 #include <unistd.h>
  20 #include <errno.h>
  21 #include <sys/types.h>
  22 #include <sys/socket.h>
  23 #include <sys/un.h>
  24
  25 int
  26 ctl_create(char *name)
  27 {
  28         int s;
  29         struct sockaddr_un su;
  30         int rc;
  31
  32         if ((s = socket(PF_UNIX, SOCK_STREAM, 0)) == -1)
  33                 return -1;
  34         su.sun_family = AF_UNIX;
  35         strlcpy(su.sun_path, name, UNIX_PATH_MAX);
  36         if (bind(s, (struct sockaddr *)&su, sizeof(struct sockaddr_un)) == -1) {
  37                 rc = errno; close(s); errno = rc;
  38                 return -1;
  39         }
  40         if (listen(s, 5) == -1) {
  41                 rc = errno; close(s); errno = rc;
  42                 return -1;
  43         }
  44         return s;
  45 }
  46
  47 int
  48 ctl_connect(char *name)
  49 {
  50         int s;
  51         struct sockaddr_un su;
  52         int rc;
  53
  54         if ((s = socket(PF_UNIX, SOCK_STREAM, 0)) == -1)
  55                 return -1;
  56         su.sun_family = AF_UNIX;
  57         strlcpy(su.sun_path, name, UNIX_PATH_MAX);
  58         if (connect(s, (struct sockaddr *)&su, sizeof(struct sockaddr_un)) == -1) {
  59                 rc = errno;
  60                 LLOG_WARN("unable to connect to socket " LLDPD_CTL_SOCKET);
  61                 errno = rc; return -1;
  62         }
  63         return s;
  64 }
  65
  66 int
  67 ctl_accept(struct lldpd *cfg, int c)
  68 {
  69         int s;
  70         struct lldpd_client *lc;
  71         if ((s = accept(c, NULL, NULL)) == -1) {
  72                 LLOG_WARN("unable to accept connection from socket");
  73                 return -1;
  74         }
  75         if ((lc = (struct lldpd_client *)malloc(sizeof(
  76                             struct lldpd_client))) == NULL) {
  77                 LLOG_WARN("failed to allocate memory for new client");
  78                 close(s);
  79                 return -1;
  80         }
  81         lc->fd = s;
  82         TAILQ_INSERT_TAIL(&cfg->g_clients, lc, next);
  83         return 1;
  84 }
  85
  86 void
  87 ctl_msg_init(struct hmsg *t, enum hmsg_type type)
  88 {
  89         t->hdr.type = type;
  90         t->hdr.len = 0;
  91         t->hdr.pid = getpid();
  92 }
  93
  94 int
  95 ctl_msg_send(int fd, struct hmsg *t)
  96 {
  97         return write(fd, t, t->hdr.len + sizeof(struct hmsg_hdr));
  98 }
  99
 100 int
 101 ctl_msg_recv(int fd, struct hmsg *t)
 102 {
 103         int n;
 104         if ((n = read(fd, t, MAX_HMSGSIZE)) == -1) {
 105                 return -1;
 106         }
 107         if (n < sizeof(struct hmsg_hdr)) {
 108                 LLOG_WARNX("message received too short");
 109                 errno = 0;
 110                 return -1;
 111         }
 112         if (n != sizeof(struct hmsg_hdr) + t->hdr.len) {
 113                 LLOG_WARNX("message from %d seems to be truncated (or too large)",
 114                         t->hdr.pid);
 115                 errno = 0;
 116                 return -1;
 117         }
 118         return 1;
 119 }
 120
 121 int
 122 ctl_close(struct lldpd *cfg, int c)
 123 {
 124         struct lldpd_client *client, *client_next;
 125         for (client = TAILQ_FIRST(&cfg->g_clients);
 126             client != NULL;
 127             client = client_next) {
 128                 client_next = TAILQ_NEXT(client, next);
 129                 if (client->fd == c) {
 130                         close(client->fd);
 131                         TAILQ_REMOVE(&cfg->g_clients, client, next);
 132                         free(client);
 133                         return 1;
 134                 }
 135         }
 136         /* Not found */
 137         return -1;
 138 }
 139
 140 void
 141 ctl_cleanup(char *name)
 142 {
 143         if (unlink(name) == -1)
 144                 LLOG_WARN("unable to unlink %s", name);
 145 }
 146
 147 /* Packing/unpacking */
 148
 149 /* This structure is used to track memory allocation when unpacking */
 150 struct gc {
 151         TAILQ_ENTRY(gc) next;
 152         void *pointer;
 153 };
 154 TAILQ_HEAD(gc_l, gc);
 155
 156 typedef struct { char c; int16_t x; } st_int16;
 157 typedef struct { char c; int32_t x; } st_int32;
 158 typedef struct { char c; void *x; } st_void_p;
 159
 160 #define INT16_ALIGN (sizeof(st_int16) - sizeof(int16_t))
 161 #define INT32_ALIGN (sizeof(st_int32) - sizeof(int32_t))
 162 #define VOID_P_ALIGN (sizeof(st_void_p) - sizeof(void *))
 163
 164 struct formatdef {
 165         char format;
 166         int  size;
 167         int  alignment;
 168         int (*pack)(struct hmsg*, void **, void *,
 169             const struct formatdef *);
 170         int (*unpack)(struct hmsg*, void **, void *,
 171             const struct formatdef *, struct gc_l *);
 172 };
 173
 174 /* void** is a pointer to a pointer to the end of struct hmsg*. It should be
 175  * updated. void* is a pointer to the entity to pack */
 176
 177 int
 178 ctl_alloc_pointer(struct gc_l *pointers, void *pointer)
 179 {
 180         struct gc *gpointer;
 181         if (pointers != NULL) {
 182                 if ((gpointer = (struct gc *)calloc(1,
 183                             sizeof(struct gc))) == NULL) {
 184                         LLOG_WARN("unable to allocate memory for garbage collector");
 185                         return -1;
 186                 }
 187                 gpointer->pointer = pointer;
 188                 TAILQ_INSERT_TAIL(pointers, gpointer, next);
 189         }
 190         return 0;
 191 }
 192
 193 void
 194 ctl_free_pointers(struct gc_l *pointers, int listonly)
 195 {
 196         struct gc *pointer, *pointer_next;
 197         for (pointer = TAILQ_FIRST(pointers);
 198              pointer != NULL;
 199              pointer = pointer_next) {
 200                 pointer_next = TAILQ_NEXT(pointer, next);
 201                 TAILQ_REMOVE(pointers, pointer, next);
 202                 if (!listonly)
 203                         free(pointer->pointer);
 204                 free(pointer);
 205         }
 206 }
 207
 208 int
 209 pack_copy(struct hmsg *h, void **p, void *s,
 210     const struct formatdef *ct)
 211 {
 212         if (h->hdr.len + ct->size > MAX_HMSGSIZE - sizeof(struct hmsg_hdr)) {
 213                 LLOG_WARNX("message became too large");
 214                 return -1;
 215         }
 216         memcpy(*p, s, ct->size);
 217         *p += ct->size;
 218         h->hdr.len += ct->size;
 219         return ct->size;
 220 }
 221
 222 int
 223 unpack_copy(struct hmsg *h, void **p, void *s,
 224     const struct formatdef *ct, struct gc_l *pointers)
 225 {
 226         memcpy(s, *p, ct->size);
 227         *p += ct->size;
 228         return ct->size;
 229 }
 230
 231 int
 232 pack_string(struct hmsg *h, void **p, void *s,
 233     const struct formatdef *ct)
 234 {
 235         int len, ss;
 236         if ((*(char**)s) == NULL)
 237                 len = -1;
 238         else
 239                 len = strlen(*(char**)s);
 240         if (h->hdr.len + len + sizeof(int) > MAX_HMSGSIZE -
 241             sizeof(struct hmsg_hdr)) {
 242                 LLOG_WARNX("message became too large");
 243                 return -1;
 244         }
 245         memcpy(*p, &len, sizeof(int));
 246         *p += sizeof(int);
 247         ss = sizeof(int);
 248         if (len != -1) {
 249                 memcpy(*p, *(char **)s, len);
 250                 *p += len;
 251                 ss += len;
 252         }
 253         h->hdr.len += ss;
 254         return ss;
 255 }
 256
 257 int
 258 unpack_string(struct hmsg *h, void **p, void *s,
 259     const struct formatdef *ct, struct gc_l *pointers)
 260 {
 261         char *string;
 262         int len = *(int*)*p;
 263         *p += sizeof(int);
 264         if (len == -1) {
 265                 string = NULL;
 266         } else {
 267                 if ((string = (char *)calloc(1, len + 1)) == NULL) {
 268                         LLOG_WARNX("unable to allocate new string");
 269                         return -1;
 270                 }
 271                 if (ctl_alloc_pointer(pointers, string) == -1) {
 272                         free(string);
 273                         return -1;
 274                 }
 275                 memcpy(string, *p, len);
 276                 *p += len;
 277         }
 278         memcpy(s, &string, sizeof(char *));
 279         return sizeof(char*);
 280 }
 281
 282 int
 283 pack_chars(struct hmsg *h, void **p, void *s,
 284     const struct formatdef *ct)
 285 {
 286         char *string;
 287         int string_len;
 288         string = *(char **)s;
 289         s += sizeof(char *);
 290         string_len = *(int *)s;
 291
 292         if (h->hdr.len + string_len + sizeof(int) > MAX_HMSGSIZE -
 293             sizeof(struct hmsg_hdr)) {
 294                 LLOG_WARNX("message became too large");
 295                 return -1;
 296         }
 297         memcpy(*p, &string_len, sizeof(int));
 298         *p += sizeof(int);
 299         memcpy(*p, string, string_len);
 300         *p += string_len;
 301         h->hdr.len += sizeof(int) + string_len;
 302         return sizeof(int) + string_len;
 303 }
 304
 305 int
 306 unpack_chars(struct hmsg *h, void **p, void *s,
 307     const struct formatdef *ct, struct gc_l *pointers)
 308 {
 309         char *string;
 310         struct {
 311                 char *string;
 312                 int len;
 313         } reals __attribute__ ((__packed__));
 314         int len = *(int*)*p;
 315         *p += sizeof(int);
 316         if ((string = (char *)malloc(len)) == NULL) {
 317                 LLOG_WARN("unable to allocate new string");
 318                 return -1;
 319         }
 320         if (ctl_alloc_pointer(pointers, string) == -1) {
 321                 free(string);
 322                 return -1;
 323         }
 324         memcpy(string, *p, len);
 325         *p += len;
 326         reals.string = string;
 327         reals.len = len;
 328         memcpy(s, &reals, sizeof(reals));
 329         return sizeof(char*);
 330 }
 331
 332 int
 333 pack_zero(struct hmsg *h, void **p, void *s,
 334     const struct formatdef *ct)
 335 {
 336         if (h->hdr.len + ct->size > MAX_HMSGSIZE - sizeof(struct hmsg_hdr)) {
 337                 LLOG_WARNX("message became too large");
 338                 return -1;
 339         }
 340         memset(*p, 0, ct->size);
 341         *p += ct->size;
 342         h->hdr.len += ct->size;
 343         return ct->size;
 344 }
 345
 346 static struct formatdef conv_table[] = {
 347         {'b',   1,                              0,
 348          pack_copy,     unpack_copy},
 349         {'w',   2,                              INT16_ALIGN,
 350          pack_copy,     unpack_copy},
 351         {'l',   4,                              INT32_ALIGN,
 352          pack_copy,     unpack_copy},
 353         /* Null terminated string */
 354         {'s',   sizeof(void*),                  VOID_P_ALIGN,
 355          pack_string,   unpack_string},
 356         /* Pointer (is packed with 0) */
 357         {'P',   sizeof(void*),                  VOID_P_ALIGN,
 358          pack_zero,     unpack_copy},
 359         /* A list (same as pointer), should be at the beginning */
 360         {'L',   sizeof(void*)*2,                VOID_P_ALIGN,
 361          pack_zero,     unpack_copy},
 362         /* Non null terminated string, followed by an int for the size */
 363         {'C',   sizeof(void*) + sizeof(int),    VOID_P_ALIGN,
 364          pack_chars,    unpack_chars},
 365         {0}
 366 };
 367
 368 /* Lists can be packed only if the "next" member is the first one of the
 369  * structure! No check is done for this. */
 370 struct fakelist_m {
 371         TAILQ_ENTRY(fakelist_m)  next;
 372         void *data;
 373 };
 374 TAILQ_HEAD(fakelist_l, fakelist_m);
 375
 376 int ctl_msg_get_alignment(char *format)
 377 {
 378         char *f;
 379         int maxalign = 0, align;
 380         int paren = 0;
 381         struct formatdef *ce;
 382
 383         /* We just want to get the maximum required alignment for the
 384          * structure. Instead of going recursive, we just count parentheses to
 385          * get the end of the structure. */
 386         for (f = format; *f != 0; f++) {
 387                 if (*f == ')') {
 388                         paren--;
 389                         if (!paren)
 390                                 return maxalign;
 391                         continue;
 392                 } else if (*f == '(') {
 393                         paren++;
 394                         continue;
 395                 } else {
 396                         for (ce = conv_table;
 397                              (ce->format != 0) && (ce->format != *f);
 398                              ce++);
 399                         align = ce->alignment;
 400                 }
 401                 if (align != 0)
 402                         maxalign = (maxalign>align)?maxalign:align;
 403         }
 404         if (paren)
 405                 LLOG_WARNX("unbalanced parenthesis in format '%s'",
 406                     format);
 407         return maxalign;
 408 }
 409
 410 /* Define a stack of align values */
 411 struct stack_align {
 412         SLIST_ENTRY(stack_align) next;
 413         int                      align;
 414 };
 415
 416 int
 417 ctl_msg_packunpack_structure(char *format, void *structure, unsigned int size,
 418     struct hmsg *h, void **p, struct gc_l *pointers, int pack)
 419 {
 420         char *f;
 421         struct formatdef *ce = NULL;
 422         unsigned int csize = 0;
 423         uintptr_t offset;
 424         struct stack_align *align, *align_next;
 425         int talign;
 426         SLIST_HEAD(, stack_align) aligns;
 427
 428         SLIST_INIT(&aligns);
 429         for (f = format; *f != 0; f++) {
 430                 /* If we have a substructure, when entering into the structure,
 431                  * we get the alignment and push it to the stack. When exiting
 432                  * the structure, we pop the alignment from the stack and we do
 433                  * the padding. This means that the whole structure should be
 434                  * enclosed into parentheses, otherwise the padding won't
 435                  * occur. */
 436                 ce = NULL;
 437                 if (*f == '(') {
 438                         if ((align = calloc(1,
 439                                     sizeof(struct stack_align))) == NULL) {
 440                                 LLOG_WARN("unable to allocate memory "
 441                                     "for alignment stack");
 442                                 goto packunpack_error;
 443                         }
 444                         talign = align->align = ctl_msg_get_alignment(f);
 445                         SLIST_INSERT_HEAD(&aligns, align, next);
 446                 } else if (*f == ')') {
 447                         /* Pad the structure */
 448                         align = SLIST_FIRST(&aligns);
 449                         talign = align->align;
 450                         if ((talign > 0) && (csize % talign != 0)) {
 451                                 structure += talign - (csize % talign);
 452                                 csize += talign - (csize % talign);
 453                         }
 454                         align_next = SLIST_NEXT(align, next);
 455                         SLIST_REMOVE_HEAD(&aligns, next);
 456                         free(align);
 457                         continue;
 458                 } else {
 459                         for (ce = conv_table;
 460                              (ce->format != 0) && (ce->format != *f);
 461                              ce++);
 462                         if (ce->format != *f) {
 463                                 LLOG_WARNX("unknown format char %c", *f);
 464                                 goto packunpack_error;
 465                         }
 466                         talign = ce->alignment;
 467                 }
 468
 469                 /* Align the structure member */
 470                 if (talign != 0) {
 471                         offset = (uintptr_t)structure % talign;
 472                         if (offset != 0) {
 473                                 structure += talign - offset;
 474                                 csize += talign - offset;
 475                         }
 476                 }
 477
 478                 if (!ce) continue;
 479
 480                 /* Check that the size is still ok */
 481                 csize += ce->size;
 482                 if (csize > size) {
 483                         LLOG_WARNX("size of structure is too small for given "
 484                             "format (%d vs %d)", size, csize);
 485                         goto packunpack_error;
 486                 }
 487
 488                 /* Pack or unpack */
 489                 if (pack) {
 490                         if (ce->pack(h, p, structure, ce) == -1) {
 491                                 LLOG_WARNX("error while packing %c in %s", *f,
 492                                     format);
 493                                 goto packunpack_error;
 494                         }
 495                 } else {
 496                         if (ce->unpack(h, p, structure, ce, pointers) == -1) {
 497                                 LLOG_WARNX("error while unpacking %c", *f);
 498                                 goto packunpack_error;
 499                         }
 500                 }
 501                 structure += ce->size;
 502         }
 503
 504         if (size < csize) {
 505                 LLOG_WARNX("size of structure does not match its "
 506                     "declaration (%d vs %d)", size, csize);
 507                 goto packunpack_error;
 508         }
 509         if (!SLIST_EMPTY(&aligns)) {
 510                 LLOG_WARNX("format is badly balanced ('%s')", format);
 511                 goto packunpack_error;
 512         }
 513         return 0;
 514
 515 packunpack_error:
 516         for (align = SLIST_FIRST(&aligns);
 517              align != NULL;
 518              align = align_next) {
 519                 align_next = SLIST_NEXT(align, next);
 520                 SLIST_REMOVE_HEAD(&aligns, next);
 521                 free(align);
 522         }
 523         return -1;
 524
 525 }
 526
 527 int
 528 ctl_msg_pack_structure(char *format, void *structure, unsigned int size,
 529     struct hmsg *h, void **p)
 530 {
 531         return ctl_msg_packunpack_structure(format, structure, size, h, p, NULL, 1);
 532 }
 533
 534 int
 535 ctl_msg_unpack_structure(char *format, void *structure, unsigned int size,
 536     struct hmsg *h, void **p)
 537 {
 538         struct gc_l pointers;
 539         int rc;
 540         TAILQ_INIT(&pointers);
 541         if ((rc = ctl_msg_packunpack_structure(format, structure, size,
 542                     h, p, &pointers, 0)) == -1) {
 543                 LLOG_WARNX("unable to unpack structure, freeing");
 544                 ctl_free_pointers(&pointers, 0);
 545                 return -1;
 546         }
 547         ctl_free_pointers(&pointers, 1);
 548         return rc;
 549 }
 550
 551 int
 552 ctl_msg_pack_list(char *format, void *list, unsigned int size, struct hmsg *h, void **p)
 553 {
 554         struct fakelist_m *member;
 555         struct fakelist_l *flist = (struct fakelist_l *)list;
 556         TAILQ_FOREACH(member, flist, next) {
 557                 if (ctl_msg_pack_structure(format, member, size, h, p) == -1) {
 558                         LLOG_WARNX("error while packing list, aborting");
 559                         return -1;
 560                 }
 561         }
 562         return 0;
 563 }
 564
 565 int
 566 ctl_msg_unpack_list(char *format, void *list, unsigned int size, struct hmsg *h, void **p)
 567 {
 568         struct fakelist_m *member, *member_next;
 569         struct gc_l pointers;
 570         struct fakelist_l *flist = (struct fakelist_l *)list;
 571         TAILQ_INIT(flist);
 572         TAILQ_INIT(&pointers);
 573         while (*p - (void *)h - sizeof(struct hmsg_hdr) < h->hdr.len) {
 574                 if ((member = calloc(1, size)) == NULL) {
 575                         LLOG_WARN("unable to allocate memory for structure");
 576                         return -1;
 577                 }
 578                 if (ctl_msg_packunpack_structure(format, member, size,
 579                         h, p, &pointers, 0) == -1) {
 580                         LLOG_WARNX("unable to unpack list, aborting");
 581                         free(member);
 582                         /* Free each list member */
 583                         for (member = TAILQ_FIRST(flist);
 584                              member != NULL;
 585                              member = member_next) {
 586                                 member_next = TAILQ_NEXT(member, next);
 587                                 TAILQ_REMOVE(flist, member, next);
 588                                 free(member);
 589                         }
 590                         ctl_free_pointers(&pointers, 0);
 591                         return -1;
 592                 }
 593                 TAILQ_INSERT_TAIL(flist, member, next);
 594         }
 595         ctl_free_pointers(&pointers, 1);
 596         return 0;
 597 }