add blink support to diag module
[openwrt/svn-archive/openwrt.git] / openwrt / target / linux / package / diag / src / diag.c
index abd682ac45c06d3f802a3161a607bc7c1b9de925..891c65830d0375f3e2e9e9d9719b319d5844757a 100644 (file)
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  *
- * $Id$
+ * $Id:$
  */
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/kmod.h>
 #include <linux/proc_fs.h>
 #include <linux/moduleparam.h>
+#include <linux/tqueue.h>
+#include <linux/timer.h>
 #include <asm/uaccess.h>
 
 #include <osl.h>
 
 #define MODULE_NAME "diag"
 
-#undef AUTO
 #define MAX_GPIO 8
-
-//#define DEBUG
-//#define BUTTON_READ
+#define FLASH_TIME HZ/8
 
 static unsigned int gpiomask = 0;
 module_param(gpiomask, int, 0644);
 
 enum polarity_t {
-       AUTO = 0,
+       REVERSE = 0,
        NORMAL = 1,
-       REVERSE = 2,
 };
 
 enum {
@@ -62,7 +60,6 @@ struct prochandler_t {
        void *ptr;
 };
 
-
 struct button_t {
        struct prochandler_t proc;
        char *name;
@@ -77,6 +74,7 @@ struct led_t {
        char *name;
        u16 gpio;
        u8 polarity;
+       u8 flash;
 };
 
 struct platform_t {
@@ -125,197 +123,201 @@ enum {
        WR850GV2,
 };
 
-static struct platform_t platforms[] = {
+static struct platform_t __init platforms[] = {
        /* Linksys */
        [WAP54GV1] = {
                .name           = "Linksys WAP54G V1",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 0 },
                },
                .leds           = { 
-                       { .name = "diag", .gpio = 1 << 3 },
-                       { .name = "wlan", .gpio = 1 << 4 },
+                       { .name = "diag",       .gpio = 1 << 3 },
+                       { .name = "wlan",       .gpio = 1 << 4 },
                },
        },
        [WAP54GV3] = {
                .name           = "Linksys WAP54G V3",
                .buttons        = {
                        /* FIXME: verify this */
-                       { .name = "reset", .gpio = 1 << 7 },
-                       { .name = "ses", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 7 },
+                       { .name = "ses",        .gpio = 1 << 0 },
                },
                .leds           = { 
                        /* FIXME: diag? */
-                       { .name = "ses", .gpio = 1 << 1 },
+                       { .name = "ses",        .gpio = 1 << 1 },
                },
        },
        [WRT54GV1] = {
                .name           = "Linksys WRT54G V1.x",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
+                       { .name = "reset",      .gpio = 1 << 6 },
                },
                .leds           = { 
                        /* FIXME */
+                       { .name = "diag",       .gpio = 1 << 1 },
+                       { .name = "dmz",        .gpio = 1 << 7 },
+                       { .name = "wlan",       .gpio = 1 << 0 },
                },
        },
        [WRT54G] = {
-               .name           = "Linksys WRT54G(S)",
+               .name           = "Linksys WRT54G*",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
-                       { .name = "ses", .gpio = 1 << 4 }
+                       { .name = "reset",      .gpio = 1 << 6 },
+                       { .name = "ses",        .gpio = 1 << 4 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 1, .polarity = NORMAL },
-                       { .name = "dmz", .gpio = 1 << 7 },
-                       { .name = "ses_white", .gpio = 1 << 2 },
-                       { .name = "ses_orange", .gpio = 1 << 3 },
+                       { .name = "power",      .gpio = 1 << 1, .polarity = NORMAL },
+                       { .name = "dmz",        .gpio = 1 << 7, .polarity = REVERSE },
+                       { .name = "ses_white",  .gpio = 1 << 2, .polarity = REVERSE },
+                       { .name = "ses_orange", .gpio = 1 << 3, .polarity = REVERSE },
+                       { .name = "wlan",       .gpio = 1 << 0, .polarity = REVERSE },
                },
        },
        [WRTSL54GS] = {
                .name           = "Linksys WRTSL54GS",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
-                       { .name = "ses", .gpio = 1 << 4 }
+                       { .name = "reset",      .gpio = 1 << 6 },
+                       { .name = "ses",        .gpio = 1 << 4 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 1, .polarity = NORMAL },
-                       { .name = "dmz", .gpio = 1 << 7 },
-                       { .name = "ses_white", .gpio = 1 << 5 },
-                       { .name = "ses_orange", .gpio = 1 << 7 },
+                       { .name = "power",      .gpio = 1 << 1, .polarity = NORMAL },
+                       { .name = "dmz",        .gpio = 1 << 7, .polarity = REVERSE },
+                       { .name = "ses_white",  .gpio = 1 << 5, .polarity = REVERSE },
+                       { .name = "ses_orange", .gpio = 1 << 7, .polarity = REVERSE },
                },
        },
        [WRT54G3G] = {
                .name           = "Linksys WRT54G3G",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
-                       { .name = "3g", .gpio = 1 << 4 }
+                       { .name = "reset",      .gpio = 1 << 6 },
+                       { .name = "3g",         .gpio = 1 << 4 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 1, .polarity = NORMAL },
-                       { .name = "dmz", .gpio = 1 << 7 },
-                       { .name = "3g_green", .gpio = 1 << 2, .polarity = NORMAL },
-                       { .name = "3g_blue", .gpio = 1 << 3, .polarity = NORMAL },
-                       { .name = "3g_blink", .gpio = 1 << 5, .polarity = NORMAL },
+                       { .name = "power",      .gpio = 1 << 1, .polarity = NORMAL },
+                       { .name = "dmz",        .gpio = 1 << 7, .polarity = REVERSE },
+                       { .name = "3g_green",   .gpio = 1 << 2, .polarity = NORMAL },
+                       { .name = "3g_blue",    .gpio = 1 << 3, .polarity = NORMAL },
+                       { .name = "3g_blink",   .gpio = 1 << 5, .polarity = NORMAL },
                },
        },
        /* Asus */
        [WLHDD] = {
                .name           = "ASUS WL-HDD",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
+                       { .name = "reset",      .gpio = 1 << 6 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 0 },
+                       { .name = "power",      .gpio = 1 << 0, .polarity = REVERSE },
                },
        },
        [WL300G] = {
                .name           = "ASUS WL-500g",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
+                       { .name = "reset",      .gpio = 1 << 6 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 0 },
+                       { .name = "power",      .gpio = 1 << 0, .polarity = REVERSE },
                },
        },
        [WL500G] = {
                .name           = "ASUS WL-500g",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
+                       { .name = "reset",      .gpio = 1 << 6 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 0 },
+                       { .name = "power",      .gpio = 1 << 0, .polarity = REVERSE },
                },
        },
        [WL500GD] = {
                .name           = "ASUS WL-500g Deluxe",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
+                       { .name = "reset",      .gpio = 1 << 6 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 0 },
+                       { .name = "power",      .gpio = 1 << 0, .polarity = REVERSE },
                },
        },
        [WL500GP] = {
                .name           = "ASUS WL-500g Premium",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 0 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 1, .polarity = NORMAL },
-                       { .name = "ses", .gpio = 1 << 4 },
+                       { .name = "power",      .gpio = 1 << 1, .polarity = NORMAL },
+                       { .name = "ses",        .gpio = 1 << 4, .polarity = REVERSE },
                },
        },
        [ASUS_4702] = {
                .name           = "ASUS (unknown, BCM4702)",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 6 },
+                       { .name = "reset",      .gpio = 1 << 6 },
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 0 },
+                       { .name = "power",      .gpio = 1 << 0, .polarity = REVERSE },
                },
        },
        /* Buffalo */
        [WHR_G54S] = {
                .name           = "Buffalo WHR-G54S",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 4 },
-                       { .name = "ses", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 4 },
+                       { .name = "ses",        .gpio = 1 << 0 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
-                       { .name = "internal", .gpio = 1 << 3 },
-                       { .name = "ses", .gpio = 1 << 6 },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
+                       { .name = "internal",   .gpio = 1 << 3, .polarity = REVERSE },
+                       { .name = "ses",        .gpio = 1 << 6, .polarity = REVERSE },
                },
        },
        [WBR2_G54] = {
                .name           = "Buffalo WBR2-G54",
                /* FIXME: verify */
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 7 },
+                       { .name = "reset",      .gpio = 1 << 7 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
                },
        },
        [WHR_HP_G54] = {
                .name           = "Buffalo WHR-HP-G54",
                /* FIXME: verify */
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 4 },
-                       { .name = "ses", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 4 },
+                       { .name = "ses",        .gpio = 1 << 0 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
-                       { .name = "internal", .gpio = 1 << 3 },
-                       { .name = "ses", .gpio = 1 << 6 },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
+                       { .name = "internal",   .gpio = 1 << 3, .polarity = REVERSE },
+                       { .name = "ses",        .gpio = 1 << 6, .polarity = REVERSE },
                },
        },
        [WLA2_G54L] = {
                .name           = "Buffalo WLA2-G54L",
                /* FIXME: verify */
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 7 },
+                       { .name = "reset",      .gpio = 1 << 7 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
                },
        },
        [BUFFALO_UNKNOWN] = {
                .name           = "Buffalo (unknown)",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 7 },
+                       { .name = "reset",      .gpio = 1 << 7 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
                },
        },
        [BUFFALO_UNKNOWN_4710] = {
                .name           = "Buffalo (unknown, BCM4710)",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 4 },
+                       { .name = "reset",      .gpio = 1 << 4 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
                },
        },
        /* Siemens */
@@ -325,8 +327,8 @@ static struct platform_t platforms[] = {
                        /* No usable buttons */
                },
                .leds           = {
-                       { .name = "dmz", .gpio = 1 << 4 },
-                       { .name = "wlan", .gpio = 1 << 3 },
+                       { .name = "dmz",        .gpio = 1 << 4, .polarity = REVERSE },
+                       { .name = "wlan",       .gpio = 1 << 3, .polarity = REVERSE },
                },
        },
        [SE505V2] = {
@@ -335,9 +337,9 @@ static struct platform_t platforms[] = {
                        /* No usable buttons */
                },
                .leds           = {
-                       { .name = "power", .gpio = 1 << 5 },
-                       { .name = "dmz", .gpio = 1 << 0 },
-                       { .name = "wlan", .gpio = 1 << 3 },
+                       { .name = "power",      .gpio = 1 << 5, .polarity = REVERSE },
+                       { .name = "dmz",        .gpio = 1 << 0, .polarity = REVERSE },
+                       { .name = "wlan",       .gpio = 1 << 3, .polarity = REVERSE },
                },
        },
        /* US Robotics */
@@ -347,41 +349,41 @@ static struct platform_t platforms[] = {
                        /* No usable buttons */
                },
                .leds           = {
-                       { .name = "wlan", .gpio = 1 << 0 },
-                       { .name = "printer", .gpio = 1 << 1 },
+                       { .name = "wlan",       .gpio = 1 << 0, .polarity = REVERSE },
+                       { .name = "printer",    .gpio = 1 << 1, .polarity = REVERSE },
                },
        },
        /* Dell */
        [TM2300] = {
                .name           = "Dell TrueMobile 2300",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 0 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 7 },
+                       { .name = "diag",       .gpio = 1 << 7, .polarity = REVERSE },
                },
        },
        /* Motorola */
        [WR850GV1] = {
                .name           = "Motorola WR850G V1",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 0 },
+                       { .name = "reset",      .gpio = 1 << 0 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 3 },
-                       { .name = "wlan_red", .gpio = 1 << 5, .polarity = NORMAL },
-                       { .name = "wlan_green", .gpio = 1 << 7, .polarity = NORMAL },
+                       { .name = "diag",       .gpio = 1 << 3, .polarity = REVERSE },
+                       { .name = "wlan_red",   .gpio = 1 << 5, .polarity = NORMAL },
+                       { .name = "wlan_green", .gpio = 1 << 7, .polarity = NORMAL },
                },
        },
        [WR850GV2] = {
                .name           = "Motorola WR850G V2",
                .buttons        = {
-                       { .name = "reset", .gpio = 1 << 5 },
+                       { .name = "reset",      .gpio = 1 << 5 },
                },
                .leds           = {
-                       { .name = "diag", .gpio = 1 << 1 },
-                       { .name = "wlan", .gpio = 1 << 0 },
-                       { .name = "modem", .gpio = 1 << 7, .polarity = NORMAL },
+                       { .name = "diag",       .gpio = 1 << 1, .polarity = REVERSE },
+                       { .name = "wlan",       .gpio = 1 << 0, .polarity = REVERSE },
+                       { .name = "modem",      .gpio = 1 << 7, .polarity = NORMAL },
                },
        },
 };
@@ -392,18 +394,18 @@ extern void *bcm947xx_sbh;
 #define sbh bcm947xx_sbh
 
 static int sb_irq(void *sbh);
-static struct platform_t *platform;
+static struct platform_t platform;
 
-#include <linux/tqueue.h>
-static struct tq_struct tq;
 extern char *nvram_get(char *str);
 
-static inline char *getvar(char *str)
+static void led_flash(unsigned long dummy);
+
+static inline char __init *getvar(char *str)
 {
        return nvram_get(str)?:"";
 }
 
-static struct platform_t *platform_detect(void)
+static struct platform_t __init *platform_detect(void)
 {
        char *boardnum, *boardtype, *buf;
 
@@ -511,21 +513,19 @@ static ssize_t diag_proc_read(struct file *file, char *buf, size_t count, loff_t
        if (dent->data != NULL) {
                struct prochandler_t *handler = (struct prochandler_t *) dent->data;
                switch (handler->type) {
-#ifdef BUTTON_READ
-                       case PROC_BUTTON: {
-                               struct button_t * button = (struct button_t *) handler->ptr;
-                               len = sprintf(page, "%d\n", button->pressed);
-                       }
-#endif
                        case PROC_LED: {
                                struct led_t * led = (struct led_t *) handler->ptr;
-                               int in = (sb_gpioin(sbh) & led->gpio ? 1 : 0);
-                               int p = (led->polarity == NORMAL ? 0 : 1);
-                               len = sprintf(page, "%d\n", ((in ^ p) ? 1 : 0));
+                               if (led->flash) {
+                                       len = sprintf(page, "f\n");
+                               } else {
+                                       int in = (sb_gpioin(sbh) & led->gpio ? 1 : 0);
+                                       int p = (led->polarity == NORMAL ? 0 : 1);
+                                       len = sprintf(page, "%d\n", ((in ^ p) ? 1 : 0));
+                               }
                                break;
                        }
                        case PROC_MODEL:
-                               len = sprintf(page, "%s\n", platform->name);
+                               len = sprintf(page, "%s\n", platform.name);
                                break;
                        case PROC_GPIOMASK:
                                len = sprintf(page, "%d\n", gpiomask);
@@ -578,9 +578,16 @@ static ssize_t diag_proc_write(struct file *file, const char *buf, size_t count,
                                
                                if (led->gpio & gpiomask)
                                        break;
-                               sb_gpiocontrol(sbh, led->gpio, 0);
-                               sb_gpioouten(sbh, led->gpio, led->gpio);
-                               sb_gpioout(sbh, led->gpio, ((p ^ (page[0] == '1')) ? led->gpio : 0));
+
+                               if (page[0] == 'f') {
+                                       led->flash = 1;
+                                       led_flash(0);
+                               } else {
+                                       led->flash = 0;
+                                       sb_gpioouten(sbh, led->gpio, led->gpio);
+                                       sb_gpiocontrol(sbh, led->gpio, 0);
+                                       sb_gpioout(sbh, led->gpio, ((p ^ (page[0] == '1')) ? led->gpio : 0));
+                               }
                                break;
                        }
                        case PROC_GPIOMASK:
@@ -594,101 +601,110 @@ static ssize_t diag_proc_write(struct file *file, const char *buf, size_t count,
        return ret;
 }
 
-static void hotplug_button(struct button_t *b)
-{
-       char *argv [3], *envp[6], *buf, *scratch;
-       int i;
-       
-       if (!hotplug_path[0])
-               return;
-
-       if (in_interrupt()) {
-               printk(MODULE_NAME ": HOTPLUG WHILE IN IRQ!\n");
-               return;
-       }
-
-       if (!(buf = kmalloc (256, GFP_KERNEL)))
-               return;
+struct event_t {
+       struct tq_struct tq;
+       char buf[256];
+       char *argv[3];
+       char *envp[6];
+};
 
-       scratch = buf;
-
-       i = 0;
-       argv[i++] = hotplug_path;
-       argv[i++] = "button";
-       argv[i] = 0;
-
-       i = 0;
-       envp[i++] = "HOME=/";
-       envp[i++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
-       envp[i++] = scratch;
-       scratch += sprintf (scratch, "ACTION=%s", b->pressed?"pressed":"released") + 1;
-       envp[i++] = scratch;
-       scratch += sprintf (scratch, "BUTTON=%s", b->name) + 1;
-       envp[i++] = scratch;
-       scratch += sprintf (scratch, "SEEN=%ld", (jiffies - b->seen)/HZ) + 1;
-       envp[i] = 0;
-
-       call_usermodehelper (argv [0], argv, envp);
-       kfree (buf);
+static void hotplug_button(struct event_t *event)
+{
+       call_usermodehelper (event->argv[0], event->argv, event->envp);
+       kfree(event);
 }
 
 static void button_handler(int irq, void *dev_id, struct pt_regs *regs)
 {
-       
        struct button_t *b;
        int in = sb_gpioin(sbh);
+       struct event_t *event;
 
-       for (b = platform->buttons; b->name; b++) { 
+       for (b = platform.buttons; b->name; b++) { 
                if (b->gpio & gpiomask)
                        continue;
+
                if (b->polarity != (in & b->gpio)) {
                        /* ASAP */
                        b->polarity ^= b->gpio;
                        sb_gpiointpolarity(sbh, b->gpio, b->polarity);
 
                        b->pressed ^= 1;
-#ifdef DEBUG
-                       printk(MODULE_NAME ": button: %s pressed: %d seen: %ld\n",b->name, b->pressed, (jiffies - b->seen)/HZ);
-#endif
-                       INIT_TQUEUE(&tq, (void *)(void *)hotplug_button, (void *)b);
-                       schedule_task(&tq);
+
+                       if ((event = (struct event_t *)kmalloc (256, GFP_KERNEL))) {
+                               int i;
+                               char *scratch = event->buf;
+
+                               i = 0;
+                               event->argv[i++] = hotplug_path;
+                               event->argv[i++] = "button";
+                               event->argv[i] = 0;
+
+                               i = 0;
+                               event->envp[i++] = "HOME=/";
+                               event->envp[i++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
+                               event->envp[i++] = scratch;
+                               scratch += sprintf (scratch, "ACTION=%s", b->pressed?"pressed":"released") + 1;
+                               event->envp[i++] = scratch;
+                               scratch += sprintf (scratch, "BUTTON=%s", b->name) + 1;
+                               event->envp[i++] = scratch;
+                               scratch += sprintf (scratch, "SEEN=%ld", (jiffies - b->seen)/HZ) + 1;
+                               event->envp[i] = 0;
+
+                               INIT_TQUEUE(&event->tq, (void *)(void *)hotplug_button, (void *)event);
+                               schedule_task(&event->tq);
+                       }
 
                        b->seen = jiffies;
                }
        }
 }
 
-static void register_buttons(struct button_t *b)
+static struct timer_list led_timer = {
+       function: &led_flash
+};
+
+static void led_flash(unsigned long dummy) {
+       struct led_t *l;
+       unsigned mask = 0;
+
+       for (l = platform.leds; l->name; l++) {
+               if (l->flash) 
+                       mask |= l->gpio; 
+       }
+
+       mask &= ~gpiomask;
+
+       if (mask) {
+               unsigned val;
+
+               val = ~sb_gpioin(sbh);
+               val &= mask;
+
+               sb_gpioouten(sbh, mask, mask);
+               sb_gpiocontrol(sbh, mask, 0);
+               sb_gpioout(sbh, mask, val);
+
+               mod_timer(&led_timer,jiffies+FLASH_TIME);
+       }
+}
+
+static void __init register_buttons(struct button_t *b)
 {
        int irq = sb_irq(sbh) + 2;
        chipcregs_t *cc;
 
-#ifdef BUTTON_READ
-       struct proc_dir_entry *p;
-
-       buttons = proc_mkdir("button", diag);
-       if (!buttons)
-               return;
-#endif
-       
        request_irq(irq, button_handler, SA_SHIRQ | SA_SAMPLE_RANDOM, "gpio", button_handler);
 
        for (; b->name; b++) {
                if (b->gpio & gpiomask)
                        continue;
+
                sb_gpioouten(sbh,b->gpio,0);
                sb_gpiocontrol(sbh,b->gpio,0);
                b->polarity = sb_gpioin(sbh) & b->gpio;
                sb_gpiointpolarity(sbh, b->gpio, b->polarity);
                sb_gpiointmask(sbh, b->gpio,b->gpio);
-#ifdef BUTTON_READ
-               if ((p = create_proc_entry(b->name, S_IRUSR, buttons))) {
-                       b->proc.type = PROC_BUTTON;
-                       b->proc.ptr = (void *) b;
-                       p->data = (void *) &b->proc;
-                       p->proc_fops = &diag_proc_fops;
-               }
-#endif
        }
 
        if ((cc = sb_setcore(sbh, SB_CC, 0))) {
@@ -700,24 +716,17 @@ static void register_buttons(struct button_t *b)
        }
 }
 
-static void unregister_buttons(struct button_t *b)
+static void __exit unregister_buttons(struct button_t *b)
 {
        int irq = sb_irq(sbh) + 2;
 
-       for (; b->name; b++) {
+       for (; b->name; b++)
                sb_gpiointmask(sbh, b->gpio, 0);
-#ifdef BUTTON_READ
-               remove_proc_entry(b->name, buttons);
-#endif
-       }
-#ifdef BUTTON_READ
-       remove_proc_entry("buttons", diag);
-#endif
 
        free_irq(irq, button_handler);
 }
 
-static void register_leds(struct led_t *l)
+static void __init register_leds(struct led_t *l)
 {
        struct proc_dir_entry *p;
 
@@ -726,7 +735,7 @@ static void register_leds(struct led_t *l)
                return;
 
        for(; l->name; l++) {
-               sb_gpiointmask(sbh, l->gpio, 0);
+               l->flash = 0;
                if ((p = create_proc_entry(l->name, S_IRUSR, leds))) {
                        l->proc.type = PROC_LED;
                        l->proc.ptr = l;
@@ -736,20 +745,25 @@ static void register_leds(struct led_t *l)
        }
 }
 
-static void unregister_leds(struct led_t *l)
+static void __exit unregister_leds(struct led_t *l)
 {
-       for(; l->name; l++) {
+       for(; l->name; l++)
                remove_proc_entry(l->name, leds);
-       }
+
        remove_proc_entry("led", diag);
 }
 
 static void __exit diag_exit(void)
 {
-       if (platform->buttons)
-               unregister_buttons(platform->buttons);
-       if (platform->leds)
-               unregister_leds(platform->leds);
+
+       del_timer(&led_timer);
+
+       if (platform.buttons)
+               unregister_buttons(platform.buttons);
+
+       if (platform.leds)
+               unregister_leds(platform.leds);
+
        remove_proc_entry("model", diag);
        remove_proc_entry("gpiomask", diag);
        remove_proc_entry("diag", NULL);
@@ -761,13 +775,17 @@ static struct prochandler_t proc_gpiomask = { .type = PROC_GPIOMASK };
 static int __init diag_init(void)
 {
        static struct proc_dir_entry *p;
+       static struct platform_t *detected;
 
-       platform = platform_detect();
-       if (!platform) {
+       detected = platform_detect();
+       if (!detected) {
                printk(MODULE_NAME ": Router model not detected.\n");
                return -ENODEV;
        }
-       printk(MODULE_NAME ": Detected '%s'\n", platform->name);
+
+       memcpy(&platform, detected, sizeof(struct platform_t));
+
+       printk(MODULE_NAME ": Detected '%s'\n", platform.name);
 
        if (!(diag = proc_mkdir("diag", NULL))) {
                printk(MODULE_NAME ": proc_mkdir on /proc/diag failed\n");
@@ -782,11 +800,11 @@ static int __init diag_init(void)
                p->proc_fops = &diag_proc_fops;
        }
 
-       if (platform->buttons)
-               register_buttons(platform->buttons);
+       if (platform.buttons)
+               register_buttons(platform.buttons);
 
-       if (platform->leds)
-               register_leds(platform->leds);
+       if (platform.leds)
+               register_leds(platform.leds);
 
        return 0;
 }