Logo Search packages:      
Sourcecode: zaptel version File versions

ztd-eth.c

/*
 * Dynamic Span Interface for Zaptel (Ethernet Interface)
 *
 * Written by Mark Spencer <markster@linux-support.net>
 *
 * Copyright (C) 2001, Linux Support Services, Inc.
 *
 * All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 */

#include <linux/kernel.h>
#include <linux/errno.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/spinlock.h>
#include <linux/slab.h>
#include <linux/kmod.h>
#include <linux/netdevice.h>
#include <linux/notifier.h>

#ifdef CONFIG_DEVFS_FS
#include <linux/devfs_fs_kernel.h>
#endif
#ifdef STANDALONE_ZAPATA
#include "zaptel.h"
#else
#include <linux/zaptel.h>
#endif

#define ETH_P_ZTDETH    0xd00d

struct ztdeth_header {
      unsigned short subaddr;
};

/* We take the raw message, put it in an ethernet frame, and add a
   two byte addressing header at the top for future use */

static spinlock_t zlock = SPIN_LOCK_UNLOCKED;

static struct ztdeth {
      unsigned char addr[ETH_ALEN];
      unsigned short subaddr; /* Network byte order */
      struct zt_span *span;
      char ethdev[IFNAMSIZ];
      struct net_device *dev;
      struct ztdeth *next;
} *zdevs = NULL;

struct zt_span *ztdeth_getspan(unsigned char *addr, unsigned short subaddr)
{
      unsigned long flags;
      struct ztdeth *z;
      struct zt_span *span = NULL;
      spin_lock_irqsave(&zlock, flags);
      z = zdevs;
      while(z) {
            if (!memcmp(addr, z->addr, ETH_ALEN) &&
                  z->subaddr == subaddr)
                  break;
            z = z->next;
      }
      if (z)
            span = z->span;
      spin_unlock_irqrestore(&zlock, flags);
      return span;
}

static int ztdeth_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt)
{
      struct zt_span *span;
      struct ztdeth_header *zh;
      zh = (struct ztdeth_header *)skb->nh.raw;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,9)
        span = ztdeth_getspan(eth_hdr(skb)->h_source, zh->subaddr);
#else
      span = ztdeth_getspan(skb->mac.ethernet->h_source, zh->subaddr);
#endif
      if (span) {
            skb_pull(skb, sizeof(struct ztdeth_header));
            zt_dynamic_receive(span, (unsigned char *)skb->data, skb->len);
      }
      kfree_skb(skb);
      return 0;
}

static int ztdeth_notifier(struct notifier_block *block, unsigned long event, void *ptr)
{
      struct net_device *dev = ptr;
      struct ztdeth *z;
      unsigned long flags;
      switch(event) {
      case NETDEV_GOING_DOWN:
      case NETDEV_DOWN:
            spin_lock_irqsave(&zlock, flags);
            z = zdevs;
            while(z) {
                  /* Note that the device no longer exists */
                  if (z->dev == dev)
                        z->dev = NULL;
                  z = z->next;
            }
            spin_unlock_irqrestore(&zlock, flags);
            break;
      case NETDEV_UP:
            spin_lock_irqsave(&zlock, flags);
            z = zdevs;
            while(z) {
                  /* Now that the device exists again, use it */
                  if (!strcmp(z->ethdev, dev->name))
                        z->dev = dev;
                  z = z->next;
            }
            spin_unlock_irqrestore(&zlock, flags);
            break;
      }
      return 0;
}

static int ztdeth_transmit(void *pvt, unsigned char *msg, int msglen)
{
      struct ztdeth *z;
      struct sk_buff *skb;
      struct ztdeth_header *zh;
      unsigned long flags;
      struct net_device *dev;
      unsigned char addr[ETH_ALEN];
      unsigned short subaddr; /* Network byte order */

      spin_lock_irqsave(&zlock, flags);
      z = pvt;
      if (z->dev) {
            /* Copy fields to local variables to remove spinlock ASAP */
            dev = z->dev;
            memcpy(addr, z->addr, sizeof(z->addr));
            subaddr = z->subaddr;
            spin_unlock_irqrestore(&zlock, flags);
            skb = dev_alloc_skb(msglen + dev->hard_header_len + sizeof(struct ztdeth_header) + 32);
            if (skb) {
                  /* Reserve header space */
                  skb_reserve(skb, dev->hard_header_len + sizeof(struct ztdeth_header));

                  /* Copy message body */
                  memcpy(skb_put(skb, msglen), msg, msglen);

                  /* Throw on header */
                  zh = (struct ztdeth_header *)skb_push(skb, sizeof(struct ztdeth_header));
                  zh->subaddr = subaddr;

                  /* Setup protocol and such */
                  skb->protocol = __constant_htons(ETH_P_ZTDETH);
                  skb->nh.raw = skb->data;
                  skb->dev = dev;
                  if (dev->hard_header)
                        dev->hard_header(skb, dev, ETH_P_ZTDETH, addr, dev->dev_addr, skb->len);
                  dev_queue_xmit(skb);
            }
      }
      else
            spin_unlock_irqrestore(&zlock, flags);
      return 0;
}

static struct packet_type ztdeth_ptype = {
      type: __constant_htons(ETH_P_ZTDETH),           /* Protocol */
      dev: NULL,                          /* Device (NULL = wildcard) */
      func: ztdeth_rcv,                   /* Receiver */
};

static int digit2int(char d)
{
      switch(d) {
      case 'F':
      case 'E':
      case 'D':
      case 'C':
      case 'B':
      case 'A':
            return d - 'A' + 10;
      case 'f':
      case 'e':
      case 'd':
      case 'c':
      case 'b':
      case 'a':
            return d - 'a' + 10;
      case '9':
      case '8':
      case '7':
      case '6':
      case '5':
      case '4':
      case '3':
      case '2':
      case '1':
      case '0':
            return d - '0';
      }
      return -1;
}

static int hex2int(char *s)
{
      int res;
      int tmp;
      /* Gotta be at least one digit */
      if (strlen(s) < 1)
            return -1;
      /* Can't be more than two */
      if (strlen(s) > 2)
            return -1;
      /* Grab the first digit */
      res = digit2int(s[0]);
      if (res < 0)
            return -1;
      tmp = res;
      /* Grab the next */
      if (strlen(s) > 1) {
            res = digit2int(s[1]);
            if (res < 0)
                  return -1;
            tmp = tmp * 16 + res;
      }
      return tmp;
}

static void ztdeth_destroy(void *pvt)
{
      struct ztdeth *z = pvt;
      unsigned long flags;
      struct ztdeth *prev=NULL, *cur;
      spin_lock_irqsave(&zlock, flags);
      cur = zdevs;
      while(cur) {
            if (cur == z) {
                  if (prev)
                        prev->next = cur->next;
                  else
                        zdevs = cur->next;
                  break;
            }
            prev = cur;
            cur = cur->next;
      }
      spin_unlock_irqrestore(&zlock, flags);
      if (cur == z) {   /* Successfully removed */
            printk("TDMoE: Removed interface for %s\n", z->span->name);
            kfree(z);
#ifndef LINUX26
            MOD_DEC_USE_COUNT;
#endif
      }
}

static void *ztdeth_create(struct zt_span *span, char *addr)
{
      struct ztdeth *z;
      char src[256];
      char tmp[256], *tmp2, *tmp3, *tmp4 = NULL;
      int res,x;
      unsigned long flags;

      z = kmalloc(sizeof(struct ztdeth), GFP_KERNEL);
      if (z) {
            /* Zero it out */
            memset(z, 0, sizeof(struct ztdeth));

            /* Address should be <dev>/<macaddr>[/subaddr] */
            strncpy(tmp, addr, sizeof(tmp) - 1);
            tmp2 = strchr(tmp, '/');
            if (tmp2) {
                  *tmp2 = '\0';
                  tmp2++;
                  strncpy(z->ethdev, tmp, sizeof(z->ethdev) - 1);
            } else {
                  printk("Invalid TDMoE address (no device) '%s'\n", addr);
                  kfree(z);
                  return NULL;
            }
            if (tmp2) {
                  tmp4 = strchr(tmp2+1, '/');
                  if (tmp4) {
                        *tmp4 = '\0';
                        tmp4++;
                  }
                  /* We don't have SSCANF :(  Gotta do this the hard way */
                  tmp3 = strchr(tmp2, ':');
                  for (x=0;x<6;x++) {
                        if (tmp2) {
                              if (tmp3) {
                                    *tmp3 = '\0';
                                    tmp3++;
                              }
                              res = hex2int(tmp2);
                              if (res < 0)
                                    break;
                              z->addr[x] = res & 0xff;
                        } else
                              break;
                        if ((tmp2 = tmp3))
                              tmp3 = strchr(tmp2, ':');
                  }
                  if (x != 6) {
                        printk("TDMoE: Invalid MAC address in: %s\n", addr);
                        kfree(z);
                        return NULL;
                  }
            } else {
                  printk("TDMoE: Missing MAC address\n");
                  kfree(z);
                  return NULL;
            }
            if (tmp4) {
                  int sub = 0;
                  int mul = 1;

                  /* We have a subaddr */
                  tmp3 = tmp4 + strlen (tmp4) - 1;
                  while (tmp3 >= tmp4) {
                        if (*tmp3 >= '0' && *tmp3 <= '9') {
                              sub += (*tmp3 - '0') * mul;
                        } else {
                              printk("TDMoE: Invalid subaddress\n");
                              kfree(z);
                              return NULL;
                        }
                        mul *= 10;
                        tmp3--;
                  }
                  z->subaddr = htons(sub);
            }
            z->dev = dev_get_by_name(z->ethdev);
            if (!z->dev) {
                  printk("TDMoE: Invalid device '%s'\n", z->ethdev);
                  kfree(z);
                  return NULL;
            }
            z->span = span;
            src[0] ='\0';
            for (x=0;x<5;x++)
                  sprintf(src + strlen(src), "%02x:", z->dev->dev_addr[x]);
            sprintf(src + strlen(src), "%02x", z->dev->dev_addr[5]);
            printk("TDMoE: Added new interface for %s at %s (addr=%s, src=%s, subaddr=%d)\n", span->name, z->dev->name, addr, src, ntohs(z->subaddr));
                  
            spin_lock_irqsave(&zlock, flags);
            z->next = zdevs;
            zdevs = z;
            spin_unlock_irqrestore(&zlock, flags);
#ifndef LINUX26
            MOD_INC_USE_COUNT;
#endif
      }
      return z;
}

static struct zt_dynamic_driver ztd_eth = {
      "eth",
      "Ethernet",
      ztdeth_create,
      ztdeth_destroy,
      ztdeth_transmit
};

static struct notifier_block ztdeth_nblock = {
      notifier_call: ztdeth_notifier,
};

static int __init ztdeth_init(void)
{
      dev_add_pack(&ztdeth_ptype);
      register_netdevice_notifier(&ztdeth_nblock);
      zt_dynamic_register(&ztd_eth);
      return 0;
}

static void __exit ztdeth_exit(void)
{
      dev_remove_pack(&ztdeth_ptype);
      unregister_netdevice_notifier(&ztdeth_nblock);
      zt_dynamic_unregister(&ztd_eth);
}

MODULE_DESCRIPTION("Zaptel Dynamic TDMoE Support");
MODULE_AUTHOR("Mark Spencer <markster@linux-support.net>");
#ifdef MODULE_LICENSE
MODULE_LICENSE("GPL");
#endif

module_init(ztdeth_init);
module_exit(ztdeth_exit);

Generated by  Doxygen 1.6.0   Back to index