block-qcow2.c.svn-base

我们自己开发的一个OSEK操作系统！不知道可不可以？

    /* find a new entry in the least used one */
    min_index = 0;
    min_count = 0xffffffff;
    for(i = 0; i < L2_CACHE_SIZE; i++) {
        if (s->l2_cache_counts[i] < min_count) {
            min_count = s->l2_cache_counts[i];
            min_index = i;
        }
    }
    return min_index;
}

static int64_t align_offset(int64_t offset, int n)
{
    offset = (offset + n - 1) & ~(n - 1);
    return offset;
}

static int grow_l1_table(BlockDriverState *bs, int min_size)
{
    BDRVQcowState *s = bs->opaque;
    int new_l1_size, new_l1_size2, ret, i;
    uint64_t *new_l1_table;
    uint64_t new_l1_table_offset;
    uint64_t data64;
    uint32_t data32;

    new_l1_size = s->l1_size;
    if (min_size <= new_l1_size)
        return 0;
    while (min_size > new_l1_size) {
        new_l1_size = (new_l1_size * 3 + 1) / 2;
    }
#ifdef DEBUG_ALLOC2
    printf("grow l1_table from %d to %d\n", s->l1_size, new_l1_size);
#endif

    new_l1_size2 = sizeof(uint64_t) * new_l1_size;
    new_l1_table = qemu_mallocz(new_l1_size2);
    if (!new_l1_table)
        return -ENOMEM;
    memcpy(new_l1_table, s->l1_table, s->l1_size * sizeof(uint64_t));

    /* write new table (align to cluster) */
    new_l1_table_offset = alloc_clusters(bs, new_l1_size2);

    for(i = 0; i < s->l1_size; i++)
        new_l1_table[i] = cpu_to_be64(new_l1_table[i]);
    ret = bdrv_pwrite(s->hd, new_l1_table_offset, new_l1_table, new_l1_size2);
    if (ret != new_l1_size2)
        goto fail;
    for(i = 0; i < s->l1_size; i++)
        new_l1_table[i] = be64_to_cpu(new_l1_table[i]);

    /* set new table */
    data64 = cpu_to_be64(new_l1_table_offset);
    if (bdrv_pwrite(s->hd, offsetof(QCowHeader, l1_table_offset),
                    &data64, sizeof(data64)) != sizeof(data64))
        goto fail;
    data32 = cpu_to_be32(new_l1_size);
    if (bdrv_pwrite(s->hd, offsetof(QCowHeader, l1_size),
                    &data32, sizeof(data32)) != sizeof(data32))
        goto fail;
    qemu_free(s->l1_table);
    free_clusters(bs, s->l1_table_offset, s->l1_size * sizeof(uint64_t));
    s->l1_table_offset = new_l1_table_offset;
    s->l1_table = new_l1_table;
    s->l1_size = new_l1_size;
    return 0;
 fail:
    qemu_free(s->l1_table);
    return -EIO;
}

/* 'allocate' is:
 *
 * 0 not to allocate.
 *
 * 1 to allocate a normal cluster (for sector indexes 'n_start' to
 * 'n_end')
 *
 * 2 to allocate a compressed cluster of size
 * 'compressed_size'. 'compressed_size' must be > 0 and <
 * cluster_size
 *
 * return 0 if not allocated.
 */
static uint64_t get_cluster_offset(BlockDriverState *bs,
                                   uint64_t offset, int allocate,
                                   int compressed_size,
                                   int n_start, int n_end)
{
    BDRVQcowState *s = bs->opaque;
    int min_index, i, j, l1_index, l2_index, ret;
    uint64_t l2_offset, *l2_table, cluster_offset, tmp, old_l2_offset;

    l1_index = offset >> (s->l2_bits + s->cluster_bits);
    if (l1_index >= s->l1_size) {
        /* outside l1 table is allowed: we grow the table if needed */
        if (!allocate)
            return 0;
        if (grow_l1_table(bs, l1_index + 1) < 0)
            return 0;
    }
    l2_offset = s->l1_table[l1_index];
    if (!l2_offset) {
        if (!allocate)
            return 0;
    l2_allocate:
        old_l2_offset = l2_offset;
        /* allocate a new l2 entry */
        l2_offset = alloc_clusters(bs, s->l2_size * sizeof(uint64_t));
        /* update the L1 entry */
        s->l1_table[l1_index] = l2_offset | QCOW_OFLAG_COPIED;
        tmp = cpu_to_be64(l2_offset | QCOW_OFLAG_COPIED);
        if (bdrv_pwrite(s->hd, s->l1_table_offset + l1_index * sizeof(tmp),
                        &tmp, sizeof(tmp)) != sizeof(tmp))
            return 0;
        min_index = l2_cache_new_entry(bs);
        l2_table = s->l2_cache + (min_index << s->l2_bits);

        if (old_l2_offset == 0) {
            memset(l2_table, 0, s->l2_size * sizeof(uint64_t));
        } else {
            if (bdrv_pread(s->hd, old_l2_offset,
                           l2_table, s->l2_size * sizeof(uint64_t)) !=
                s->l2_size * sizeof(uint64_t))
                return 0;
        }
        if (bdrv_pwrite(s->hd, l2_offset,
                        l2_table, s->l2_size * sizeof(uint64_t)) !=
            s->l2_size * sizeof(uint64_t))
            return 0;
    } else {
        if (!(l2_offset & QCOW_OFLAG_COPIED)) {
            if (allocate) {
                free_clusters(bs, l2_offset, s->l2_size * sizeof(uint64_t));
                goto l2_allocate;
            }
        } else {
            l2_offset &= ~QCOW_OFLAG_COPIED;
        }
        for(i = 0; i < L2_CACHE_SIZE; i++) {
            if (l2_offset == s->l2_cache_offsets[i]) {
                /* increment the hit count */
                if (++s->l2_cache_counts[i] == 0xffffffff) {
                    for(j = 0; j < L2_CACHE_SIZE; j++) {
                        s->l2_cache_counts[j] >>= 1;
                    }
                }
                l2_table = s->l2_cache + (i << s->l2_bits);
                goto found;
            }
        }
        /* not found: load a new entry in the least used one */
        min_index = l2_cache_new_entry(bs);
        l2_table = s->l2_cache + (min_index << s->l2_bits);
        if (bdrv_pread(s->hd, l2_offset, l2_table, s->l2_size * sizeof(uint64_t)) !=
            s->l2_size * sizeof(uint64_t))
            return 0;
    }
    s->l2_cache_offsets[min_index] = l2_offset;
    s->l2_cache_counts[min_index] = 1;
 found:
    l2_index = (offset >> s->cluster_bits) & (s->l2_size - 1);
    cluster_offset = be64_to_cpu(l2_table[l2_index]);
    if (!cluster_offset) {
        if (!allocate)
            return cluster_offset;
    } else if (!(cluster_offset & QCOW_OFLAG_COPIED)) {
        if (!allocate)
            return cluster_offset;
        /* free the cluster */
        if (cluster_offset & QCOW_OFLAG_COMPRESSED) {
            int nb_csectors;
            nb_csectors = ((cluster_offset >> s->csize_shift) &
                           s->csize_mask) + 1;
            free_clusters(bs, (cluster_offset & s->cluster_offset_mask) & ~511,
                          nb_csectors * 512);
        } else {
            free_clusters(bs, cluster_offset, s->cluster_size);
        }
    } else {
        cluster_offset &= ~QCOW_OFLAG_COPIED;
        return cluster_offset;
    }
    if (allocate == 1) {
        /* allocate a new cluster */
        cluster_offset = alloc_clusters(bs, s->cluster_size);

        /* we must initialize the cluster content which won't be
           written */
        if ((n_end - n_start) < s->cluster_sectors) {
            uint64_t start_sect;

            start_sect = (offset & ~(s->cluster_size - 1)) >> 9;
            ret = copy_sectors(bs, start_sect,
                               cluster_offset, 0, n_start);
            if (ret < 0)
                return 0;
            ret = copy_sectors(bs, start_sect,
                               cluster_offset, n_end, s->cluster_sectors);
            if (ret < 0)
                return 0;
        }
        tmp = cpu_to_be64(cluster_offset | QCOW_OFLAG_COPIED);
    } else {
        int nb_csectors;
        cluster_offset = alloc_bytes(bs, compressed_size);
        nb_csectors = ((cluster_offset + compressed_size - 1) >> 9) -
            (cluster_offset >> 9);
        cluster_offset |= QCOW_OFLAG_COMPRESSED |
            ((uint64_t)nb_csectors << s->csize_shift);
        /* compressed clusters never have the copied flag */
        tmp = cpu_to_be64(cluster_offset);
    }
    /* update L2 table */
    l2_table[l2_index] = tmp;
    if (bdrv_pwrite(s->hd,
                    l2_offset + l2_index * sizeof(tmp), &tmp, sizeof(tmp)) != sizeof(tmp))
        return 0;
    return cluster_offset;
}

static int qcow_is_allocated(BlockDriverState *bs, int64_t sector_num,
                             int nb_sectors, int *pnum)
{
    BDRVQcowState *s = bs->opaque;
    int index_in_cluster, n;
    uint64_t cluster_offset;

    cluster_offset = get_cluster_offset(bs, sector_num << 9, 0, 0, 0, 0);
    index_in_cluster = sector_num & (s->cluster_sectors - 1);
    n = s->cluster_sectors - index_in_cluster;
    if (n > nb_sectors)
        n = nb_sectors;
    *pnum = n;
    return (cluster_offset != 0);
}

static int decompress_buffer(uint8_t *out_buf, int out_buf_size,
                             const uint8_t *buf, int buf_size)
{
    z_stream strm1, *strm = &strm1;
    int ret, out_len;

    memset(strm, 0, sizeof(*strm));

    strm->next_in = (uint8_t *)buf;
    strm->avail_in = buf_size;
    strm->next_out = out_buf;
    strm->avail_out = out_buf_size;

    ret = inflateInit2(strm, -12);
    if (ret != Z_OK)
        return -1;
    ret = inflate(strm, Z_FINISH);
    out_len = strm->next_out - out_buf;
    if ((ret != Z_STREAM_END && ret != Z_BUF_ERROR) ||
        out_len != out_buf_size) {
        inflateEnd(strm);
        return -1;
    }
    inflateEnd(strm);
    return 0;
}

static int decompress_cluster(BDRVQcowState *s, uint64_t cluster_offset)
{
    int ret, csize, nb_csectors, sector_offset;
    uint64_t coffset;

    coffset = cluster_offset & s->cluster_offset_mask;
    if (s->cluster_cache_offset != coffset) {
        nb_csectors = ((cluster_offset >> s->csize_shift) & s->csize_mask) + 1;
        sector_offset = coffset & 511;
        csize = nb_csectors * 512 - sector_offset;
        ret = bdrv_read(s->hd, coffset >> 9, s->cluster_data, nb_csectors);
        if (ret < 0) {
            return -1;
        }
        if (decompress_buffer(s->cluster_cache, s->cluster_size,
                              s->cluster_data + sector_offset, csize) < 0) {
            return -1;
        }
        s->cluster_cache_offset = coffset;
    }
    return 0;
}

/* handle reading after the end of the backing file */
static int backing_read1(BlockDriverState *bs,
                         int64_t sector_num, uint8_t *buf, int nb_sectors)
{
    int n1;
    if ((sector_num + nb_sectors) <= bs->total_sectors)
        return nb_sectors;
    if (sector_num >= bs->total_sectors)
        n1 = 0;
    else
        n1 = bs->total_sectors - sector_num;
    memset(buf + n1 * 512, 0, 512 * (nb_sectors - n1));
    return n1;
}

static int qcow_read(BlockDriverState *bs, int64_t sector_num,
                     uint8_t *buf, int nb_sectors)
{
    BDRVQcowState *s = bs->opaque;
    int ret, index_in_cluster, n, n1;
    uint64_t cluster_offset;

    while (nb_sectors > 0) {
        cluster_offset = get_cluster_offset(bs, sector_num << 9, 0, 0, 0, 0);
        index_in_cluster = sector_num & (s->cluster_sectors - 1);
        n = s->cluster_sectors - index_in_cluster;
        if (n > nb_sectors)
            n = nb_sectors;
        if (!cluster_offset) {
            if (bs->backing_hd) {
                /* read from the base image */
                n1 = backing_read1(bs->backing_hd, sector_num, buf, n);
                if (n1 > 0) {
                    ret = bdrv_read(bs->backing_hd, sector_num, buf, n1);
                    if (ret < 0)
                        return -1;
                }
            } else {
                memset(buf, 0, 512 * n);
            }
        } else if (cluster_offset & QCOW_OFLAG_COMPRESSED) {
            if (decompress_cluster(s, cluster_offset) < 0)
                return -1;
            memcpy(buf, s->cluster_cache + index_in_cluster * 512, 512 * n);
        } else {
            ret = bdrv_pread(s->hd, cluster_offset + index_in_cluster * 512, buf, n * 512);
            if (ret != n * 512)
                return -1;
            if (s->crypt_method) {
                encrypt_sectors(s, sector_num, buf, buf, n, 0,
                                &s->aes_decrypt_key);
            }
        }
        nb_sectors -= n;
        sector_num += n;
        buf += n * 512;
    }
    return 0;
}

static int qcow_write(BlockDriverState *bs, int64_t sector_num,
                     const uint8_t *buf, int nb_sectors)
{
    BDRVQcowState *s = bs->opaque;
    int ret, index_in_cluster, n;
    uint64_t cluster_offset;

    while (nb_sectors > 0) {
        index_in_cluster = sector_num & (s->cluster_sectors - 1);
        n = s->cluster_sectors - index_in_cluster;
        if (n > nb_sectors)
            n = nb_sectors;
        cluster_offset = get_cluster_offset(bs, sector_num << 9, 1, 0,
                                            index_in_cluster,
                                            index_in_cluster + n);
        if (!cluster_offset)
            return -1;
        if (s->crypt_method) {
            encrypt_sectors(s, sector_num, s->cluster_data, buf, n, 1,
                            &s->aes_encrypt_key);
            ret = bdrv_pwrite(s->hd, cluster_offset + index_in_cluster * 512,
                              s->cluster_data, n * 512);
        } else {
            ret = bdrv_pwrite(s->hd, cluster_offset + index_in_cluster * 512, buf, n * 512);
        }
        if (ret != n * 512)
            return -1;
        nb_sectors -= n;
        sector_num += n;
        buf += n * 512;
    }
    s->cluster_cache_offset = -1; /* disable compressed cache */
    return 0;
}

typedef struct QCowAIOCB {
    BlockDriverAIOCB common;
    int64_t sector_num;
    uint8_t *buf;
    int nb_sectors;
    int n;
    uint64_t cluster_offset;
    uint8_t *cluster_data;
    BlockDriverAIOCB *hd_aiocb;
} QCowAIOCB;

static void qcow_aio_read_cb(void *opaque, int ret)
{
    QCowAIOCB *acb = opaque;
    BlockDriverState *bs = acb->common.bs;
    BDRVQcowState *s = bs->opaque;
    int index_in_cluster, n1;

    acb->hd_aiocb = NULL;
    if (ret < 0) {
    fail: