From 872af797f8f0588df93271ee4af5bd387e44cd48 Mon Sep 17 00:00:00 2001 From: markster Date: Wed, 13 Feb 2002 20:24:17 +0000 Subject: Version 0.1.6 from FTP git-svn-id: http://svn.digium.com/svn/zaptel/trunk@59 5390a7c7-147a-4af0-8ec9-7488f05a26cb --- ztd-eth.c | 348 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 348 insertions(+) create mode 100755 ztd-eth.c (limited to 'ztd-eth.c') 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 + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef CONFIG_DEVFS_FS +#include +#endif +#ifdef STANDALONE_ZAPATA +#include "zaptel.h" +#else +#include +#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 /[/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); -- cgit v1.2.3