daemons/mrpd/mmrp.c

   1 /****************************************************************************
   2   Copyright (c) 2012, Intel Corporation
   3   All rights reserved.
   4
   5   Redistribution and use in source and binary forms, with or without
   6   modification, are permitted provided that the following conditions are met:
   7
   8    1. Redistributions of source code must retain the above copyright notice,
   9       this list of conditions and the following disclaimer.
  10
  11    2. Redistributions in binary form must reproduce the above copyright
  12       notice, this list of conditions and the following disclaimer in the
  13       documentation and/or other materials provided with the distribution.
  14
  15    3. Neither the name of the Intel Corporation nor the names of its
  16       contributors may be used to endorse or promote products derived from
  17       this software without specific prior written permission.
  18
  19   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  20   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  21   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  22   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  23   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  24   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  25   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  26   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  27   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  28   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  29   POSSIBILITY OF SUCH DAMAGE.
  30
  31 ******************************************************************************/
  32 /*
  33  * MMRP protocol (part of 802.1Q-2011)
  34  */
  35
  36 #include <stdlib.h>
  37 #include <stdio.h>
  38 #include <stddef.h>
  39 #include <string.h>
  40
  41 #include "parse.h"
  42 #include "mrpd.h"
  43 #include "mrp.h"
  44 #include "mmrp.h"
  45
  46 int mmrp_send_notifications(struct mmrp_attribute *attrib, int notify);
  47 int mmrp_txpdu(void);
  48
  49 unsigned char MMRP_ADDR[] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x20 };
  50
  51 extern unsigned char STATION_ADDR[];
  52
  53 SOCKET mmrp_socket;
  54
  55 struct mmrp_database *MMRP_db;
  56
  57 struct mmrp_attribute *mmrp_lookup(struct mmrp_attribute *rattrib)
  58 {
  59         struct mmrp_attribute *attrib;
  60         int mac_eq;
  61
  62         attrib = MMRP_db->attrib_list;
  63         while (NULL != attrib) {
  64                 if (rattrib->type == attrib->type) {
  65                         if (MMRP_SVCREQ_TYPE == attrib->type) {
  66                                 /* this makes no sense to me ...
  67                                  * its boolean .. but ..
  68                                  */
  69                                 if (attrib->attribute.svcreq ==
  70                                     rattrib->attribute.svcreq)
  71                                         return attrib;
  72                         } else {
  73                                 mac_eq = memcmp(attrib->attribute.macaddr,
  74                                                 rattrib->attribute.macaddr, 6);
  75                                 if (0 == mac_eq)
  76                                         return attrib;
  77                         }
  78                 }
  79                 attrib = attrib->next;
  80         }
  81         return NULL;
  82 }
  83
  84 int mmrp_add(struct mmrp_attribute *rattrib)
  85 {
  86         struct mmrp_attribute *attrib;
  87         struct mmrp_attribute *attrib_tail;
  88         int mac_eq;
  89
  90         /* XXX do a lookup first to guarantee uniqueness? */
  91
  92         attrib_tail = attrib = MMRP_db->attrib_list;
  93
  94         while (NULL != attrib) {
  95                 /* sort list into types, then sorted in order within types */
  96                 if (rattrib->type == attrib->type) {
  97                         if (MMRP_SVCREQ_TYPE == attrib->type) {
  98                                 if (attrib->attribute.svcreq <
  99                                     rattrib->attribute.svcreq) {
 100                                         /* possible tail insertion ... */
 101
 102                                         if (NULL != attrib->next) {
 103                                                 if (MMRP_SVCREQ_TYPE ==
 104                                                     attrib->next->type) {
 105                                                         attrib = attrib->next;
 106                                                         continue;
 107                                                 }
 108                                         }
 109
 110                                         rattrib->next = attrib->next;
 111                                         rattrib->prev = attrib;
 112                                         attrib->next = rattrib;
 113                                         return 0;
 114
 115                                 } else {
 116                                         /* head insertion ... */
 117                                         rattrib->next = attrib;
 118                                         rattrib->prev = attrib->prev;
 119                                         attrib->prev = rattrib;
 120                                         if (NULL != rattrib->prev)
 121                                                 rattrib->prev->next = rattrib;
 122                                         else
 123                                                 MMRP_db->attrib_list = rattrib;
 124
 125                                         return 0;
 126                                 }
 127                         } else {
 128                                 mac_eq = memcmp(attrib->attribute.macaddr,
 129                                                 rattrib->attribute.macaddr, 6);
 130
 131                                 if (mac_eq < 0) {
 132                                         /* possible tail insertion ... */
 133
 134                                         if (NULL != attrib->next) {
 135                                                 if (MMRP_MACVEC_TYPE ==
 136                                                     attrib->next->type) {
 137                                                         attrib = attrib->next;
 138                                                         continue;
 139                                                 }
 140                                         }
 141
 142                                         rattrib->next = attrib->next;
 143                                         rattrib->prev = attrib;
 144                                         attrib->next = rattrib;
 145                                         return 0;
 146
 147                                 } else {
 148                                         /* head insertion ... */
 149                                         rattrib->next = attrib;
 150                                         rattrib->prev = attrib->prev;
 151                                         attrib->prev = rattrib;
 152                                         if (NULL != rattrib->prev)
 153                                                 rattrib->prev->next = rattrib;
 154                                         else
 155                                                 MMRP_db->attrib_list = rattrib;
 156
 157                                         return 0;
 158                                 }
 159                         }
 160                 }
 161                 attrib_tail = attrib;
 162                 attrib = attrib->next;
 163         }
 164
 165         /* if we are here we didn't need to stitch in a a sorted entry
 166          * so append it onto the tail (if it exists)
 167          */
 168
 169         if (NULL == attrib_tail) {
 170                 rattrib->next = NULL;
 171                 rattrib->prev = NULL;
 172                 MMRP_db->attrib_list = rattrib;
 173         } else {
 174                 rattrib->next = NULL;
 175                 rattrib->prev = attrib_tail;
 176                 attrib_tail->next = rattrib;
 177         }
 178
 179         return 0;
 180 }
 181
 182 int mmrp_merge(struct mmrp_attribute *rattrib)
 183 {
 184         struct mmrp_attribute *attrib;
 185
 186         attrib = mmrp_lookup(rattrib);
 187
 188         if (NULL == attrib)
 189                 return -1;      /* shouldn't happen */
 190
 191         /* primarily we update the last mac address state for diagnostics */
 192         memcpy(attrib->registrar.macaddr, rattrib->registrar.macaddr, 6);
 193         return 0;
 194 }
 195
 196 int mmrp_event(int event, struct mmrp_attribute *rattrib)
 197 {
 198         struct mmrp_attribute *attrib;
 199
 200         switch (event) {
 201         case MRP_EVENT_LVATIMER:
 202                 mrp_jointimer_stop(&(MMRP_db->mrp_db));
 203                 /* update state */
 204                 attrib = MMRP_db->attrib_list;
 205
 206                 while (NULL != attrib) {
 207                         mrp_applicant_fsm(&(attrib->applicant), MRP_EVENT_TXLA);
 208                         mrp_registrar_fsm(&(attrib->registrar),
 209                                           &(MMRP_db->mrp_db), MRP_EVENT_TXLA);
 210                         attrib = attrib->next;
 211                 }
 212
 213                 mrp_lvatimer_fsm(&(MMRP_db->mrp_db), MRP_EVENT_LVATIMER);
 214
 215                 mmrp_txpdu();
 216                 break;
 217         case MRP_EVENT_RLA:
 218                 mrp_jointimer_stop(&(MMRP_db->mrp_db));
 219                 /* update state */
 220                 attrib = MMRP_db->attrib_list;
 221
 222                 while (NULL != attrib) {
 223                         mrp_applicant_fsm(&(attrib->applicant), MRP_EVENT_RLA);
 224                         mrp_registrar_fsm(&(attrib->registrar),
 225                                           &(MMRP_db->mrp_db), MRP_EVENT_RLA);
 226                         attrib = attrib->next;
 227                 }
 228
 229                 mrp_lvatimer_fsm(&(MMRP_db->mrp_db), MRP_EVENT_RLA);
 230
 231                 break;
 232         case MRP_EVENT_TX:
 233                 mrp_jointimer_stop(&(MMRP_db->mrp_db));
 234                 attrib = MMRP_db->attrib_list;
 235
 236                 while (NULL != attrib) {
 237                         mrp_applicant_fsm(&(attrib->applicant), MRP_EVENT_TX);
 238                         attrib = attrib->next;
 239                 }
 240
 241                 mrp_lvatimer_fsm(&(MMRP_db->mrp_db), MRP_EVENT_TX);
 242
 243                 mmrp_txpdu();
 244                 break;
 245         case MRP_EVENT_LVTIMER:
 246                 mrp_lvtimer_stop(&(MMRP_db->mrp_db));
 247                 attrib = MMRP_db->attrib_list;
 248
 249                 while (NULL != attrib) {
 250                         mrp_registrar_fsm(&(attrib->registrar),
 251                                           &(MMRP_db->mrp_db),
 252                                           MRP_EVENT_LVTIMER);
 253
 254                         attrib = attrib->next;
 255                 }
 256                 break;
 257         case MRP_EVENT_PERIODIC:
 258                 attrib = MMRP_db->attrib_list;
 259
 260                 while (NULL != attrib) {
 261                         mrp_applicant_fsm(&(attrib->applicant),
 262                                           MRP_EVENT_PERIODIC);
 263                         attrib = attrib->next;
 264                 }
 265                 break;
 266         case MRP_EVENT_NEW:
 267         case MRP_EVENT_JOIN:
 268         case MRP_EVENT_RNEW:
 269         case MRP_EVENT_RJOININ:
 270         case MRP_EVENT_RJOINMT:
 271         case MRP_EVENT_LV:
 272         case MRP_EVENT_RIN:
 273         case MRP_EVENT_RMT:
 274         case MRP_EVENT_RLV:
 275                 mrp_jointimer_start(&(MMRP_db->mrp_db));
 276                 if (NULL == rattrib)
 277                         return -1;      /* XXX internal fault */
 278
 279                 /* update state */
 280                 attrib = mmrp_lookup(rattrib);
 281
 282                 if (NULL == attrib) {
 283                         mmrp_add(rattrib);
 284                         attrib = rattrib;
 285                 } else {
 286                         mmrp_merge(rattrib);
 287                         free(rattrib);
 288                 }
 289
 290                 mrp_applicant_fsm(&(attrib->applicant), event);
 291                 /* remap local requests into registrar events */
 292                 switch (event) {
 293                 case MRP_EVENT_NEW:
 294                         mrp_registrar_fsm(&(attrib->registrar),
 295                                           &(MMRP_db->mrp_db), MRP_EVENT_RNEW);
 296                         break;
 297                 case MRP_EVENT_JOIN:
 298                         if (MRP_IN_STATE == attrib->registrar.mrp_state)
 299                                 mrp_registrar_fsm(&(attrib->registrar),
 300                                                   &(MMRP_db->mrp_db),
 301                                                   MRP_EVENT_RJOININ);
 302                         else
 303                                 mrp_registrar_fsm(&(attrib->registrar),
 304                                                   &(MMRP_db->mrp_db),
 305                                                   MRP_EVENT_RJOINMT);
 306                         break;
 307                 case MRP_EVENT_LV:
 308                         mrp_registrar_fsm(&(attrib->registrar),
 309                                           &(MMRP_db->mrp_db), MRP_EVENT_RLV);
 310                         break;
 311                 default:
 312                         mrp_registrar_fsm(&(attrib->registrar),
 313                                           &(MMRP_db->mrp_db), event);
 314                         break;
 315                 }
 316                 break;
 317         default:
 318                 break;
 319         }
 320
 321         /*
 322          * XXX should honor the MMRP_db->mrp_db.registration and
 323          * MMRP_db->mrp_db.participant controls
 324          */
 325
 326         /* generate local notifications */
 327         attrib = MMRP_db->attrib_list;
 328
 329         while (NULL != attrib) {
 330                 if (MRP_NOTIFY_NONE != attrib->registrar.notify) {
 331                         mmrp_send_notifications(attrib,
 332                                                 attrib->registrar.notify);
 333                         attrib->registrar.notify = MRP_NOTIFY_NONE;
 334                 }
 335                 attrib = attrib->next;
 336         }
 337
 338         return 0;
 339 }
 340
 341 struct mmrp_attribute *mmrp_alloc()
 342 {
 343         struct mmrp_attribute *attrib;
 344
 345         attrib = malloc(sizeof(struct mmrp_attribute));
 346         if (NULL == attrib)
 347                 return NULL;
 348
 349         memset(attrib, 0, sizeof(struct mmrp_attribute));
 350
 351         attrib->applicant.mrp_state = MRP_VO_STATE;
 352         attrib->applicant.tx = 0;
 353         attrib->applicant.sndmsg = MRP_SND_NULL;
 354         attrib->applicant.encode = MRP_ENCODE_OPTIONAL;
 355
 356         attrib->registrar.mrp_state = MRP_MT_STATE;
 357         attrib->registrar.notify = MRP_NOTIFY_NONE;
 358
 359         return attrib;
 360 }
 361
 362 void mmrp_increment_macaddr(uint8_t * macaddr)
 363 {
 364
 365         int i;
 366
 367         i = 5;
 368         while (i > 0) {
 369                 if (255 != macaddr[i]) {
 370                         macaddr[i]++;
 371                         return;
 372                 }
 373                 macaddr[i] = 0;
 374                 i--;
 375         }
 376         return;
 377 }
 378
 379 int mmrp_recv_msg()
 380 {
 381         char *msgbuf;
 382         int bytes = 0;
 383         eth_hdr_t *eth;
 384         mrpdu_t *mrpdu;
 385         mrpdu_message_t *mrpdu_msg;
 386         unsigned char *mrpdu_msg_ptr;
 387         unsigned char *mrpdu_msg_eof;
 388         mrpdu_vectorattrib_t *mrpdu_vectorptr;
 389         uint16_t numvalues;
 390         uint16_t numvalues_processed;
 391         int numvectorbytes;
 392         uint8_t vect_3pack;
 393         int vectidx;
 394         int vectevt[3];
 395         int vectevt_idx;
 396         uint8_t svcreq_firstval;
 397         uint8_t macvec_firstval[6];
 398         struct mmrp_attribute *attrib;
 399         int endmarks;
 400
 401         bytes = mrpd_recvmsgbuf(mmrp_socket, &msgbuf);
 402         if (bytes <= 0)
 403                 goto out;
 404
 405         if ((unsigned int)bytes < (sizeof(eth_hdr_t) + sizeof(mrpdu_t) +
 406                                    sizeof(mrpdu_message_t)))
 407                 goto out;
 408
 409         eth = (eth_hdr_t *) msgbuf;
 410
 411         /* note that MMRP frames should always arrive untagged (no vlan) */
 412         if (MMRP_ETYPE != ntohs(eth->typelen))
 413                 goto out;
 414
 415         /* XXX check dest mac address too? */
 416
 417         mrpdu = (mrpdu_t *) (msgbuf + sizeof(struct eth_hdr));
 418
 419         if (MMRP_PROT_VER != mrpdu->ProtocolVersion)    /* should accept */
 420                 goto out;
 421
 422         mrpdu_msg_ptr = MRPD_GET_MRPDU_MESSAGE_LIST(mrpdu);
 423
 424         mrpdu_msg_eof = (unsigned char *)mrpdu_msg_ptr;
 425         mrpdu_msg_eof += bytes;
 426         mrpdu_msg_eof -= sizeof(eth_hdr_t);
 427         mrpdu_msg_eof -= MRPD_OFFSETOF_MRPD_GET_MRPDU_MESSAGE_LIST;
 428
 429         endmarks = 0;
 430
 431         while (mrpdu_msg_ptr < (mrpdu_msg_eof - 2)) {
 432                 mrpdu_msg = (mrpdu_message_t *) mrpdu_msg_ptr;
 433                 if ((mrpdu_msg->AttributeType == 0) &&
 434                     (mrpdu_msg->AttributeLength == 0)) {
 435                         mrpdu_msg_ptr += 2;
 436                         endmarks++;
 437                         if (endmarks < 2)
 438                                 continue;       /* end-mark */
 439                         else
 440                                 break;  /* two endmarks - end of messages */
 441                 }
 442
 443                 endmarks = 0;   /* reset the endmark counter */
 444
 445                 switch (mrpdu_msg->AttributeType) {
 446                 case MMRP_SVCREQ_TYPE:
 447                         if (mrpdu_msg->AttributeLength != 1) {
 448                                 /* we could seek for an endmark to recover
 449                                  */
 450                                 goto out;
 451                         }
 452                         /* AttributeListLength not used for MMRP, hence
 453                          * Data points to the beginning of the VectorAttributes
 454                          */
 455                         mrpdu_vectorptr =
 456                             (mrpdu_vectorattrib_t *) mrpdu_msg->Data;
 457                         mrpdu_msg_ptr = (uint8_t *) mrpdu_vectorptr;
 458
 459                         while (!((mrpdu_msg_ptr[0] == 0)
 460                                  && (mrpdu_msg_ptr[1] == 0))) {
 461                                 numvalues =
 462                                     MRPDU_VECT_NUMVALUES(ntohs
 463                                                          (mrpdu_vectorptr->
 464                                                           VectorHeader));
 465
 466                                 if (0 == numvalues)
 467                                         /* Malformed - cant tell how long the trailing vectors are */
 468                                         goto out;
 469
 470                                 if ((mrpdu_vectorptr->FirstValue_VectorEvents +
 471                                      numvalues / 3) >= mrpdu_msg_eof)
 472                                         /* Malformed - runs off the end of the pdu */
 473                                         goto out;
 474
 475                                 svcreq_firstval =
 476                                     mrpdu_vectorptr->FirstValue_VectorEvents[0];
 477
 478                                 /* if not an even multiple ... */
 479                                 if (numvalues != ((numvalues / 3) * 3))
 480                                         numvectorbytes = (numvalues / 3) + 1;
 481                                 else
 482                                         numvectorbytes = (numvalues / 3);
 483
 484                                 for (vectidx = 1;
 485                                      vectidx <= (numvectorbytes + 1);
 486                                      vectidx++) {
 487                                         vect_3pack =
 488                                             mrpdu_vectorptr->
 489                                             FirstValue_VectorEvents[vectidx];
 490                                         vectevt[0] = vect_3pack / 36;
 491                                         vectevt[1] =
 492                                             (vect_3pack - vectevt[0] * 36) / 6;
 493                                         vectevt[2] =
 494                                             vect_3pack - (36 * vectevt[0]) -
 495                                             (6 * vectevt[1]);
 496
 497                                         numvalues_processed =
 498                                             (numvalues > 3 ? 3 : numvalues);
 499
 500                                         for (vectevt_idx = 0;
 501                                              vectevt_idx < numvalues_processed;
 502                                              vectevt_idx++) {
 503
 504                                                 if (1 < svcreq_firstval)        /*discard junk */
 505                                                         continue;
 506
 507                                                 attrib = mmrp_alloc();
 508                                                 if (NULL == attrib)
 509                                                         goto out;       /* oops - internal error */
 510
 511                                                 attrib->type = MMRP_SVCREQ_TYPE;
 512                                                 attrib->attribute.svcreq =
 513                                                     svcreq_firstval;
 514                                                 svcreq_firstval++;
 515                                                 memcpy(attrib->registrar.
 516                                                        macaddr, eth->srcaddr,
 517                                                        6);
 518
 519                                                 switch (vectevt[vectevt_idx]) {
 520                                                 case MRPDU_NEW:
 521                                                         mmrp_event
 522                                                             (MRP_EVENT_RNEW,
 523                                                              attrib);
 524                                                         break;
 525                                                 case MRPDU_JOININ:
 526                                                         mmrp_event
 527                                                             (MRP_EVENT_RJOININ,
 528                                                              attrib);
 529                                                         break;
 530                                                 case MRPDU_IN:
 531                                                         mmrp_event
 532                                                             (MRP_EVENT_RIN,
 533                                                              attrib);
 534                                                         break;
 535                                                 case MRPDU_JOINMT:
 536                                                         mmrp_event
 537                                                             (MRP_EVENT_RJOINMT,
 538                                                              attrib);
 539                                                         break;
 540                                                 case MRPDU_MT:
 541                                                         mmrp_event
 542                                                             (MRP_EVENT_RMT,
 543                                                              attrib);
 544                                                         break;
 545                                                 case MRPDU_LV:
 546                                                         mmrp_event
 547                                                             (MRP_EVENT_RLV,
 548                                                              attrib);
 549                                                         break;
 550                                                 default:
 551                                                         free(attrib);
 552                                                         break;
 553                                                 }
 554                                         }
 555                                         numvalues -= numvalues_processed;
 556                                 }
 557
 558                                 /* as either firstval is either 0 or 1 ... the common
 559                                  * case will be nulls for evt_2 and evt_3 ...
 560                                  */
 561                                 if (MRPDU_VECT_LVA
 562                                     (ntohs(mrpdu_vectorptr->VectorHeader)))
 563                                         mmrp_event(MRP_EVENT_RLA, NULL);
 564
 565                                 /* 2 byte numvalues + 34 byte FirstValue + (n) vector bytes */
 566                                 mrpdu_msg_ptr = (uint8_t *) mrpdu_vectorptr;
 567                                 mrpdu_msg_ptr += 3 + numvectorbytes;
 568                                 mrpdu_vectorptr =
 569                                     (mrpdu_vectorattrib_t *) mrpdu_msg_ptr;
 570                         }
 571
 572                         break;
 573                 case MMRP_MACVEC_TYPE:
 574                         if (mrpdu_msg->AttributeLength != 6) {
 575                                 /* we can seek for an endmark to recover .. but this version
 576                                  * dumps the entire packet as malformed
 577                                  */
 578                                 goto out;
 579                         }
 580                         /* AttributeListLength not used for MMRP, hence
 581                          * Data points to the beginning of the VectorAttributes
 582                          */
 583                         mrpdu_vectorptr =
 584                             (mrpdu_vectorattrib_t *) mrpdu_msg->Data;
 585                         mrpdu_msg_ptr = (uint8_t *) mrpdu_vectorptr;
 586
 587                         while (!((mrpdu_msg_ptr[0] == 0)
 588                                  && (mrpdu_msg_ptr[1] == 0))) {
 589                                 numvalues =
 590                                     MRPDU_VECT_NUMVALUES(ntohs
 591                                                          (mrpdu_vectorptr->
 592                                                           VectorHeader));
 593
 594                                 if (0 == numvalues)
 595                                         /* Malformed - cant tell how long the trailing vectors are */
 596                                         goto out;
 597
 598                                 if ((mrpdu_vectorptr->FirstValue_VectorEvents +
 599                                      numvalues / 3) >= mrpdu_msg_eof)
 600                                         /* Malformed - runs off the end of the pdu */
 601                                         goto out;
 602
 603                                 memcpy(macvec_firstval,
 604                                        mrpdu_vectorptr->FirstValue_VectorEvents,
 605                                        6);
 606
 607                                 /* if not an even multiple ... */
 608                                 if (numvalues != ((numvalues / 3) * 3))
 609                                         numvectorbytes = (numvalues / 3) + 1;
 610                                 else
 611                                         numvectorbytes = (numvalues / 3);
 612
 613                                 for (vectidx = 6;
 614                                      vectidx <= (numvectorbytes + 6);
 615                                      vectidx++) {
 616                                         vect_3pack =
 617                                             mrpdu_vectorptr->
 618                                             FirstValue_VectorEvents[vectidx];
 619                                         vectevt[0] = vect_3pack / 36;
 620                                         vectevt[1] =
 621                                             (vect_3pack - vectevt[0] * 36) / 6;
 622                                         vectevt[2] =
 623                                             vect_3pack - (36 * vectevt[0]) -
 624                                             (6 * vectevt[1]);
 625
 626                                         numvalues_processed =
 627                                             (numvalues > 3 ? 3 : numvalues);
 628
 629                                         for (vectevt_idx = 0;
 630                                              vectevt_idx < numvalues_processed;
 631                                              vectevt_idx++) {
 632
 633                                                 attrib = mmrp_alloc();
 634                                                 if (NULL == attrib)
 635                                                         goto out;       /* oops - internal error */
 636
 637                                                 attrib->type = MMRP_MACVEC_TYPE;
 638                                                 memcpy(attrib->attribute.
 639                                                        macaddr, macvec_firstval,
 640                                                        6);
 641                                                 mmrp_increment_macaddr
 642                                                     (macvec_firstval);
 643
 644                                                 memcpy(attrib->registrar.
 645                                                        macaddr, eth->srcaddr,
 646                                                        6);
 647
 648                                                 switch (vectevt[vectevt_idx]) {
 649                                                 case MRPDU_NEW:
 650                                                         mmrp_event
 651                                                             (MRP_EVENT_RNEW,
 652                                                              attrib);
 653                                                         break;
 654                                                 case MRPDU_JOININ:
 655                                                         mmrp_event
 656                                                             (MRP_EVENT_RJOININ,
 657                                                              attrib);
 658                                                         break;
 659                                                 case MRPDU_IN:
 660                                                         mmrp_event
 661                                                             (MRP_EVENT_RIN,
 662                                                              attrib);
 663                                                         break;
 664                                                 case MRPDU_JOINMT:
 665                                                         mmrp_event
 666                                                             (MRP_EVENT_RJOINMT,
 667                                                              attrib);
 668                                                         break;
 669                                                 case MRPDU_MT:
 670                                                         mmrp_event
 671                                                             (MRP_EVENT_RMT,
 672                                                              attrib);
 673                                                         break;
 674                                                 case MRPDU_LV:
 675                                                         mmrp_event
 676                                                             (MRP_EVENT_RLV,
 677                                                              attrib);
 678                                                         break;
 679                                                 default:
 680                                                         free(attrib);
 681                                                         break;
 682                                                 }
 683                                         }
 684                                         numvalues -= numvalues_processed;
 685                                 }
 686
 687                                 if (MRPDU_VECT_LVA
 688                                     (ntohs(mrpdu_vectorptr->VectorHeader)))
 689                                         mmrp_event(MRP_EVENT_RLA, NULL);
 690
 691                                 /* 1 byte Type, 1 byte Len, 6 byte FirstValue, and (n) vector bytes */
 692                                 mrpdu_msg_ptr = (uint8_t *) mrpdu_vectorptr;
 693                                 mrpdu_msg_ptr += 8 + numvectorbytes;
 694
 695                                 mrpdu_vectorptr =
 696                                     (mrpdu_vectorattrib_t *) mrpdu_msg_ptr;
 697                         }
 698                         break;
 699                 default:
 700                         /* unrecognized attribute type
 701                          * we can seek for an endmark to recover .. but this version
 702                          * dumps the entire packet as malformed
 703                          */
 704                         goto out;
 705                 }
 706         }
 707
 708         free(msgbuf);
 709         return 0;
 710  out:
 711         free(msgbuf);
 712
 713         return -1;
 714 }
 715
 716 int
 717 mmrp_emit_svcvectors(unsigned char *msgbuf, unsigned char *msgbuf_eof,
 718                      int *bytes_used, int lva)
 719 {
 720         mrpdu_vectorattrib_t *mrpdu_vectorptr;
 721         uint16_t numvalues;
 722         uint8_t vect_3pack;
 723         int vectidx;
 724         int vectevt[3];
 725         int vectevt_idx;
 726         uint8_t svcreq_firstval;
 727         struct mmrp_attribute *attrib, *vattrib;
 728         mrpdu_message_t *mrpdu_msg;
 729         unsigned char *mrpdu_msg_ptr = msgbuf;
 730         unsigned char *mrpdu_msg_eof = msgbuf_eof;
 731
 732         /* need at least 6 bytes for a single vector */
 733         if (mrpdu_msg_ptr > (mrpdu_msg_eof - 6))
 734                 goto oops;
 735
 736         mrpdu_msg = (mrpdu_message_t *) mrpdu_msg_ptr;
 737         mrpdu_msg->AttributeType = MMRP_SVCREQ_TYPE;
 738         mrpdu_msg->AttributeLength = 1;
 739
 740         attrib = MMRP_db->attrib_list;
 741
 742         mrpdu_vectorptr = (mrpdu_vectorattrib_t *) mrpdu_msg->Data;
 743
 744         while ((mrpdu_msg_ptr < (mrpdu_msg_eof - 2)) && (NULL != attrib)) {
 745
 746                 if (MMRP_SVCREQ_TYPE != attrib->type) {
 747                         attrib = attrib->next;
 748                         continue;
 749                 }
 750
 751                 if (0 == attrib->applicant.tx) {
 752                         attrib = attrib->next;
 753                         continue;
 754                 }
 755                 if (MRP_ENCODE_OPTIONAL == attrib->applicant.encode) {
 756                         attrib->applicant.tx = 0;
 757                         attrib = attrib->next;
 758                         continue;
 759                 }
 760
 761                 /* pointing to at least one attribute which needs to be transmitted */
 762                 svcreq_firstval = attrib->attribute.svcreq;
 763                 mrpdu_vectorptr->FirstValue_VectorEvents[0] =
 764                     attrib->attribute.svcreq;
 765
 766                 switch (attrib->applicant.sndmsg) {
 767                 case MRP_SND_IN:
 768                         /*
 769                          * If 'In' in indicated by the applicant attribute, the
 770                          * look at the registrar state to determine whether to
 771                          * send an In (if registrar is also In) or an Mt if the
 772                          * registrar is either Mt or Lv.
 773                          */
 774                         if (MRP_IN_STATE == attrib->registrar.mrp_state)
 775                                 vectevt[0] = MRPDU_IN;
 776                         else
 777                                 vectevt[0] = MRPDU_MT;
 778                         break;
 779                 case MRP_SND_NEW:
 780                         vectevt[0] = MRPDU_NEW;
 781                         break;
 782                 case MRP_SND_LV:
 783                         vectevt[0] = MRPDU_LV;
 784                         break;
 785                 case MRP_SND_JOIN:
 786                         /* IF 'Join' in indicated by the applicant, look at
 787                          * the corresponding registrar state to determine whether
 788                          * to send a JoinIn (if the registar state is 'In') or
 789                          * a JoinMt if the registrar state is MT or LV.
 790                          */
 791                         if (MRP_IN_STATE == attrib->registrar.mrp_state)
 792                                 vectevt[0] = MRPDU_JOININ;
 793                         else
 794                                 vectevt[0] = MRPDU_JOINMT;
 795                         break;
 796                 default:
 797                         /* huh? */
 798                         goto oops;
 799                         break;
 800                 }
 801
 802                 vectevt_idx = 1;
 803                 numvalues = 1;
 804
 805                 /* now attempt to vectorize contiguous other attributes
 806                  * which also need to be transmitted
 807                  */
 808
 809                 vectidx = 2;
 810                 vattrib = attrib->next;
 811
 812                 while (NULL != vattrib) {
 813                         if (MMRP_SVCREQ_TYPE != vattrib->type)
 814                                 break;
 815
 816                         if (0 == vattrib->applicant.tx)
 817                                 break;
 818
 819                         svcreq_firstval++;
 820
 821                         if (vattrib->attribute.svcreq != svcreq_firstval)
 822                                 break;
 823
 824                         vattrib->applicant.tx = 0;
 825
 826                         switch (vattrib->applicant.sndmsg) {
 827                         case MRP_SND_IN:
 828                                 /*
 829                                  * If 'In' in indicated by the applicant attribute, the
 830                                  * look at the registrar state to determine whether to
 831                                  * send an In (if registrar is also In) or an Mt if the
 832                                  * registrar is either Mt or Lv.
 833                                  */
 834                                 if (MRP_IN_STATE ==
 835                                     vattrib->registrar.mrp_state)
 836                                         vectevt[vectevt_idx] = MRPDU_IN;
 837                                 else
 838                                         vectevt[vectevt_idx] = MRPDU_MT;
 839                                 break;
 840                         case MRP_SND_NEW:
 841                                 vectevt[vectevt_idx] = MRPDU_NEW;
 842                                 break;
 843                         case MRP_SND_LV:
 844                                 vectevt[vectevt_idx] = MRPDU_LV;
 845                                 break;
 846                         case MRP_SND_JOIN:
 847                                 /* IF 'Join' in indicated by the applicant, look at
 848                                  * the corresponding registrar state to determine whether
 849                                  * to send a JoinIn (if the registar state is 'In') or
 850                                  * a JoinMt if the registrar state is MT or LV.
 851                                  */
 852                                 if (MRP_IN_STATE == attrib->registrar.mrp_state)
 853                                         vectevt[vectevt_idx] = MRPDU_JOININ;
 854                                 else
 855                                         vectevt[vectevt_idx] = MRPDU_JOINMT;
 856                                 break;
 857                         default:
 858                                 /* huh? */
 859                                 goto oops;
 860                                 break;
 861                         }
 862
 863                         vectevt_idx++;
 864                         numvalues++;
 865
 866                         if (vectevt_idx > 2) {
 867                                 vect_3pack = MRPDU_3PACK_ENCODE(vectevt[0],
 868                                                                 vectevt[1],
 869                                                                 vectevt[2]);
 870
 871                                 mrpdu_vectorptr->FirstValue_VectorEvents
 872                                     [vectidx] = vect_3pack;
 873                                 vectidx++;
 874                                 vectevt[0] = 0;
 875                                 vectevt[1] = 0;
 876                                 vectevt[2] = 0;
 877                                 vectevt_idx = 0;
 878                         }
 879
 880                         if (&(mrpdu_vectorptr->FirstValue_VectorEvents[vectidx])
 881                             > (mrpdu_msg_eof - 2))
 882                                 goto oops;
 883
 884                         vattrib = vattrib->next;
 885                 }
 886
 887                 /* handle any trailers */
 888                 if (vectevt_idx > 0) {
 889                         vect_3pack = MRPDU_3PACK_ENCODE(vectevt[0],
 890                                                         vectevt[1], vectevt[2]);
 891
 892                         mrpdu_vectorptr->FirstValue_VectorEvents[vectidx] =
 893                             vect_3pack;
 894                         vectidx++;
 895                 }
 896
 897                 if (&(mrpdu_vectorptr->FirstValue_VectorEvents[vectidx]) >
 898                     (mrpdu_msg_eof - 2))
 899                         goto oops;
 900
 901                 mrpdu_vectorptr->VectorHeader = MRPDU_VECT_NUMVALUES(numvalues);
 902
 903                 if (lva)
 904                         mrpdu_vectorptr->VectorHeader |= MRPDU_VECT_LVA(0xFFFF);
 905
 906                 mrpdu_vectorptr->VectorHeader =
 907                     htons(mrpdu_vectorptr->VectorHeader);
 908
 909                 /* 2 byte header, followed by FirstValues/Vectors - remember vectidx starts at 0 */
 910                 mrpdu_msg_ptr =
 911                     &(mrpdu_vectorptr->FirstValue_VectorEvents[vectidx]);
 912
 913                 attrib = attrib->next;
 914
 915                 mrpdu_vectorptr = (mrpdu_vectorattrib_t *) mrpdu_msg_ptr;
 916
 917         }
 918
 919         if (mrpdu_vectorptr == (mrpdu_vectorattrib_t *) mrpdu_msg->Data) {
 920                 *bytes_used = 0;
 921                 return 0;
 922         }
 923
 924         /* endmark */
 925         *mrpdu_msg_ptr = 0;
 926         mrpdu_msg_ptr++;
 927         *mrpdu_msg_ptr = 0;
 928         mrpdu_msg_ptr++;
 929
 930         *bytes_used = (mrpdu_msg_ptr - msgbuf);
 931         return 0;
 932  oops:
 933         /* an internal error - caller should assume TXLAF */
 934         *bytes_used = 0;
 935         return -1;
 936 }
 937
 938 int
 939 mmrp_emit_macvectors(unsigned char *msgbuf, unsigned char *msgbuf_eof,
 940                      int *bytes_used, int lva)
 941 {
 942         mrpdu_vectorattrib_t *mrpdu_vectorptr;
 943         mrpdu_message_t *mrpdu_msg;
 944         unsigned char *mrpdu_msg_ptr = msgbuf;
 945         unsigned char *mrpdu_msg_eof = msgbuf_eof;
 946         uint16_t numvalues;
 947         uint8_t vect_3pack;
 948         int vectidx;
 949         int vectevt[3];
 950         int vectevt_idx;
 951         uint8_t macvec_firstval[6];
 952         struct mmrp_attribute *attrib, *vattrib;
 953         int mac_eq;
 954
 955         /* need at least 11 bytes for a single vector */
 956         if (mrpdu_msg_ptr > (mrpdu_msg_eof - 11))
 957                 goto oops;
 958
 959         mrpdu_msg = (mrpdu_message_t *) mrpdu_msg_ptr;
 960         mrpdu_msg->AttributeType = MMRP_MACVEC_TYPE;
 961         mrpdu_msg->AttributeLength = 6;
 962
 963         attrib = MMRP_db->attrib_list;
 964
 965         mrpdu_vectorptr = (mrpdu_vectorattrib_t *) mrpdu_msg->Data;
 966
 967         while ((mrpdu_msg_ptr < (mrpdu_msg_eof - 2)) && (NULL != attrib)) {
 968
 969                 if (MMRP_MACVEC_TYPE != attrib->type) {
 970                         attrib = attrib->next;
 971                         continue;
 972                 }
 973
 974                 if (0 == attrib->applicant.tx) {
 975                         attrib = attrib->next;
 976                         continue;
 977                 }
 978                 if (MRP_ENCODE_OPTIONAL == attrib->applicant.encode) {
 979                         attrib->applicant.tx = 0;
 980                         attrib = attrib->next;
 981                         continue;
 982                 }
 983
 984                 /* pointing to at least one attribute which needs to be transmitted */
 985                 memcpy(macvec_firstval, attrib->attribute.macaddr, 6);
 986                 memcpy(mrpdu_vectorptr->FirstValue_VectorEvents,
 987                        attrib->attribute.macaddr, 6);
 988
 989                 switch (attrib->applicant.sndmsg) {
 990                 case MRP_SND_IN:
 991                         /*
 992                          * If 'In' in indicated by the applicant attribute, the
 993                          * look at the registrar state to determine whether to
 994                          * send an In (if registrar is also In) or an Mt if the
 995                          * registrar is either Mt or Lv.
 996                          */
 997                         if (MRP_IN_STATE == attrib->registrar.mrp_state)
 998                                 vectevt[0] = MRPDU_IN;
 999                         else
1000                                 vectevt[0] = MRPDU_MT;
1001                         break;
1002                 case MRP_SND_NEW:
1003                         vectevt[0] = MRPDU_NEW;
1004                         break;
1005                 case MRP_SND_LV:
1006                         vectevt[0] = MRPDU_LV;
1007                         break;
1008                 case MRP_SND_JOIN:
1009                         /* IF 'Join' in indicated by the applicant, look at
1010                          * the corresponding registrar state to determine whether
1011                          * to send a JoinIn (if the registar state is 'In') or
1012                          * a JoinMt if the registrar state is MT or LV.
1013                          */
1014                         if (MRP_IN_STATE == attrib->registrar.mrp_state)
1015                                 vectevt[0] = MRPDU_JOININ;
1016                         else
1017                                 vectevt[0] = MRPDU_JOINMT;
1018                         break;
1019                 default:
1020                         /* huh? */
1021                         goto oops;
1022                         break;
1023                 }
1024
1025                 vectevt_idx = 1;
1026                 numvalues = 1;
1027
1028                 vectevt[1] = 0;
1029                 vectevt[2] = 0;
1030
1031                 /* now attempt to vectorize contiguous other attributes
1032                  * which also need to be transmitted
1033                  */
1034
1035                 vectidx = 6;
1036                 vattrib = attrib->next;
1037
1038                 while (NULL != vattrib) {
1039                         if (MMRP_MACVEC_TYPE != vattrib->type)
1040                                 break;
1041
1042                         if (0 == vattrib->applicant.tx)
1043                                 break;
1044
1045                         mmrp_increment_macaddr(macvec_firstval);
1046
1047                         mac_eq =
1048                             memcmp(vattrib->attribute.macaddr, macvec_firstval,
1049                                    6);
1050
1051                         if (0 != mac_eq)
1052                                 break;
1053
1054                         vattrib->applicant.tx = 0;
1055
1056                         switch (vattrib->applicant.sndmsg) {
1057                         case MRP_SND_IN:
1058                                 /*
1059                                  * If 'In' in indicated by the applicant attribute, the
1060                                  * look at the registrar state to determine whether to
1061                                  * send an In (if registrar is also In) or an Mt if the
1062                                  * registrar is either Mt or Lv.
1063                                  */
1064                                 if (MRP_IN_STATE ==
1065                                     vattrib->registrar.mrp_state)
1066                                         vectevt[vectevt_idx] = MRPDU_IN;
1067                                 else
1068                                         vectevt[vectevt_idx] = MRPDU_MT;
1069                                 break;
1070                         case MRP_SND_NEW:
1071                                 vectevt[vectevt_idx] = MRPDU_NEW;
1072                                 break;
1073                         case MRP_SND_LV:
1074                                 vectevt[vectevt_idx] = MRPDU_LV;
1075                                 break;
1076                         case MRP_SND_JOIN:
1077                                 /* IF 'Join' in indicated by the applicant, look at
1078                                  * the corresponding registrar state to determine whether
1079                                  * to send a JoinIn (if the registar state is 'In') or
1080                                  * a JoinMt if the registrar state is MT or LV.
1081                                  */
1082                                 if (MRP_IN_STATE == attrib->registrar.mrp_state)
1083                                         vectevt[vectevt_idx] = MRPDU_JOININ;
1084                                 else
1085                                         vectevt[vectevt_idx] = MRPDU_JOINMT;
1086                                 break;
1087                         default:
1088                                 /* huh? */
1089                                 goto oops;
1090                                 break;
1091                         }
1092
1093                         vectevt_idx++;
1094                         numvalues++;
1095
1096                         if (vectevt_idx > 2) {
1097                                 vect_3pack = MRPDU_3PACK_ENCODE(vectevt[0],
1098                                                                 vectevt[1],
1099                                                                 vectevt[2]);
1100
1101                                 mrpdu_vectorptr->FirstValue_VectorEvents
1102                                     [vectidx] = vect_3pack;
1103                                 vectidx++;
1104                                 vectevt[0] = 0;
1105                                 vectevt[1] = 0;
1106                                 vectevt[2] = 0;
1107                                 vectevt_idx = 0;
1108                         }
1109
1110                         if (&(mrpdu_vectorptr->FirstValue_VectorEvents[vectidx])
1111                             > (mrpdu_msg_eof - 11))
1112                                 goto oops;
1113
1114                         vattrib = vattrib->next;
1115                 }
1116
1117                 /* handle any trailers */
1118                 if (vectevt_idx > 0) {
1119                         vect_3pack = MRPDU_3PACK_ENCODE(vectevt[0],
1120                                                         vectevt[1], vectevt[2]);
1121
1122                         mrpdu_vectorptr->FirstValue_VectorEvents[vectidx] =
1123                             vect_3pack;
1124                         vectidx++;
1125                 }
1126
1127                 if (&(mrpdu_vectorptr->FirstValue_VectorEvents[vectidx]) >
1128                     (mrpdu_msg_eof - 2))
1129                         goto oops;
1130
1131                 mrpdu_vectorptr->VectorHeader = MRPDU_VECT_NUMVALUES(numvalues);
1132
1133                 if (lva)
1134                         mrpdu_vectorptr->VectorHeader |= MRPDU_VECT_LVA(0xFFFF);
1135
1136                 mrpdu_vectorptr->VectorHeader =
1137                     htons(mrpdu_vectorptr->VectorHeader);
1138
1139                 /* 2 byte header, followed by FirstValues/Vectors - remember vectidx starts at 0 */
1140                 mrpdu_msg_ptr =
1141                     &(mrpdu_vectorptr->FirstValue_VectorEvents[vectidx]);
1142
1143                 attrib = attrib->next;
1144
1145                 mrpdu_vectorptr = (mrpdu_vectorattrib_t *) mrpdu_msg_ptr;
1146         }
1147
1148         if (mrpdu_vectorptr == (mrpdu_vectorattrib_t *) mrpdu_msg->Data) {
1149                 *bytes_used = 0;
1150                 return 0;
1151         }
1152
1153         /* endmark */
1154         *mrpdu_msg_ptr = 0;
1155         mrpdu_msg_ptr++;
1156         *mrpdu_msg_ptr = 0;
1157         mrpdu_msg_ptr++;
1158
1159         *bytes_used = (mrpdu_msg_ptr - msgbuf);
1160         return 0;
1161  oops:
1162         /* an internal error - caller should assume TXLAF */
1163         *bytes_used = 0;
1164         return -1;
1165 }
1166
1167 int mmrp_txpdu(void)
1168 {
1169         unsigned char *msgbuf, *msgbuf_wrptr;
1170         int msgbuf_len;
1171         int bytes = 0;
1172         eth_hdr_t *eth;
1173         mrpdu_t *mrpdu;
1174         unsigned char *mrpdu_msg_ptr;
1175         unsigned char *mrpdu_msg_eof;
1176         int rc;
1177         int lva = 0;
1178
1179         msgbuf = (unsigned char *)malloc(MAX_FRAME_SIZE);
1180         if (NULL == msgbuf)
1181                 return -1;
1182         msgbuf_len = 0;
1183
1184         msgbuf_wrptr = msgbuf;
1185
1186         eth = (eth_hdr_t *) msgbuf_wrptr;
1187
1188         /* note that MMRP frames should always be untagged (no vlan) */
1189         eth->typelen = htons(MMRP_ETYPE);
1190         memcpy(eth->destaddr, MMRP_ADDR, sizeof(eth->destaddr));
1191         memcpy(eth->srcaddr, STATION_ADDR, sizeof(eth->srcaddr));
1192
1193         msgbuf_wrptr += sizeof(eth_hdr_t);
1194
1195         mrpdu = (mrpdu_t *) msgbuf_wrptr;
1196
1197         /*
1198          * ProtocolVersion handling - a receiver must process received frames with a lesser
1199          * protcol version consistent with the older protocol processing requirements (e.g. a V2
1200          * agent receives a V1 message, the V1 message shoudl be parsed with V1 rules).
1201          *
1202          * However - if an agent receives a NEWER protocol, the agent shoudl still attempt
1203          * to parse the frame. If the agent finds an AttributeType not recognized
1204          * the agent discards the current message including any associated trailing vectors
1205          * up to the end-mark, and resumes with the next message or until the end of the PDU
1206          * is reached.
1207          *
1208          * If a VectorAttribute is found with an unknown Event for the Type, the specific
1209          * VectorAttrute is discarded and processing continues with the next VectorAttribute.
1210          */
1211
1212         mrpdu->ProtocolVersion = MMRP_PROT_VER;
1213         mrpdu_msg_ptr = MRPD_GET_MRPDU_MESSAGE_LIST(mrpdu);
1214         mrpdu_msg_eof = (unsigned char *)msgbuf + MAX_FRAME_SIZE;
1215
1216         /*
1217          * Iterate over all attributes, transmitting those marked
1218          * with 'tx', attempting to coalesce multiple contiguous tx attributes
1219          * with appropriate vector encodings.
1220          *
1221          * MMRP_MACVEC_TYPE FirstValue is the 6-byte MAC address, with
1222          * corresponding attrib_length=6
1223          *
1224          * MMRP_SVCREQ_TYPE FirstValue is a single byte - values follow,
1225          * attrib_length=1
1226          *
1227          * MMRP uses ThreePackedEvents for all vector encodings
1228          *
1229          * the expanded version of the MRPDU looks like
1230          * ProtocolVersion
1231          * AttributeType
1232          * AttributeLength
1233          * <Variable number of>
1234          *              LeaveAllEvent | NumberOfValues
1235          *              <Variable FirstValue>
1236          *              <VectorEventBytes>
1237          * EndMark
1238          *
1239          * in effect, each AttributeType present gets its own 'message' with
1240          * variable number of vectors. So with MMRP, you will have at most
1241          * two messages (SVCREQ or MACVEC)
1242          */
1243
1244         if (MMRP_db->mrp_db.lva.tx) {
1245                 lva = 1;
1246                 MMRP_db->mrp_db.lva.tx = 0;
1247         }
1248
1249         rc = mmrp_emit_macvectors(mrpdu_msg_ptr, mrpdu_msg_eof, &bytes, lva);
1250         if (-1 == rc)
1251                 goto out;
1252
1253         mrpdu_msg_ptr += bytes;
1254
1255         if (mrpdu_msg_ptr >= (mrpdu_msg_eof - 2))
1256                 goto out;
1257
1258         rc = mmrp_emit_svcvectors(mrpdu_msg_ptr, mrpdu_msg_eof, &bytes, lva);
1259         if (-1 == rc)
1260                 goto out;
1261
1262         mrpdu_msg_ptr += bytes;
1263
1264         /* endmark */
1265
1266         if (mrpdu_msg_ptr == MRPD_GET_MRPDU_MESSAGE_LIST(mrpdu)) {
1267                 free(msgbuf);
1268                 return 0;
1269         }
1270
1271         if (mrpdu_msg_ptr < (mrpdu_msg_eof - 2)) {
1272                 *mrpdu_msg_ptr = 0;
1273                 mrpdu_msg_ptr++;
1274                 *mrpdu_msg_ptr = 0;
1275                 mrpdu_msg_ptr++;
1276         } else
1277                 goto out;
1278
1279         msgbuf_len = mrpdu_msg_ptr - msgbuf;
1280
1281         bytes = mrpd_send(mmrp_socket, msgbuf, msgbuf_len, 0);
1282         if (bytes <= 0)
1283                 goto out;
1284
1285         free(msgbuf);
1286         return 0;
1287  out:
1288         free(msgbuf);
1289         /* caller should assume TXLAF */
1290         return -1;
1291 }
1292
1293 int mmrp_send_notifications(struct mmrp_attribute *attrib, int notify)
1294 {
1295         char *msgbuf;
1296         char *stage;
1297         char *variant;
1298         char *regsrc;
1299         client_t *client;
1300
1301         if (NULL == attrib)
1302                 return -1;
1303
1304         msgbuf = (char *)malloc(MAX_MRPD_CMDSZ);
1305         if (NULL == msgbuf)
1306                 return -1;
1307
1308         stage = variant = regsrc = NULL;
1309
1310         stage = (char *)malloc(128);
1311         variant = (char *)malloc(128);
1312         regsrc = (char *)malloc(128);
1313
1314         if ((NULL == stage) || (NULL == variant) || (NULL == regsrc))
1315                 goto free_msgbuf;
1316
1317         memset(msgbuf, 0, MAX_MRPD_CMDSZ);
1318
1319         if (MMRP_SVCREQ_TYPE == attrib->type) {
1320                 sprintf(variant, "S=%d", attrib->attribute.svcreq);
1321         } else {
1322                 sprintf(variant, "M=%02x%02x%02x%02x%02x%02x",
1323                         attrib->attribute.macaddr[0],
1324                         attrib->attribute.macaddr[1],
1325                         attrib->attribute.macaddr[2],
1326                         attrib->attribute.macaddr[3],
1327                         attrib->attribute.macaddr[4],
1328                         attrib->attribute.macaddr[5]);
1329         }
1330
1331         sprintf(regsrc, "R=%02x%02x%02x%02x%02x%02x",
1332                 attrib->registrar.macaddr[0],
1333                 attrib->registrar.macaddr[1],
1334                 attrib->registrar.macaddr[2],
1335                 attrib->registrar.macaddr[3],
1336                 attrib->registrar.macaddr[4], attrib->registrar.macaddr[5]);
1337         switch (attrib->registrar.mrp_state) {
1338         case MRP_IN_STATE:
1339                 sprintf(stage, "MIN %s %s\n", variant, regsrc);
1340                 break;
1341         case MRP_LV_STATE:
1342                 sprintf(stage, "MLV %s %s\n", variant, regsrc);
1343                 break;
1344         case MRP_MT_STATE:
1345                 sprintf(stage, "MMT %s %s\n", variant, regsrc);
1346                 break;
1347         default:
1348                 break;
1349         }
1350
1351         switch (notify) {
1352         case MRP_NOTIFY_NEW:
1353                 snprintf(msgbuf, MAX_MRPD_CMDSZ - 1, "MNE %s", stage);
1354                 break;
1355         case MRP_NOTIFY_JOIN:
1356                 snprintf(msgbuf, MAX_MRPD_CMDSZ - 1, "MJO %s", stage);
1357                 break;
1358         case MRP_NOTIFY_LV:
1359                 snprintf(msgbuf, MAX_MRPD_CMDSZ - 1, "MLE %s", stage);
1360                 break;
1361         default:
1362                 goto free_msgbuf;
1363                 break;
1364         }
1365
1366         client = MMRP_db->mrp_db.clients;
1367         while (NULL != client) {
1368                 mrpd_send_ctl_msg(&(client->client), msgbuf, MAX_MRPD_CMDSZ);
1369                 client = client->next;
1370         }
1371
1372  free_msgbuf:
1373         if (variant)
1374                 free(variant);
1375         if (stage)
1376                 free(stage);
1377         if (regsrc)
1378                 free(regsrc);
1379         free(msgbuf);
1380         return 0;
1381 }
1382
1383 int mmrp_dumptable(struct sockaddr_in *client)
1384 {
1385         char *msgbuf;
1386         char *msgbuf_wrptr;
1387         char *stage;
1388         char *variant;
1389         char *regsrc;
1390         struct mmrp_attribute *attrib;
1391
1392         msgbuf = (char *)malloc(MAX_MRPD_CMDSZ);
1393         if (NULL == msgbuf)
1394                 return -1;
1395
1396         stage = variant = regsrc = NULL;
1397
1398         stage = (char *)malloc(128);
1399         variant = (char *)malloc(128);
1400         regsrc = (char *)malloc(128);
1401
1402         if ((NULL == stage) || (NULL == variant) || (NULL == regsrc))
1403                 goto free_msgbuf;
1404
1405         memset(msgbuf, 0, MAX_MRPD_CMDSZ);
1406
1407         msgbuf_wrptr = msgbuf;
1408
1409         attrib = MMRP_db->attrib_list;
1410
1411         if (attrib == NULL) {
1412                 sprintf(msgbuf, "Empty");
1413         }
1414
1415         while (NULL != attrib) {
1416                 if (MMRP_SVCREQ_TYPE == attrib->type) {
1417                         sprintf(variant, "S=%d", attrib->attribute.svcreq);
1418                 } else {
1419                         sprintf(variant, "M=%02x%02x%02x%02x%02x%02x",
1420                                 attrib->attribute.macaddr[0],
1421                                 attrib->attribute.macaddr[1],
1422                                 attrib->attribute.macaddr[2],
1423                                 attrib->attribute.macaddr[3],
1424                                 attrib->attribute.macaddr[4],
1425                                 attrib->attribute.macaddr[5]);
1426                 }
1427                 sprintf(regsrc, "R=%02x%02x%02x%02x%02x%02x",
1428                         attrib->registrar.macaddr[0],
1429                         attrib->registrar.macaddr[1],
1430                         attrib->registrar.macaddr[2],
1431                         attrib->registrar.macaddr[3],
1432                         attrib->registrar.macaddr[4],
1433                         attrib->registrar.macaddr[5]);
1434                 switch (attrib->registrar.mrp_state) {
1435                 case MRP_IN_STATE:
1436                         sprintf(stage, "MIN %s %s\n", variant, regsrc);
1437                         break;
1438                 case MRP_LV_STATE:
1439                         sprintf(stage, "MLV %s %s\n", variant, regsrc);
1440                         break;
1441                 case MRP_MT_STATE:
1442                         sprintf(stage, "MMT %s %s\n", variant, regsrc);
1443                         break;
1444                 default:
1445                         break;
1446                 }
1447                 sprintf(msgbuf_wrptr, "%s", stage);
1448                 msgbuf_wrptr += strnlen(stage, 128);
1449                 attrib = attrib->next;
1450         }
1451
1452         mrpd_send_ctl_msg(client, msgbuf, MAX_MRPD_CMDSZ);
1453
1454  free_msgbuf:
1455         if (regsrc)
1456                 free(regsrc);
1457         if (variant)
1458                 free(variant);
1459         if (stage)
1460                 free(stage);
1461         free(msgbuf);
1462         return 0;
1463
1464 }
1465
1466 int mmrp_cmd_parse_mac(
1467                 char *buf, int buflen,
1468                 uint8_t * mac,
1469                 int * err_index
1470 )
1471 {
1472         struct parse_param specs[] = {
1473                 {"M" PARSE_ASSIGN, parse_mac, mac},
1474                 {0, parse_null, 0}
1475         };
1476         if (buflen < 17)
1477                 return -1;
1478         memset(mac, 0, 6);
1479         return parse(buf + 4, buflen - 4, specs, err_index);
1480 }
1481 int mmrp_cmd_mac(
1482                 uint8_t * mac,
1483                 int mrp_event)
1484 {
1485         struct mmrp_attribute *attrib;
1486         attrib = mmrp_alloc();
1487         if (NULL == attrib)
1488                 return -1;
1489         attrib->type = MMRP_MACVEC_TYPE;
1490         memcpy(attrib->attribute.macaddr, mac, 6);
1491         mmrp_event(mrp_event, attrib);
1492         return 0;
1493 }
1494 int mmrp_cmd_parse_service(
1495                 char *buf, int buflen,
1496                 uint8_t * service,
1497                 int * err_index
1498 )
1499 {
1500         struct parse_param specs[] = {
1501                 {"S" PARSE_ASSIGN, parse_u8, service},
1502                 {0, parse_null, 0}
1503         };
1504         *service = 0;
1505         if (buflen < 5)
1506                 return -1;
1507         return parse(buf + 4, buflen - 4, specs, err_index);
1508 }
1509
1510 int mmrp_cmd_service(
1511                 uint8_t service,
1512                 int mrp_event)
1513 {
1514         struct mmrp_attribute *attrib;
1515         attrib = mmrp_alloc();
1516         if (NULL == attrib)
1517                 return -1;
1518         attrib->type = MMRP_SVCREQ_TYPE;
1519         attrib->attribute.svcreq = service;
1520         mmrp_event(mrp_event, attrib);
1521         return 0;
1522 }
1523
1524 int mmrp_recv_cmd(char *buf, int buflen, struct sockaddr_in *client)
1525 {
1526         int rc;
1527         int err_index;
1528         char respbuf[8];
1529         uint8_t svcreq_param;
1530         uint8_t macvec_param[6];
1531
1532         if (NULL == MMRP_db) {
1533                 snprintf(respbuf, sizeof(respbuf) - 1, "ERC %s", buf);
1534                 mrpd_send_ctl_msg(client, respbuf, sizeof(respbuf));
1535                 goto out;
1536         }
1537
1538         rc = mrp_client_add(&(MMRP_db->mrp_db.clients), client);
1539
1540         if (buflen < 3)
1541                 return -1;
1542
1543         if ('M' != buf[0])
1544                 return -1;
1545
1546         /*
1547          * M?? - query MMRP Registrar MAC Address database
1548          * M+? - JOIN a MAC address or service declaration
1549          * M++   NEW a MAC Address (XXX: MMRP doesn't use 'New' though?)
1550          * M-- - LV a MAC address or service declaration
1551          */
1552         if (strncmp(buf, "M??", 3) == 0) {
1553                 mmrp_dumptable(client);
1554         } else if (strncmp(buf, "M--:S", 5) == 0) {
1555                 rc = mmrp_cmd_parse_service(buf, buflen, &svcreq_param,
1556                                 &err_index);
1557                 if (rc)
1558                         goto out_ERP;
1559                 rc = mmrp_cmd_service(svcreq_param, MRP_EVENT_LV);
1560                 if (rc)
1561                         goto out_ERI;
1562         } else if ( strncmp(buf, "M--:M", 5) == 0) {
1563                 /*
1564                  * XXX note could also register VID with mac address if we ever wanted to
1565                  * support more than one Spanning Tree context
1566                  */
1567
1568                 /* buf[] should look similar to 'M--:M=010203040506' */
1569                 rc = mmrp_cmd_parse_mac(buf, buflen, macvec_param,
1570                                 &err_index);
1571                 if (rc)
1572                         goto out_ERP;
1573                 rc = mmrp_cmd_mac(macvec_param, MRP_EVENT_LV);
1574                 if (rc)
1575                         goto out_ERI;
1576         } else if ((strncmp(buf, "M++:S", 5) == 0) || (strncmp(buf, "M+?S", 4) == 0)){
1577                 /* buf[] should look similar to 'M+?:S=1' or 'M++:S=1'
1578                  */
1579                 rc = mmrp_cmd_parse_service(buf, buflen, &svcreq_param,
1580                                 &err_index);
1581                 if (rc)
1582                         goto out_ERP;
1583                 rc = mmrp_cmd_service(svcreq_param, MRP_EVENT_JOIN);
1584                 if (rc)
1585                         goto out_ERI;
1586         } else if ( strncmp(buf, "M++:M", 5) == 0) {
1587                 /* buf[] should look similar to 'M+?:M=010203040506' */
1588                 rc = mmrp_cmd_parse_mac(buf, buflen, macvec_param,
1589                                 &err_index);
1590                 if (rc)
1591                         goto out_ERP;
1592                 rc = mmrp_cmd_mac(macvec_param, MRP_EVENT_JOIN);
1593                 if (rc)
1594                         goto out_ERI;
1595         } else {
1596                 snprintf(respbuf, sizeof(respbuf) - 1, "ERC %s", buf);
1597                 mrpd_send_ctl_msg(client, respbuf, sizeof(respbuf));
1598                 goto out;
1599         }
1600         return 0;
1601
1602
1603 out_ERI:
1604         snprintf(respbuf, sizeof(respbuf) - 1, "ERI %s", buf);
1605         mrpd_send_ctl_msg(client, respbuf, sizeof(respbuf));
1606         goto out;
1607
1608 out_ERP:
1609         snprintf(respbuf, sizeof(respbuf) - 1, "ERP %s", buf);
1610         mrpd_send_ctl_msg(client, respbuf, sizeof(respbuf));
1611         goto out;
1612
1613  out:
1614         return -1;
1615 }
1616
1617 int mmrp_init(int mmrp_enable)
1618 {
1619         int rc;
1620
1621         /* XXX doesn't handle re-start */
1622
1623         mmrp_socket = INVALID_SOCKET;
1624         MMRP_db = NULL;
1625
1626         if (0 == mmrp_enable) {
1627                 return 0;
1628         }
1629
1630         rc = mrpd_init_protocol_socket(MMRP_ETYPE, &mmrp_socket, MMRP_ADDR);
1631         if (rc < 0)
1632                 return -1;
1633
1634         MMRP_db = malloc(sizeof(struct mmrp_database));
1635
1636         if (NULL == MMRP_db)
1637                 goto abort_socket;
1638
1639         memset(MMRP_db, 0, sizeof(struct mmrp_database));
1640
1641         /* if registration is FIXED or FORBIDDEN
1642          * updates from MRP are discarded, and
1643          * only IN and JOININ messages are sent
1644          */
1645         MMRP_db->mrp_db.registration = MRP_REGISTRAR_CTL_NORMAL;        /* default */
1646
1647         /* if participant role is 'SILENT' (or non-participant)
1648          * applicant doesn't send any messages -
1649          *
1650          * Note - theoretically configured per-attribute
1651          */
1652         MMRP_db->mrp_db.participant = MRP_APPLICANT_CTL_NORMAL; /* default */
1653
1654         rc = mrpd_init_timers(&(MMRP_db->mrp_db));
1655
1656         if (rc < 0)
1657                 goto abort_alloc;
1658
1659         mrp_lvatimer_fsm(&(MMRP_db->mrp_db), MRP_EVENT_BEGIN);
1660         return 0;
1661
1662  abort_alloc:
1663         /* free MMRP_db and related structures */
1664         free(MMRP_db);
1665         MMRP_db = NULL;
1666  abort_socket:
1667         mrpd_close_socket(mmrp_socket);
1668         mmrp_socket = INVALID_SOCKET;
1669         /* XXX */
1670         return -1;
1671
1672         return 0;
1673 }
1674
1675 int mmrp_reclaim(void)
1676 {
1677         struct mmrp_attribute *mattrib, *free_mattrib;
1678
1679         if (NULL == MMRP_db)
1680                 return 0;
1681
1682         mattrib = MMRP_db->attrib_list;
1683         while (NULL != mattrib) {
1684                 if ((mattrib->registrar.mrp_state == MRP_MT_STATE) &&
1685                     ((mattrib->applicant.mrp_state == MRP_VO_STATE) ||
1686                      (mattrib->applicant.mrp_state == MRP_AO_STATE) ||
1687                      (mattrib->applicant.mrp_state == MRP_QO_STATE))) {
1688                         if (NULL != mattrib->prev)
1689                                 mattrib->prev->next = mattrib->next;
1690                         else
1691                                 MMRP_db->attrib_list = mattrib->next;
1692                         if (NULL != mattrib->next)
1693                                 mattrib->next->prev = mattrib->prev;
1694                         free_mattrib = mattrib;
1695                         mattrib = mattrib->next;
1696                         mmrp_send_notifications(free_mattrib, MRP_NOTIFY_LV);
1697                         free(free_mattrib);
1698                 } else
1699                         mattrib = mattrib->next;
1700         }
1701         return 0;
1702 }
1703
1704 void mmrp_bye(struct sockaddr_in *client)
1705 {
1706         if (NULL != MMRP_db)
1707                 mrp_client_delete(&(MMRP_db->mrp_db.clients), client);
1708 }