Wednesday, December 18, 2013

Write to block device from kernel...


int
disk_open(dev_minor_t *devinst, const char *path)
{
        int     partnum;
        dev_t   devt;
        struct  hd_struct *part;
        unsigned char   *buf = NULL;
        struct  bio *bb;
        int     i, j;
        struct  page *page;
        unsigned char *d = NULL;
        DECLARE_COMPLETION_ONSTACK(waithdl);

        if ((devt = blk_lookup_devt(path, 0)) == 0) {
                printk(KERN_INFO "blk_lookup_devt %d\n", devt);
                return (-1);
        }
        printk(KERN_INFO "Success blk_lookup_devt %d\n", devt);
        if ((devinst->disk = get_gendisk(devt, &partnum)) == NULL) {
                printk(KERN_INFO "Failed to get generic device ..");
                return (-1);
        }
  
        if ((part = disk_get_part(devinst->disk, 0)) == NULL) {
                printk(KERN_INFO "failed disk_get_part");
                return (-1);
        }
  
        if ((devinst->bdev = bdget_disk(devinst->disk, 0)) == NULL) {
                printk(KERN_INFO "failed bdget_disk");
                return (-1);
        }
        return (0);
}


void
disk_close(struct file *filp)
{
     
}


int
disk_read(dev_minor_t *devinst, unsigned long long offset,
                                       unsigned char *data, unsigned int size)
{
        struct  page *page;
        unsigned char *d = NULL;
        DECLARE_COMPLETION_ONSTACK(waithdl);
        unsigned char   *buf = NULL;
        struct  bio *bb;
        int     ret = 0;

        printk(KERN_INFO "Enter %s\n", __FUNCTION__);
        if (data == NULL) {
                page = alloc_page(GFP_KERNEL);
                d = page_address(page);
                memset(d, 0x00, 4096);
        } else {
                d = data;
                memset(d, 0x00, 4096);
        }

        bb = bio_map_kern(devinst->disk->queue, d, 4096, GFP_KERNEL);
        if (IS_ERR(bb)) {
                printk(KERN_ALERT "FAiled to map kernel memory");
                return (-1);
        }
        //bb->bi_sector = offset >> 9;
        bb->bi_sector = 0;
        printk(KERN_INFO "offset %d size %d\n", bb->bi_sector, size);
        bb->bi_bdev = devinst->bdev->bd_contains;
        if (bb->bi_bdev == NULL) {
                bb->bi_bdev = blkdev_get_by_dev(devinst->bdev->bd_dev, FMODE_READ|FMODE_WRITE, NULL);
        }
        if (bb->bi_bdev == NULL) {
                printk(KERN_ALERT "Doesn't have blk device ...\n");
                return (-1);
        }
        bb->bi_end_io = callback_fn;
        bb->bi_private = &waithdl;
        bio_get(bb);

        submit_bio(READ_SYNC, bb); //read operation
        wait_for_completion(&waithdl);

        if (!bio_flagged(bb, BIO_UPTODATE)) {
               printk(KERN_ALERT "Failed ...");
                ret = -1;
        }
        bio_put(bb);
        printk(KERN_INFO "Data read %x%x%x%x", d[0],d[1],d[2],d[3]);
#if 0
        if (data)
                memcpy(data, d, 4096);
#endif
        return (ret);
}

int
disk_write(dev_minor_t *devinst, unsigned long long offset,
                                     unsigned char *data, unsigned int size)
{
        struct  page *page;
        unsigned char *d = NULL;
        DECLARE_COMPLETION_ONSTACK(waithdl);
        unsigned char   *buf = NULL;
        struct  bio *bb;
        struct bio_vec bio_vec;

        int     ret = 0;

        printk(KERN_INFO "Enter %s\n", __FUNCTION__);

        bb = bio_map_kern(devinst->disk->queue, data, 4096, GFP_KERNEL);
        if (IS_ERR(bb)) {
                printk(KERN_ALERT "FAiled to map kernel memory");
                return (-1);
        }
        bb->bi_sector = 1;
        bb->bi_bdev = devinst->bdev->bd_contains;
        if (bb->bi_bdev == NULL) {
                bb->bi_bdev = blkdev_get_by_dev(devinst->bdev->bd_dev, FMODE_READ|FMODE_WRITE, NULL);
        }
        if (bb->bi_bdev == NULL) {
                printk(KERN_ALERT "Doesn't have blk device ...\n");
                return (-1);
        }
        bb->bi_end_io = callback_fn;
        bb->bi_private = &waithdl;

        bio_get(bb);
        submit_bio(WRITE_SYNC, bb);

        wait_for_completion(&waithdl);

        if (!test_bit(BIO_UPTODATE, &bb->bi_flags)) {
                bio_put(bb);
                ret = -1;
        }
        bio_put(bb);
        return (ret);
}

/* backup code */
page_read() {

        bio_init(&bio);
        bio.bi_io_vec = &bio_vec;
        bio_vec.bv_page = p;
        bio_vec.bv_len = size;
        bio_vec.bv_offset = 0;
        bio.bi_vcnt = 1;
        bio.bi_idx = 0;
        bio.bi_size = size;
        bio.bi_bdev = bdev->bd_contains;
        bio.bi_sector = sect;
        init_completion(&complete);
        bio.bi_private = &complete;
        bio.bi_end_io = rq_complete;

        //submit_bio(WRITE_SYNC, &bio);
        submit_bio(READ_SYNC, &bio);
        wait_for_completion(&complete);

        if (test_bit(BIO_UPTODATE, &bio.bi_flags)) {
               printk(KERN_INFO "Read BIO OK 0x%x%x%x%x\n", d[0],d[1],d[2],d[3]);
        } else {
                printk(KERN_INFO "BIO ERROR\n");
        }
}







No comments:

Post a Comment