Linux block device driver: ramdisk

/*
 * linux: 4.5.2
 */

#include <linux/module.h>
#include <linux/blkdev.h>
#include <linux/vmalloc.h>

#define BLKDEV_NAME "test_blkdev"
#define GENDISK_NAME "test_gendisk"

#define NSECTORS	1024
#define RAMDISK_MINORS	1

struct ramdisk_dev {
	struct gendisk *gd;
	unsigned char *data;
};

static blk_qc_t ramdisk_make_request(struct request_queue *q, struct bio *bio)
{
	struct ramdisk_dev *dev = q->queuedata;
	struct bio_vec bvec;
    struct bvec_iter iter;
	unsigned long byte_offset = bio->bi_iter.bi_sector << 9;	// !!!!!!

	bio_for_each_segment(bvec, bio, iter) {
		unsigned char * const buffer = __bio_kmap_atomic(bio, iter);
		unsigned long const cur_bv_len = bio_cur_bytes(bio);	// can be divided by 512

		if (byte_offset + cur_bv_len > (NSECTORS << 9)) {
			printk (KERN_NOTICE "Beyond-end r/w (%ld %ld)\n", byte_offset, cur_bv_len);
			goto err_beyond_end_rw;
		}

		if (bio_data_dir(bio) == WRITE)
			memcpy(dev->data + byte_offset, buffer, cur_bv_len);
		else
			memcpy(buffer, dev->data + byte_offset, cur_bv_len);

		byte_offset += cur_bv_len;

err_beyond_end_rw:
		__bio_kunmap_atomic(buffer);
	}

	bio_endio(bio);

	return 0u;
}

static struct block_device_operations ramdisk_ops = {

};

static struct ramdisk_dev *ramdisk;
static int ramdisk_major, setup_device_ret;
static int setup_device(void)
{
	ramdisk_major = register_blkdev(0, BLKDEV_NAME);	// try to allocate any unused major number
	if (ramdisk_major <= 0) {
		printk(KERN_WARNING "ramdisk: unable to get major number\n");
		return -EBUSY;
	}

	ramdisk = (struct ramdisk_dev *)kmalloc(sizeof(struct ramdisk_dev), GFP_KERNEL);
	if (!ramdisk)
		goto out_unregister;

	ramdisk->data = (unsigned char *)vmalloc(NSECTORS << 9);
	if (ramdisk->data == NULL) {
		printk(KERN_NOTICE "vmalloc failure.\n");
		goto out_vmalloc_fail;
	}

	ramdisk->gd = alloc_disk(RAMDISK_MINORS);
	if (!ramdisk->gd) {
		printk (KERN_NOTICE "alloc_disk failure\n");
		goto out_alloc_disk_fail;
	}

	ramdisk->gd->queue = blk_alloc_queue(GFP_KERNEL);
	if (ramdisk->gd->queue == NULL) {
		printk("blk_alloc_queue failed\n");
		goto out_blk_alloc_queue_fail;
	}
	
	blk_queue_make_request(ramdisk->gd->queue, &ramdisk_make_request);	
	blk_queue_logical_block_size(ramdisk->gd->queue, 512);
	ramdisk->gd->queue->queuedata = ramdisk;	// !!!

	ramdisk->gd->major = ramdisk_major;
	ramdisk->gd->first_minor = 0;
	ramdisk->gd->fops = &ramdisk_ops;
	ramdisk->gd->private_data = ramdisk;
	sprintf(ramdisk->gd->disk_name, GENDISK_NAME);
	set_capacity(ramdisk->gd, NSECTORS);

	add_disk(ramdisk->gd);

	return 0;

out_blk_alloc_queue_fail:
out_alloc_disk_fail:
	vfree(ramdisk->data);
out_vmalloc_fail:
	kfree(ramdisk);
out_unregister:
	unregister_blkdev(ramdisk_major, BLKDEV_NAME);
	return -ENOMEM;
}

static void remove_device(void)
{
	if (setup_device_ret == 0) {
		//blk_put_queue(ramdisk->gd->queue);	// reverse blk_alloc_queue. ???something wrong?
		blk_cleanup_queue(ramdisk->gd->queue);
		del_gendisk(ramdisk->gd);
		vfree(ramdisk->data);
		kfree(ramdisk);
		unregister_blkdev(ramdisk_major, BLKDEV_NAME);
	}
}

static int __init ramdisk_init(void)
{
	setup_device_ret = setup_device();

	return 0;
}
module_init(ramdisk_init);

static void __exit ramdisk_exit(void)
{
	remove_device();
}
module_exit(ramdisk_exit);

MODULE_LICENSE("GPL");