Fix subaddressing (bug #2020)

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

1 files changed, 26 insertions, 2 deletions

diff --git a/ztd-eth.c b/ztd-eth.c
index ffcfe50..91745f7 100755
--- a/ztd-eth.c
+++ b/ztd-eth.c
@@ -251,7 +251,7 @@ static void *ztdeth_create(struct zt_span *span, char *addr)
 {
 	struct ztdeth *z;
 	char src[256];
-	char tmp[256], *tmp2, *tmp3;
+	char tmp[256], *tmp2, *tmp3, *tmp4 = NULL;
 	int res,x;
 	unsigned long flags;
 
@@ -273,6 +273,11 @@ static void *ztdeth_create(struct zt_span *span, char *addr)
 			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++) {
@@ -300,6 +305,25 @@ static void *ztdeth_create(struct zt_span *span, char *addr)
 			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);
@@ -311,7 +335,7 @@ static void *ztdeth_create(struct zt_span *span, char *addr)
 		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);
+		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;