Bug 6631 - Implement PLL-based technique for RTC-based ztdummy to minimize jitter (pcadach and softins)

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

1 files changed, 141 insertions, 115 deletions

diff --git a/ztdummy.c b/ztdummy.c
index df2f19b..b2dd353 100644
--- a/ztdummy.c
+++ b/ztdummy.c
@@ -78,30 +78,23 @@
 #include "ztdummy.h"
 
 
-#ifndef LINUX_VERSION_CODE
-#  include <linux/version.h>
-#endif
-
-#ifndef VERSION_CODE
-#  define VERSION_CODE(vers,rel,seq) ( ((vers)<<16) | ((rel)<<8) | (seq) )
-#endif
-
-
-#if LINUX_VERSION_CODE < VERSION_CODE(2,4,5)
-#  error "This kernel is too old: not supported by this file"
-#endif
-
 static struct ztdummy *ztd;
 
 static int debug = 0;
 
+#if defined(LINUX26) && defined(USE_RTC)
+static int rtc_rate = 0;
+static int current_rate = 0;
+static int taskletpending = 0;
+static struct tasklet_struct ztd_tlet;
+static void ztd_tasklet(unsigned long data);
+#endif
+
 #ifdef LINUX26
+#define ZAPTEL_RATE 1000
 #ifndef USE_RTC
 /* New 2.6 kernel timer stuff */
 static struct timer_list timer;
-#if HZ != 1000
-#warning This module will not be usable since the kernel HZ setting is not 1000 ticks per second.
-#endif
 #endif
 #else
 #if LINUX_VERSION_CODE < VERSION_CODE(2,4,5)
@@ -128,50 +121,77 @@ extern uhci_t **uhci_devices;
 
 #ifdef LINUX26
 #ifdef USE_RTC
+static void update_rtc_rate(struct ztdummy *ztd)
+{
+	if (((rtc_rate & (rtc_rate - 1)) != 0) || (rtc_rate > 8192) || (rtc_rate < 2)) {
+		printk("Invalid RTC rate %d specified\n", rtc_rate);
+		rtc_rate = current_rate;	/* Set default RTC rate */
+	}
+	if (!rtc_rate || (rtc_rate != current_rate)) {
+		rtc_control(&ztd->rtc_task, RTC_IRQP_SET, current_rate = (rtc_rate ? rtc_rate : 1024));	/* 1024 Hz */
+		printk("ztdummy: RTC rate is %d\n", rtc_rate);
+		ztd->counter = 0;
+	}
+}
+
+static void ztd_tasklet(unsigned long data)
+{
+	if (taskletpending)
+		update_rtc_rate((struct ztdummy *)ztd);
+	taskletpending = 0;
+}
+
 /* rtc_interrupt - called at 1024Hz from hook in RTC handler */
 static void ztdummy_rtc_interrupt(void *private_data)
 {
 	struct ztdummy *ztd = private_data;
-	unsigned int ticks;
-
-	atomic_inc(&ztd->ticks);
-	ticks = atomic_read(&ztd->ticks);
-	if (ticks == 42 || ticks == 85) {
-		/* skip it */
-	} else if (ticks >= 128) {
-		/* skip and restart count */
-		atomic_set(&ztd->ticks, 0);
-	} else {
-		/* zaptel timing - called in 125 of every 128 interrupts = 1000Hz */
+	unsigned long flags;
+
+	/* Is spinlock required here??? */
+	spin_lock_irqsave(&ztd->rtclock, flags);
+	ztd->counter += ZAPTEL_RATE;
+	while (ztd->counter >= current_rate) {
+		ztd->counter -= current_rate;
+		/* Update of RTC IRQ rate isn't possible from interrupt handler :( */
+		if (!taskletpending && (current_rate != rtc_rate)) {
+			taskletpending = 1;
+			tasklet_hi_schedule(&ztd_tlet);
+		}
 		zt_receive(&ztd->span);
 		zt_transmit(&ztd->span);
 	}
+	spin_unlock_irqrestore(&ztd->rtclock, flags);
 }
 #else
 /* use kernel system tick timer if PC architecture RTC is not available */
 static void ztdummy_timer(unsigned long param)
 {
-    zt_receive(&ztd->span);
-    zt_transmit(&ztd->span);
-    timer.expires = jiffies + 1;
-    add_timer(&timer);
+	timer.expires = jiffies + 1;
+	add_timer(&timer);
+
+	ztd->counter += ZAPTEL_RATE;
+	while (ztd->counter >= HZ) {
+		ztd->counter -= HZ;
+		zt_receive(&ztd->span);
+		zt_transmit(&ztd->span);
+	}
 }
 #endif
 #else
 static void ztdummy_interrupt(int irq, void *dev_id, struct pt_regs *regs)
 {
-    unsigned short status;
-    unsigned int io_addr = s->io_addr;
-
-    status = inw (io_addr + USBSTS);
-    if (status != 0)  {	/* interrupt from our USB port */
-        zt_receive(&ztd->span);
-        zt_transmit(&ztd->span);
-        if (monitor && (check_int==0)) {      /* for testing if interrupt gets triggered*/
-            check_int = 1;
-            printk("ztdummy: interrupt triggered \n");     
-        }   
-     }
+	unsigned short status;
+	unsigned int io_addr = s->io_addr;
+
+	status = inw (io_addr + USBSTS);
+	if (status != 0)  {	/* interrupt from our USB port */
+		zt_receive(&ztd->span);
+		zt_transmit(&ztd->span);
+		if (monitor && (check_int==0)) {      /* for testing if interrupt gets triggered*/
+			check_int = 1;
+			printk("ztdummy: interrupt triggered \n");     
+		}   
+	}
 	return;
 }
 #endif
@@ -199,92 +219,91 @@ int init_module(void)
 {
 #ifdef LINUX26
 #ifdef USE_RTC
-    int err;
+	int err;
 #endif
 #else
-    int irq;
+	int irq;
 #ifdef DEFINE_SPINLOCK
-    DEFINE_SPINLOCK(mylock);
+	DEFINE_SPINLOCK(mylock);
 #else
-    spinlock_t mylock = SPIN_LOCK_UNLOCKED;
+	spinlock_t mylock = SPIN_LOCK_UNLOCKED;
 #endif
 	
-    if (uhci_devices==NULL){
-        printk ("ztdummy: Uhci_devices pointer error.\n");
-	    return -ENODEV;
-    }
-    s=*uhci_devices;     /* uhci device */
-    if (s==NULL){
-        printk ("ztdummy: No uhci_device found.\n");
-	    return -ENODEV;
-    }
+	if (uhci_devices==NULL) {
+		printk ("ztdummy: Uhci_devices pointer error.\n");
+		return -ENODEV;
+	}
+	s=*uhci_devices;	/* uhci device */
+	if (s==NULL) {
+		printk ("ztdummy: No uhci_device found.\n");
+		return -ENODEV;
+	}
 #endif
 
-#if defined(LINUX26) && !defined(USE_RTC)
-    if (HZ != 1000) {
-	    printk("ztdummy: This module requires the kernel HZ setting to be 1000 ticks per second\n");
-	    return -ENODEV;
-    }
-#endif /* defined(LINUX26) && !defined(USE_RTC) */
-
-    ztd = kmalloc(sizeof(struct ztdummy), GFP_KERNEL);
-    if (ztd == NULL) {
-	    printk("ztdummy: Unable to allocate memory\n");
-	    return -ENOMEM;
-    }
+	ztd = kmalloc(sizeof(struct ztdummy), GFP_KERNEL);
+	if (ztd == NULL) {
+		printk("ztdummy: Unable to allocate memory\n");
+		return -ENOMEM;
+	}
 
-    memset(ztd, 0x0, sizeof(struct ztdummy));
+	memset(ztd, 0x0, sizeof(struct ztdummy));
 
-    if (ztdummy_initialize(ztd)) {
-	printk("ztdummy: Unable to intialize zaptel driver\n");
-	kfree(ztd);
-	return -ENODEV;
-    }
+	if (ztdummy_initialize(ztd)) {
+		printk("ztdummy: Unable to intialize zaptel driver\n");
+		kfree(ztd);
+		return -ENODEV;
+	}
 
 #ifdef LINUX26
+	ztd->counter = 0;
 #ifdef USE_RTC
-    atomic_set(&ztd->ticks, 0);
-    ztd->rtc_task.func = ztdummy_rtc_interrupt;
-    ztd->rtc_task.private_data = ztd;
-    err = rtc_register(&ztd->rtc_task);
-    if (err < 0) {
-	printk("ztdummy: Unable to register zaptel rtc driver\n");
-        zt_unregister(&ztd->span);
-	kfree(ztd);
-	return err;
-    }
-    rtc_control(&ztd->rtc_task, RTC_IRQP_SET, 1024);	/* 1024 Hz */
-    rtc_control(&ztd->rtc_task, RTC_PIE_ON, 0);
+	ztd->rtclock = SPIN_LOCK_UNLOCKED;
+	ztd->rtc_task.func = ztdummy_rtc_interrupt;
+	ztd->rtc_task.private_data = ztd;
+	err = rtc_register(&ztd->rtc_task);
+	if (err < 0) {
+		printk("ztdummy: Unable to register zaptel rtc driver\n");
+		zt_unregister(&ztd->span);
+		kfree(ztd);
+		return err;
+	}
+	/* Set default RTC interrupt rate to 1024Hz */
+	if (!rtc_rate)
+		rtc_rate = 1024;
+	update_rtc_rate(ztd);
+	rtc_control(&ztd->rtc_task, RTC_PIE_ON, 0);
+	tasklet_init(&ztd_tlet, ztd_tasklet, 0);
 #else
-    init_timer(&timer);
-    timer.function = ztdummy_timer;
-    timer.expires = jiffies + 1;
-    add_timer(&timer);
+	init_timer(&timer);
+	timer.function = ztdummy_timer;
+	timer.expires = jiffies + 1;
+	add_timer(&timer);
 #endif
 #else
-    irq=s->irq;
-    spin_lock_irq(&mylock);
-    free_irq(s->irq, s);	/* remove uhci_interrupt temporaly */
-    if (request_irq (irq, ztdummy_interrupt, SA_SHIRQ, "ztdummy", ztd)) {
-    	spin_unlock_irq(&mylock);
+	irq=s->irq;
+	spin_lock_irq(&mylock);
+	free_irq(s->irq, s);	/* remove uhci_interrupt temporaly */
+	if (request_irq (irq, ztdummy_interrupt, SA_SHIRQ, "ztdummy", ztd)) {
+		spin_unlock_irq(&mylock);
 		err("Our request_irq %d failed!",irq);
 		kfree(ztd);
 		return -EIO;
-    }		/* we add our handler first, to assure, that our handler gets called first */
-    if (request_irq (irq, uhci_interrupt, SA_SHIRQ, s->uhci_pci->driver->name, s)) {
-        spin_unlock_irq(&mylock);
+	}		/* we add our handler first, to assure, that our handler gets called first */
+	if (request_irq (irq, uhci_interrupt, SA_SHIRQ, s->uhci_pci->driver->name, s)) {
+		spin_unlock_irq(&mylock);
 		err("Original request_irq %d failed!",irq);
-    }
-    spin_unlock_irq(&mylock);
-    /* add td to usb host controller interrupt queue */
-    alloc_td(s, &td, 0);
-    fill_td(td, TD_CTRL_IOC, 0, 0);
-    insert_td_horizontal(s, s->int_chain[0], td);	/* use int_chain[0] to get 1ms interrupts */
+	}
+	spin_unlock_irq(&mylock);
+
+	/* add td to usb host controller interrupt queue */
+	alloc_td(s, &td, 0);
+	fill_td(td, TD_CTRL_IOC, 0, 0);
+	insert_td_horizontal(s, s->int_chain[0], td);	/* use int_chain[0] to get 1ms interrupts */
 #endif	
 
-    if (debug)
-        printk("ztdummy: init() finished\n");
-    return 0;
+	if (debug)
+		printk("ztdummy: init() finished\n");
+	return 0;
 }
 
 
@@ -292,28 +311,35 @@ void cleanup_module(void)
 {
 #ifdef LINUX26
 #ifdef USE_RTC
-    rtc_control(&ztd->rtc_task, RTC_PIE_OFF, 0);
-    rtc_unregister(&ztd->rtc_task);
+	if (taskletpending) {
+		tasklet_disable(&ztd_tlet);
+		tasklet_kill(&ztd_tlet);
+	}
+	rtc_control(&ztd->rtc_task, RTC_PIE_OFF, 0);
+	rtc_unregister(&ztd->rtc_task);
 #else
-    del_timer(&timer);
+	del_timer(&timer);
 #endif
 #else
-    free_irq(s->irq, ztd);  /* disable interrupts */
+	free_irq(s->irq, ztd);  /* disable interrupts */
 #endif
-    zt_unregister(&ztd->span);
-    kfree(ztd);
+	zt_unregister(&ztd->span);
+	kfree(ztd);
 #ifndef LINUX26
 	unlink_td(s, td, 1);
 	delete_desc(s, td);
 #endif
-    if (debug)
-        printk("ztdummy: cleanup() finished\n");
+	if (debug)
+		printk("ztdummy: cleanup() finished\n");
 }
 
 
 
 #ifdef LINUX26
 module_param(debug, int, 0600);
+#ifdef USE_RTC
+module_param(rtc_rate, int, 0600);
+#endif
 #else
 MODULE_PARM(debug, "i");
 #endif