tridentfb: resource management fixes in probe function

Correct error paths in probe function.

The probe function enables mmio mode so it important to disable the mmio
mode before exiting the probe function.  Otherwise, the console is left in
unusable state (garbled fonts at least, lock up at worst).

[akpm@linux-foundation.org: cleanups]
Signed-off-by: Krzysztof Helt <krzysztof.h1@wp.pl>
Cc: "Antonino A. Daplas" <adaplas@pol.net>
Signed-off-by: Andrew Morton <akpm@linux-foundation.org>
Signed-off-by: Linus Torvalds <torvalds@linux-foundation.org>

1 files changed, 48 insertions, 17 deletions

diff --git a/drivers/video/tridentfb.c b/drivers/video/tridentfb.c
index 70fb4ee2b421..919ce75db9e2 100644
--- a/drivers/video/tridentfb.c
+++ b/drivers/video/tridentfb.c
@@ -564,19 +564,46 @@ static inline void write3CE(int reg, unsigned char val)
         t_outb(val, 0x3CF);
 }
 
-static inline void enable_mmio(void)
+static void enable_mmio(void)
 {
+        unsigned char tmp;
+
         /* Goto New Mode */
         outb(0x0B, 0x3C4);
         inb(0x3C5);
 
         /* Unprotect registers */
         outb(NewMode1, 0x3C4);
+        tmp = inb(0x3C5);
         outb(0x80, 0x3C5);
 
         /* Enable MMIO */
         outb(PCIReg, 0x3D4);
         outb(inb(0x3D5) | 0x01, 0x3D5);
+
+        t_outb(NewMode1, 0x3C4);
+        t_outb(tmp, 0x3C5);
+}
+
+static void disable_mmio(void)
+{
+        unsigned char tmp;
+
+        /* Goto New Mode */
+        t_outb(0x0B, 0x3C4);
+        t_inb(0x3C5);
+
+        /* Unprotect registers */
+        t_outb(NewMode1, 0x3C4);
+        tmp = t_inb(0x3C5);
+        t_outb(0x80, 0x3C5);
+
+        /* Disable MMIO */
+        t_outb(PCIReg, 0x3D4);
+        t_outb(t_inb(0x3D5) & ~0x01, 0x3D5);
+
+        outb(NewMode1, 0x3C4);
+        outb(tmp, 0x3C5);
 }
 
 #define crtc_unlock()   write3X4(CRTVSyncEnd, read3X4(CRTVSyncEnd) & 0x7F)
@@ -1239,9 +1266,9 @@ static int __devinit trident_pci_probe(struct pci_dev * dev,
         default_par.io_virt = ioremap_nocache(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
 
         if (!default_par.io_virt) {
-                release_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
                 debug("ioremap failed\n");
-                return -1;
+                err = -1;
+                goto out_unmap1;
         }
 
         enable_mmio();
@@ -1252,25 +1279,21 @@ static int __devinit trident_pci_probe(struct pci_dev * dev,
 
         if (!request_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len, "tridentfb")) {
                 debug("request_mem_region failed!\n");
+                disable_mmio();
                 err = -1;
-                goto out_unmap;
+                goto out_unmap1;
         }
 
         fb_info.screen_base = ioremap_nocache(tridentfb_fix.smem_start,
                                               tridentfb_fix.smem_len);
 
         if (!fb_info.screen_base) {
-                release_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len);
                 debug("ioremap failed\n");
                 err = -1;
-                goto out_unmap;
+                goto out_unmap2;
         }
 
         output("%s board found\n", pci_name(dev));
-#if 0
-        output("Trident board found : mem = %X, io = %X, mem_v = %X, io_v = %X\n",
-                tridentfb_fix.smem_start, tridentfb_fix.mmio_start, fb_info.screen_base, default_par.io_virt);
-#endif
         displaytype = get_displaytype();
 
         if (flatpanel)
@@ -1288,9 +1311,12 @@ static int __devinit trident_pci_probe(struct pci_dev * dev,
 
         if (!fb_find_mode(&default_var, &fb_info, mode, NULL, 0, NULL, bpp)) {
                 err = -EINVAL;
-                goto out_unmap;
+                goto out_unmap2;
         }
-        fb_alloc_cmap(&fb_info.cmap, 256, 0);
+        err = fb_alloc_cmap(&fb_info.cmap, 256, 0);
+        if (err < 0)
+                goto out_unmap2;
+
         if (defaultaccel && acc)
                 default_var.accel_flags |= FB_ACCELF_TEXT;
         else
@@ -1300,19 +1326,24 @@ static int __devinit trident_pci_probe(struct pci_dev * dev,
         fb_info.device = &dev->dev;
         if (register_framebuffer(&fb_info) < 0) {
                 printk(KERN_ERR "tridentfb: could not register Trident framebuffer\n");
+                fb_dealloc_cmap(&fb_info.cmap);
                 err = -EINVAL;
-                goto out_unmap;
+                goto out_unmap2;
         }
         output("fb%d: %s frame buffer device %dx%d-%dbpp\n",
            fb_info.node, fb_info.fix.id, default_var.xres,
            default_var.yres, default_var.bits_per_pixel);
         return 0;
 
-out_unmap:
-        if (default_par.io_virt)
-                iounmap(default_par.io_virt);
+out_unmap2:
         if (fb_info.screen_base)
                 iounmap(fb_info.screen_base);
+        release_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len);
+        disable_mmio();
+out_unmap1:
+        if (default_par.io_virt)
+                iounmap(default_par.io_virt);
+        release_mem_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
         return err;
 }
 
@@ -1323,7 +1354,7 @@ static void __devexit trident_pci_remove(struct pci_dev *dev)
         iounmap(par->io_virt);
         iounmap(fb_info.screen_base);
         release_mem_region(tridentfb_fix.smem_start, tridentfb_fix.smem_len);
-        release_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
+        release_mem_region(tridentfb_fix.mmio_start, tridentfb_fix.mmio_len);
 }
 
 /* List of boards that we are trying to support */