X-Git-Url: http://git.maemo.org/git/?p=qemu;a=blobdiff_plain;f=hw%2Fonenand.c;fp=hw%2Fonenand.c;h=4778aaea13d2fa5cae5f1db9a8c04bac4816b06a;hp=bc8107ced3dc6c65837bed39e86085986892151c;hb=c657e7366a67af59b2939b628095e5f2377cb50f;hpb=8b74e2114c9c4b3a2382cda1d82e099b14b0d5e6 diff --git a/hw/onenand.c b/hw/onenand.c index bc8107c..4778aae 100644 --- a/hw/onenand.c +++ b/hw/onenand.c @@ -33,7 +33,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; @@ -256,14 +260,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, @@ -286,35 +315,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; + int result = 0; + + uint8_t *buf, *buf2; + buf = qemu_malloc(512); + if (buf) { + buf2 = qemu_malloc(512); + if (buf2) { + memset(buf, 0xff, 512); + for (; !result && num > 0; num--, sec++) { + if (s->bdrv_cur) { + result = bdrv_write(s->bdrv_cur, sec, buf, 1); + if (!result) { + result = bdrv_read(s->bdrv_cur, + s->secs_cur + (sec >> 5), + buf2, 1) < 0; + if (!result) { + memcpy(buf2 + ((sec & 31) << 4), buf, 1 << 4); + result = bdrv_write(s->bdrv_cur, + s->secs_cur + (sec >> 5), + buf2, 1) < 0; + } + } + } else { + if (sec + 1 > s->secs_cur) { + result = 1; + } else { + memcpy(s->current + (sec << 9), buf, 512); + memcpy(s->current + (s->secs_cur << 9) + (sec << 4), + buf, 1 << 4); + } + } + } + qemu_free(buf2); + } else { + result = 1; + } + qemu_free(buf); + } else { + result = 1; } - - return 0; + + return result; } static void onenand_command(OneNANDState *s, int cmd) @@ -374,6 +455,7 @@ static void onenand_command(OneNANDState *s, int cmd) SETADDR(ONEN_BUF_BLOCK, ONEN_BUF_PAGE) SETBUF_M() + if (onenand_prog_main(s, sec, s->count, buf)) s->status |= ONEN_ERR_CMD | ONEN_ERR_PROG; @@ -534,12 +616,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 */ @@ -622,8 +704,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; @@ -696,28 +778,30 @@ static CPUWriteMemoryFunc *onenand_writefn[] = { onenand_write, }; -void *onenand_init(uint32_t id, int regshift, qemu_irq irq) +void *onenand_init(uint16_t man_id, uint16_t dev_id, uint16_t ver_id, + int regshift, qemu_irq irq, BlockDriverState *bs) { OneNANDState *s = (OneNANDState *) qemu_mallocz(sizeof(*s)); - int bdrv_index = drive_get_index(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 = 0; - 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(0, onenand_readfn, onenand_writefn, s); - if (bdrv_index == -1) + if (!bs) s->image = memset(qemu_malloc(size + (size >> 5)), 0xff, size + (size >> 5)); else - s->bdrv = drives_table[bdrv_index].bdrv; + s->bdrv = bs; s->otp = memset(qemu_malloc((64 + 2) << PAGE_SHIFT), 0xff, (64 + 2) << PAGE_SHIFT); s->ram = qemu_ram_alloc(0xc000 << s->shift); @@ -731,7 +815,12 @@ void *onenand_init(uint32_t id, int regshift, qemu_irq irq) onenand_reset(s, 1); - register_savevm("onenand", id | ((regshift & 0x7f) << 24), 0, + register_savevm("onenand", + ((regshift & 0x7f) << 24) + | ((man_id & 0xff) << 16) + | ((dev_id & 0xff) << 8) + | (ver_id & 0xff), + 0, onenand_save_state, onenand_load_state, s); return s;