p[offset + 1] &= value >> 8;                p[offset + 2] &= value >> 16;                p[offset + 3] &= value >> 24;#endif                pflash_update(pfl, offset, 4);                break;            }            pfl->status = 0x00 | ~(value & 0x80);            /* Let's pretend write is immediate */            if (pfl->bypass)                goto do_bypass;            goto reset_flash;        case 0x90:            if (pfl->bypass && cmd == 0x00) {                /* Unlock bypass reset */                goto reset_flash;            }            /* We can enter CFI query mode from autoselect mode */            if (boff == 0x55 && cmd == 0x98)                goto enter_CFI_mode;            /* No break here */        default:            DPRINTF("%s: invalid write for command %02x\n",                    __func__, pfl->cmd);            goto reset_flash;        }    case 4:        switch (pfl->cmd) {        case 0xA0:            /* Ignore writes while flash data write is occuring */            /* As we suppose write is immediate, this should never happen */            return;        case 0x80:            goto check_unlock1;        default:            /* Should never happen */            DPRINTF("%s: invalid command state %02x (wc 4)\n",                    __func__, pfl->cmd);            goto reset_flash;        }        break;    case 5:        switch (cmd) {        case 0x10:            if (boff != 0x555) {                DPRINTF("%s: chip erase: invalid address " TARGET_FMT_lx "\n",                        __func__, offset);                goto reset_flash;            }            /* Chip erase */            DPRINTF("%s: start chip erase\n", __func__);            memset(pfl->storage, 0xFF, pfl->total_len);            pfl->status = 0x00;            pflash_update(pfl, 0, pfl->total_len);            /* Let's wait 5 seconds before chip erase is done */            qemu_mod_timer(pfl->timer,                           qemu_get_clock(vm_clock) + (ticks_per_sec * 5));            break;        case 0x30:            /* Sector erase */            p = pfl->storage;            offset &= ~(pfl->sector_len - 1);            DPRINTF("%s: start sector erase at " TARGET_FMT_lx "\n", __func__,                    offset);            memset(p + offset, 0xFF, pfl->sector_len);            pflash_update(pfl, offset, pfl->sector_len);            pfl->status = 0x00;            /* Let's wait 1/2 second before sector erase is done */            qemu_mod_timer(pfl->timer,                           qemu_get_clock(vm_clock) + (ticks_per_sec / 2));            break;        default:            DPRINTF("%s: invalid command %02x (wc 5)\n", __func__, cmd);            goto reset_flash;        }        pfl->cmd = cmd;        break;    case 6:        switch (pfl->cmd) {        case 0x10:            /* Ignore writes during chip erase */            return;        case 0x30:            /* Ignore writes during sector erase */            return;        default:            /* Should never happen */            DPRINTF("%s: invalid command state %02x (wc 6)\n",                    __func__, pfl->cmd);            goto reset_flash;        }        break;    case 7: /* Special value for CFI queries */        DPRINTF("%s: invalid write in CFI query mode\n", __func__);        goto reset_flash;    default:        /* Should never happen */        DPRINTF("%s: invalid write state (wc 7)\n",  __func__);        goto reset_flash;    }    pfl->wcycle++;    return;    /* Reset flash */ reset_flash:    cpu_register_physical_memory(pfl->base, pfl->total_len,                                 pfl->off | IO_MEM_ROMD | pfl->fl_mem);    pfl->bypass = 0;    pfl->wcycle = 0;    pfl->cmd = 0;    return; do_bypass:    pfl->wcycle = 2;    pfl->cmd = 0;    return;}static uint32_t pflash_readb (void *opaque, target_phys_addr_t addr){    return pflash_read(opaque, addr, 1);}static uint32_t pflash_readw (void *opaque, target_phys_addr_t addr){    pflash_t *pfl = opaque;    return pflash_read(pfl, addr, 2);}static uint32_t pflash_readl (void *opaque, target_phys_addr_t addr){    pflash_t *pfl = opaque;    return pflash_read(pfl, addr, 4);}static void pflash_writeb (void *opaque, target_phys_addr_t addr,                           uint32_t value){    pflash_write(opaque, addr, value, 1);}static void pflash_writew (void *opaque, target_phys_addr_t addr,                           uint32_t value){    pflash_t *pfl = opaque;    pflash_write(pfl, addr, value, 2);}static void pflash_writel (void *opaque, target_phys_addr_t addr,                           uint32_t value){    pflash_t *pfl = opaque;    pflash_write(pfl, addr, value, 4);}static CPUWriteMemoryFunc *pflash_write_ops[] = {    &pflash_writeb,    &pflash_writew,    &pflash_writel,};static CPUReadMemoryFunc *pflash_read_ops[] = {    &pflash_readb,    &pflash_readw,    &pflash_readl,};/* Count trailing zeroes of a 32 bits quantity */static int ctz32 (uint32_t n){    int ret;    ret = 0;    if (!(n & 0xFFFF)) {        ret += 16;        n = n >> 16;    }    if (!(n & 0xFF)) {        ret += 8;        n = n >> 8;    }    if (!(n & 0xF)) {        ret += 4;        n = n >> 4;    }    if (!(n & 0x3)) {        ret += 2;        n = n >> 2;    }    if (!(n & 0x1)) {        ret++;        n = n >> 1;    }#if 0 /* This is not necessary as n is never 0 */    if (!n)        ret++;#endif    return ret;}pflash_t *pflash_cfi02_register(target_phys_addr_t base, ram_addr_t off,                                BlockDriverState *bs, uint32_t sector_len,                                int nb_blocs, int width,                                uint16_t id0, uint16_t id1,                                uint16_t id2, uint16_t id3){    pflash_t *pfl;    int32_t total_len;    total_len = sector_len * nb_blocs;    /* XXX: to be fixed */#if 0    if (total_len != (8 * 1024 * 1024) && total_len != (16 * 1024 * 1024) &&        total_len != (32 * 1024 * 1024) && total_len != (64 * 1024 * 1024))        return NULL;#endif    pfl = qemu_mallocz(sizeof(pflash_t));    if (pfl == NULL)        return NULL;    pfl->storage = phys_ram_base + off;    pfl->fl_mem = cpu_register_io_memory(0, pflash_read_ops, pflash_write_ops,                                         pfl);    pfl->off = off;    cpu_register_physical_memory(base, total_len,                                 off | pfl->fl_mem | IO_MEM_ROMD);    pfl->bs = bs;    if (pfl->bs) {        /* read the initial flash content */        bdrv_read(pfl->bs, 0, pfl->storage, total_len >> 9);    }#if 0 /* XXX: there should be a bit to set up read-only,       *      the same way the hardware does (with WP pin).       */    pfl->ro = 1;#else    pfl->ro = 0;#endif    pfl->timer = qemu_new_timer(vm_clock, pflash_timer, pfl);    pfl->base = base;    pfl->sector_len = sector_len;    pfl->total_len = total_len;    pfl->width = width;    pfl->wcycle = 0;    pfl->cmd = 0;    pfl->status = 0;    pfl->ident[0] = id0;    pfl->ident[1] = id1;    pfl->ident[2] = id2;    pfl->ident[3] = id3;    /* Hardcoded CFI table (mostly from SG29 Spansion flash) */    pfl->cfi_len = 0x52;    /* Standard "QRY" string */    pfl->cfi_table[0x10] = 'Q';    pfl->cfi_table[0x11] = 'R';    pfl->cfi_table[0x12] = 'Y';    /* Command set (AMD/Fujitsu) */    pfl->cfi_table[0x13] = 0x02;    pfl->cfi_table[0x14] = 0x00;    /* Primary extended table address (none) */    pfl->cfi_table[0x15] = 0x00;    pfl->cfi_table[0x16] = 0x00;    /* Alternate command set (none) */    pfl->cfi_table[0x17] = 0x00;    pfl->cfi_table[0x18] = 0x00;    /* Alternate extended table (none) */    pfl->cfi_table[0x19] = 0x00;    pfl->cfi_table[0x1A] = 0x00;    /* Vcc min */    pfl->cfi_table[0x1B] = 0x27;    /* Vcc max */    pfl->cfi_table[0x1C] = 0x36;    /* Vpp min (no Vpp pin) */    pfl->cfi_table[0x1D] = 0x00;    /* Vpp max (no Vpp pin) */    pfl->cfi_table[0x1E] = 0x00;    /* Reserved */    pfl->cfi_table[0x1F] = 0x07;    /* Timeout for min size buffer write (16 祍) */    pfl->cfi_table[0x20] = 0x04;    /* Typical timeout for block erase (512 ms) */    pfl->cfi_table[0x21] = 0x09;    /* Typical timeout for full chip erase (4096 ms) */    pfl->cfi_table[0x22] = 0x0C;    /* Reserved */    pfl->cfi_table[0x23] = 0x01;    /* Max timeout for buffer write */    pfl->cfi_table[0x24] = 0x04;    /* Max timeout for block erase */    pfl->cfi_table[0x25] = 0x0A;    /* Max timeout for chip erase */    pfl->cfi_table[0x26] = 0x0D;    /* Device size */    pfl->cfi_table[0x27] = ctz32(total_len) + 1;    /* Flash device interface (8 & 16 bits) */    pfl->cfi_table[0x28] = 0x02;    pfl->cfi_table[0x29] = 0x00;    /* Max number of bytes in multi-bytes write */    /* XXX: disable buffered write as it's not supported */    //    pfl->cfi_table[0x2A] = 0x05;    pfl->cfi_table[0x2A] = 0x00;    pfl->cfi_table[0x2B] = 0x00;    /* Number of erase block regions (uniform) */    pfl->cfi_table[0x2C] = 0x01;    /* Erase block region 1 */    pfl->cfi_table[0x2D] = nb_blocs - 1;    pfl->cfi_table[0x2E] = (nb_blocs - 1) >> 8;    pfl->cfi_table[0x2F] = sector_len >> 8;    pfl->cfi_table[0x30] = sector_len >> 16;    return pfl;}