bpdu_sock.c

   1 /*****************************************************************************
   2   Copyright (c) 2006 EMC Corporation.
   3
   4   This program is free software; you can redistribute it and/or modify it
   5   under the terms of the GNU General Public License as published by the Free
   6   Software Foundation; either version 2 of the License, or (at your option)
   7   any later version.
   8
   9   This program is distributed in the hope that it will be useful, but WITHOUT
  10   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12   more details.
  13
  14   You should have received a copy of the GNU General Public License along with
  15   this program; if not, write to the Free Software Foundation, Inc., 59
  16   Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  17
  18   The full GNU General Public License is included in this distribution in the
  19   file called LICENSE.
  20
  21   Authors: Srinivas Aji <Aji_Srinivas@emc.com>
  22
  23 ******************************************************************************/
  24
  25 #include "bpdu_sock.h"
  26 #include "epoll_loop.h"
  27 #include "netif_utils.h"
  28
  29 #include <stdio.h>
  30 #include <stdlib.h>
  31 #include <errno.h>
  32 #include <string.h>
  33 #include <unistd.h>
  34 #include <stdint.h>
  35 #include <sys/socket.h>
  36 #include <sys/ioctl.h>
  37 #include <fcntl.h>
  38
  39 #include <linux/if.h>
  40 #include <linux/if_arp.h>
  41 #include <linux/llc.h>
  42
  43 #include "log.h"
  44
  45 #ifndef AF_LLC
  46 #define AF_LLC 26
  47 #endif
  48
  49 static const uint8_t stp_mc[ETH_ALEN] = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x00 };
  50
  51 void bpdu_send(struct epoll_event_handler *h, unsigned char *data, int len)
  52 {
  53   struct sockaddr_llc to;
  54   memset(&to, 0, sizeof(to));
  55   to.sllc_family = AF_LLC;
  56   to.sllc_arphrd = ARPHRD_ETHER;
  57   to.sllc_sap = LLC_SAP_BSPAN;
  58   memcpy(to.sllc_mac, stp_mc, ETH_ALEN);
  59
  60   if (fcntl(h->fd, F_SETFL, 0) < 0)
  61     ERROR("Error unsetting O_NONBLOCK: %m");
  62
  63   int l = sendto(h->fd, data, len, 0, (struct sockaddr *)&to, sizeof(to));
  64   if (l < 0)
  65     ERROR("sendto failed: %m");
  66   else if (l != len)
  67     ERROR("short write in sendto: %d instead of %d", l, len);
  68
  69   if (fcntl(h->fd, F_SETFL, O_NONBLOCK) < 0)
  70     ERROR("Error setting O_NONBLOCK: %m");
  71 }
  72
  73 void bpdu_rcv_handler(uint32_t events, struct epoll_event_handler *h)
  74 {
  75   struct sockaddr_llc from;
  76   socklen_t fromlen = sizeof(from);
  77   int cc;
  78   unsigned char buf[2048];
  79
  80   cc = recvfrom(h->fd, &buf, sizeof(buf), 0,
  81                 (struct sockaddr *) &from, &fromlen);
  82   if (cc <= 0) {
  83     ERROR("recvfrom failed: %m");
  84     return;
  85   }
  86
  87 #if 0
  88   printf("Src %02x:%02x:%02x:%02x:%02x:%02x\n",
  89          from.sllc_mac[0], from.sllc_mac[1],
  90          from.sllc_mac[2], from.sllc_mac[3],
  91          from.sllc_mac[4], from.sllc_mac[5]);
  92   int i, j;
  93   for (i = 0; i < cc; i += 16) {
  94     for (j = 0; j < 16 && i+j < cc; j++)
  95       printf(" %02x", buf[i+j]);
  96     printf("\n");
  97   }
  98   printf("\n");
  99   fflush(stdout);
 100 #endif
 101
 102   bpdu_rcv(h->arg, buf, cc);
 103 }
 104
 105
 106 /* We added name as an arg here because we can't do if_indextoname here,
 107    That needs <net/if.h> which conflicts with <linux/if.h> */
 108 /* Needs fixing. Socket should be closed in case of errors */
 109 int bpdu_sock_create(struct epoll_event_handler *h,
 110                      int if_index, char *name, struct ifdata *arg)
 111 {
 112   struct sockaddr_llc llc_addr;
 113   memset(&llc_addr, 0, sizeof(llc_addr));
 114   llc_addr.sllc_family = AF_LLC;
 115   llc_addr.sllc_arphrd = ARPHRD_ETHER;
 116   llc_addr.sllc_sap = LLC_SAP_BSPAN;
 117
 118   int s;
 119   TSTM((s = socket(AF_LLC, SOCK_DGRAM, 0)) >= 0, -1, "%m");
 120
 121   TST(get_hwaddr(name, llc_addr.sllc_mac) == 0, -1);
 122
 123   TSTM(bind(s, (struct sockaddr *) &llc_addr, sizeof(llc_addr)) == 0, -1,
 124        "Can't bind to LLC SAP %#x: %m", llc_addr.sllc_sap);
 125   {
 126     struct ifreq ifr;
 127     memset(&ifr, 0, sizeof(ifr));
 128     strncpy(ifr.ifr_name, name, IFNAMSIZ);
 129     ifr.ifr_hwaddr.sa_family = AF_UNSPEC;
 130     memcpy(ifr.ifr_hwaddr.sa_data, stp_mc, ETH_ALEN);
 131
 132     TSTM(ioctl(s, SIOCADDMULTI, &ifr) == 0, -1,
 133          "can't set multicast address for %s: %m", ifr.ifr_name);
 134   }
 135
 136   TSTM(fcntl(s, F_SETFL, O_NONBLOCK) == 0, -1, "%m");
 137
 138   h->fd = s;
 139   h->arg = arg;
 140   h->handler = bpdu_rcv_handler;
 141
 142   if (add_epoll(h) < 0)
 143     return -1;
 144
 145   return 0;
 146 }
 147
 148 void bpdu_sock_delete(struct epoll_event_handler *h)
 149 {
 150   remove_epoll(h);
 151   close(h->fd);
 152 }
 153