Merge branch 'master' of git://git.qemu.org/qemu

1 files changed, 128 insertions, 44 deletions

diff --git a/hw/onenand.c b/hw/onenand.c
index 71c1ab40b4..b0cbebc178 100644
--- a/hw/onenand.c
+++ b/hw/onenand.c
@@ -31,7 +31,11 @@
 #define BLOCK_SHIFT	(PAGE_SHIFT + 6)
 
 typedef struct {
-    uint32_t id;
+    struct {
+        uint16_t man;
+        uint16_t dev;
+        uint16_t ver;
+    } id;
     int shift;
     target_phys_addr_t base;
     qemu_irq intr;
@@ -175,14 +179,39 @@ static inline int onenand_load_main(OneNANDState *s, int sec, int secn,
 static inline int onenand_prog_main(OneNANDState *s, int sec, int secn,
                 void *src)
 {
-    if (s->bdrv_cur)
-        return bdrv_write(s->bdrv_cur, sec, src, secn) < 0;
-    else if (sec + secn > s->secs_cur)
-        return 1;
-
-    memcpy(s->current + (sec << 9), src, secn << 9);
+    int result = 0;
+
+    if (secn > 0) {
+        uint32_t size = (uint32_t) secn * 512;
+        const uint8_t *sp = (const uint8_t *) src;
+        uint8_t *dp = 0;
+        if (s->bdrv_cur) {
+            dp = qemu_malloc(size);
+            if (!dp || bdrv_read(s->bdrv_cur, sec, dp, secn) < 0) {
+                result = 1;
+            }
+        } else {
+            if (sec + secn > s->secs_cur) {
+                result = 1;
+            } else {
+                dp = (uint8_t *) s->current + (sec << 9);
+            }
+        }
+        if (!result) {
+            uint32_t i;
+            for (i = 0; i < size; i++) {
+                dp[i] &= sp[i];
+            }
+            if (s->bdrv_cur) {
+                result = bdrv_write(s->bdrv_cur, sec, dp, secn) < 0;
+            }
+        }
+        if (dp && s->bdrv_cur) {
+            qemu_free(dp);
+        }
+    }
 
-    return 0;
+    return result;
 }
 
 static inline int onenand_load_spare(OneNANDState *s, int sec, int secn,
@@ -205,35 +234,87 @@ static inline int onenand_load_spare(OneNANDState *s, int sec, int secn,
 static inline int onenand_prog_spare(OneNANDState *s, int sec, int secn,
                 void *src)
 {
-    uint8_t buf[512];
-
-    if (s->bdrv_cur) {
-        if (bdrv_read(s->bdrv_cur, s->secs_cur + (sec >> 5), buf, 1) < 0)
-            return 1;
-        memcpy(buf + ((sec & 31) << 4), src, secn << 4);
-        return bdrv_write(s->bdrv_cur, s->secs_cur + (sec >> 5), buf, 1) < 0;
-    } else if (sec + secn > s->secs_cur)
-        return 1;
-
-    memcpy(s->current + (s->secs_cur << 9) + (sec << 4), src, secn << 4);
- 
-    return 0;
+    int result = 0;
+    if (secn > 0) {
+        const uint8_t *sp = (const uint8_t *) src;
+        uint8_t *dp = 0, *dpp = 0;
+        if (s->bdrv_cur) {
+            dp = qemu_malloc(512);
+            if (!dp || bdrv_read(s->bdrv_cur,
+                                s->secs_cur + (sec >> 5),
+                                dp, 1) < 0) {
+                result = 1;
+            } else {
+                dpp = dp + ((sec & 31) << 4);
+            }
+        } else {
+            if (sec + secn > s->secs_cur) {
+                result = 1;
+            } else {
+                dpp = s->current + (s->secs_cur << 9) + (sec << 4);
+            }
+        }
+        if (!result) {
+            uint32_t i;
+            for (i = 0; i < (secn << 4); i++) {
+                dpp[i] &= sp[i];
+            }
+            if (s->bdrv_cur) {
+                result = bdrv_write(s->bdrv_cur, s->secs_cur + (sec >> 5),
+                                dp, 1) < 0;
+            }
+        }
+        if (dp) {
+            qemu_free(dp);
+        }
+    }
+    return result;
 }
 
 static inline int onenand_erase(OneNANDState *s, int sec, int num)
 {
-    /* TODO: optimise */
-    uint8_t buf[512];
-
-    memset(buf, 0xff, sizeof(buf));
-    for (; num > 0; num --, sec ++) {
-        if (onenand_prog_main(s, sec, 1, buf))
-            return 1;
-        if (onenand_prog_spare(s, sec, 1, buf))
-            return 1;
+    uint8_t *blankbuf, *tmpbuf;
+    blankbuf = qemu_malloc(512);
+    if (!blankbuf) {
+        return 1;
+    }
+    tmpbuf = qemu_malloc(512);
+    if (!tmpbuf) {
+        qemu_free(blankbuf);
+        return 1;
+    }
+    memset(blankbuf, 0xff, 512);
+    for (; num > 0; num--, sec++) {
+        if (s->bdrv_cur) {
+            int erasesec = s->secs_cur + (sec >> 5);
+            if (bdrv_write(s->bdrv_cur, sec, blankbuf, 1)) {
+                goto fail;
+            }
+            if (bdrv_read(s->bdrv_cur, erasesec, tmpbuf, 1) < 0) {
+                goto fail;
+            }
+            memcpy(tmpbuf + ((sec & 31) << 4), blankbuf, 1 << 4);
+            if (bdrv_write(s->bdrv_cur, erasesec, tmpbuf, 1) < 0) {
+                goto fail;
+            }
+        } else {
+            if (sec + 1 > s->secs_cur) {
+                goto fail;
+            }
+            memcpy(s->current + (sec << 9), blankbuf, 512);
+            memcpy(s->current + (s->secs_cur << 9) + (sec << 4),
+                   blankbuf, 1 << 4);
+        }
     }
 
+    qemu_free(tmpbuf);
+    qemu_free(blankbuf);
     return 0;
+
+fail:
+    qemu_free(tmpbuf);
+    qemu_free(blankbuf);
+    return 1;
 }
 
 static void onenand_command(OneNANDState *s, int cmd)
@@ -453,12 +534,12 @@ static uint32_t onenand_read(void *opaque, target_phys_addr_t addr)
         return lduw_le_p(s->boot[0] + addr);
 
     case 0xf000:	/* Manufacturer ID */
-        return (s->id >> 16) & 0xff;
+        return s->id.man;
     case 0xf001:	/* Device ID */
-        return (s->id >>  8) & 0xff;
-    /* TODO: get the following values from a real chip!  */
+        return s->id.dev;
     case 0xf002:	/* Version ID */
-        return (s->id >>  0) & 0xff;
+        return s->id.ver;
+    /* TODO: get the following values from a real chip!  */
     case 0xf003:	/* Data Buffer size */
         return 1 << PAGE_SHIFT;
     case 0xf004:	/* Boot Buffer size */
@@ -541,8 +622,8 @@ static void onenand_write(void *opaque, target_phys_addr_t addr,
 
         case 0x0090:	/* Read Identification Data */
             memset(s->boot[0], 0, 3 << s->shift);
-            s->boot[0][0 << s->shift] = (s->id >> 16) & 0xff;
-            s->boot[0][1 << s->shift] = (s->id >>  8) & 0xff;
+            s->boot[0][0 << s->shift] = s->id.man & 0xff;
+            s->boot[0][1 << s->shift] = s->id.dev & 0xff;
             s->boot[0][2 << s->shift] = s->wpstatus & 0xff;
             break;
 
@@ -615,28 +696,31 @@ static CPUWriteMemoryFunc * const onenand_writefn[] = {
     onenand_write,
 };
 
-void *onenand_init(uint32_t id, int regshift, qemu_irq irq)
+void *onenand_init(BlockDriverState *bdrv,
+                uint16_t man_id, uint16_t dev_id, uint16_t ver_id,
+                int regshift, qemu_irq irq)
 {
     OneNANDState *s = (OneNANDState *) qemu_mallocz(sizeof(*s));
-    DriveInfo *dinfo = drive_get(IF_MTD, 0, 0);
-    uint32_t size = 1 << (24 + ((id >> 12) & 7));
+    uint32_t size = 1 << (24 + ((dev_id >> 4) & 7));
     void *ram;
 
     s->shift = regshift;
     s->intr = irq;
     s->rdy = NULL;
-    s->id = id;
+    s->id.man = man_id;
+    s->id.dev = dev_id;
+    s->id.ver = ver_id;
     s->blocks = size >> BLOCK_SHIFT;
     s->secs = size >> 9;
     s->blockwp = qemu_malloc(s->blocks);
-    s->density_mask = (id & (1 << 11)) ? (1 << (6 + ((id >> 12) & 7))) : 0;
+    s->density_mask = (dev_id & 0x08) ? (1 << (6 + ((dev_id >> 4) & 7))) : 0;
     s->iomemtype = cpu_register_io_memory(onenand_readfn,
                     onenand_writefn, s, DEVICE_NATIVE_ENDIAN);
-    if (!dinfo)
+    s->bdrv = bdrv;
+    if (!s->bdrv) {
         s->image = memset(qemu_malloc(size + (size >> 5)),
                         0xff, size + (size >> 5));
-    else
-        s->bdrv = dinfo->bdrv;
+    }
     s->otp = memset(qemu_malloc((64 + 2) << PAGE_SHIFT),
                     0xff, (64 + 2) << PAGE_SHIFT);
     s->ram = qemu_ram_alloc(NULL, "onenand.ram", 0xc000 << s->shift);