add brcm47xx mtd corruption fix by b.sander (#1502)

[openwrt/staging/wigyori.git] / target / linux / brcm47xx-2.6 / files / drivers / mtd / maps / bcm47xx-flash.c

diff --git a/target/linux/brcm47xx-2.6/files/drivers/mtd/maps/bcm47xx-flash.c b/target/linux/brcm47xx-2.6/files/drivers/mtd/maps/bcm47xx-flash.c
index 8d4ebe42d87b321c1ea2ca1f24f0fd050f0b3e08..6a82f362c699cbd6b1000bc14b821d9730ccd5a2 100644 (file)
--- a/target/linux/brcm47xx-2.6/files/drivers/mtd/maps/bcm47xx-flash.c
+++ b/target/linux/brcm47xx-2.6/files/drivers/mtd/maps/bcm47xx-flash.c
@@ -40,6 +40,7 @@
 #include <linux/module.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
+#include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
@@ -79,6 +80,22 @@ extern struct ssb_bus ssb;
 #endif
 static struct mtd_info *bcm947xx_mtd;
 
+static void bcm947xx_map_copy_from(struct map_info *map, void *to, unsigned long from, ssize_t len)
+{
+       if (len==1) {
+               memcpy_fromio(to, map->virt + from, len);
+       } else {
+               int i;
+               u16 *dest = (u16 *) to;
+               u16 *src  = (u16 *) (map->virt + from);
+               for (i = 0; i < (len / 2); i++) {
+                       dest[i] = src[i];
+               }
+               if (len & 1)
+                       *((u8 *)dest+len-1) = src[i] & 0xff;
+       }
+}
+
 static struct map_info bcm947xx_map = {
        name: "Physically mapped flash",
        size: WINDOW_SIZE,
@@ -371,6 +388,7 @@ int __init init_bcm947xx_map(void)
                printk("Failed to ioremap\n");
                return -EIO;
        }
+
        simple_map_init(&bcm947xx_map);
        
        if (!(bcm947xx_mtd = do_map_probe("cfi_probe", &bcm947xx_map))) {
@@ -379,6 +397,9 @@ int __init init_bcm947xx_map(void)
                return -ENXIO;
        }
 
+       /* override copy_from routine */
+       bcm947xx_map.copy_from = bcm947xx_map_copy_from;
+
        bcm947xx_mtd->owner = THIS_MODULE;
 
        size = bcm947xx_mtd->size;