Version 0.1.6 from FTP

git-svn-id: http://svn.digium.com/svn/zaptel/trunk@59 5390a7c7-147a-4af0-8ec9-7488f05a26cb

1 files changed, 348 insertions, 0 deletions

diff --git a/ztd-eth.c b/ztd-eth.c
new file mode 100755
index 0000000..a9959b5
--- /dev/null
+++ b/ztd-eth.c
@@ -0,0 +1,348 @@
+/*
+ * 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)
+{
+	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;
+	span = ztdeth_getspan(skb->mac.ethernet->h_source, zh->subaddr);
+	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;
+	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;
+	default:
+	}
+	return 0;
+}
+
+static int ztdeth_transmit(void *pvt, unsigned char *msg, int msglen)
+{
+	struct ztdeth *z;
+	struct sk_buff *skb;
+	struct ztdeth_header *zh;
+	long flags;
+
+	spin_lock_irqsave(&zlock, flags);
+	z = pvt;
+	if (z->dev) {
+		skb = dev_alloc_skb(msglen + z->dev->hard_header_len + sizeof(struct ztdeth_header) + 32);
+		if (skb) {
+			/* Reserve header space */
+			skb_reserve(skb, z->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 = z->subaddr;
+
+			/* Setup protocol and such */
+			skb->protocol = __constant_htons(ETH_P_ZTDETH);
+			skb->nh.raw = skb->data;
+			skb->dev = z->dev;
+			z->dev->hard_header(skb, z->dev, ETH_P_ZTDETH, z->addr, z->dev->dev_addr, skb->len);
+			dev_queue_xmit(skb);
+		}
+	}
+	spin_unlock_irqrestore(&zlock, flags);
+	return 0;
+}
+
+static struct packet_type ztdeth_ptype = {
+	__constant_htons(ETH_P_ZTDETH),		/* Protocol */
+	NULL,					/* Device (NULL = wildcard) */
+	ztdeth_rcv,				/* Receiver */
+	NULL,					/* Data */
+	NULL					/* Next */
+};
+
+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;
+	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);
+}
+
+static void *ztdeth_create(struct zt_span *span, char *addr)
+{
+	struct ztdeth *z;
+	char src[256];
+	char tmp[256], *tmp2, *tmp3;
+	int res,x;
+	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 = strtok(tmp, "/");
+		if (tmp2) {
+			strncpy(z->ethdev, tmp2, sizeof(z->ethdev) - 1);
+		} else {
+			printk("Invalid TDMoE address (no device) '%s'\n", addr);
+			kfree(z);
+			return NULL;
+		}
+		tmp2 = strtok(NULL, "/");
+		if (tmp2) {
+			/* We don't have SSCANF :(  Gotta do this the hard way */
+			tmp3 = strtok(tmp2, ":");
+			for (x=0;x<6;x++) {
+				if (tmp3) {
+					res = hex2int(tmp3);
+					if (res < 0)
+						break;
+					z->addr[x] = res & 0xff;
+				} else
+					break;
+				tmp3 = strtok(NULL, ":");
+			}
+			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;
+		}
+		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)\n", span->name, z->dev->name, addr, src);
+			
+		spin_lock_irqsave(&zlock, flags);
+		z->next = zdevs;
+		zdevs = z;
+		spin_unlock_irqrestore(&zlock, flags);
+	}
+	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_init(ztdeth_init);
+module_exit(ztdeth_exit);