path: root/src/kernel/mq.c

/*
 * mq.c
 *
 *  Created on: Jun 26, 2019
 *      Author: fred
 */

#include <net/sock.h>

#include "mq.h"
#include "block.h"

#define DNBD3_CMD_REQUEUED 1

//static void dnbd3_config_put(struct dnbd3_device_t *dev)
//{
//	if (refcount_dec_and_mutex_lock(&dev->config_refs,
//					&dev->config_lock)) {
//
//		dev->tag_set.timeout = 0;
//		dev->disk->queue->limits.discard_granularity = 0;
//		dev->disk->queue->limits.discard_alignment = 0;
//		blk_queue_max_discard_sectors(dev->disk->queue, UINT_MAX);
//		blk_queue_flag_clear(QUEUE_FLAG_DISCARD, dev->disk->queue);
//
//		mutex_unlock(&dev->config_lock);
//		nbd_put(dev);
//		module_put(THIS_MODULE);
//	}
//}

#define dnbd3_priv_to_cmd(req)        ((req)->cmd_flags >> REQ_FLAG_BITS)

static int dnbd3_send_cmd(struct dnbd3_device_t *dev, struct dnbd3_cmd *cmd, int index)
{
	struct request *req = blk_mq_rq_from_pdu(cmd);
	dnbd3_request_t dnbd3_request;
	dnbd3_reply_t dnbd3_reply;
	struct msghdr msg;
	struct kvec iov;
	struct req_iterator iter;
	struct bio_vec bvec_inst;
	struct bio_vec *bvec = &bvec_inst;
	sigset_t blocked, oldset;
	void *kaddr;
	int result;

	init_msghdr(msg);

	dnbd3_request.magic = dnbd3_packet_magic;

	switch (req_op(req)) {
	case REQ_OP_DISCARD:
		printk(KERN_DEBUG "dnbd3: request operation discard on device %d\n", dev->minor);
		break;
	case REQ_OP_FLUSH:
		printk(KERN_DEBUG "dnbd3: request operation flush on device %d\n", dev->minor);
		break;
	case REQ_OP_WRITE:
		printk(KERN_DEBUG "dnbd3: request operation write on device %d\n", dev->minor);
		break;
	case REQ_OP_READ:
		printk(KERN_DEBUG "dnbd3: request operation read on device %d\n", dev->minor);
		dnbd3_request.cmd = CMD_GET_BLOCK;
		dnbd3_request.offset = blk_rq_pos(req) << 9; // *512
		dnbd3_request.size = blk_rq_bytes(req); // bytes left to complete entire request
		break;
	case REQ_OP_DRV_IN:
		printk(KERN_DEBUG "dnbd3: request operation driver in on device %d\n", dev->minor);
		dnbd3_request.cmd = dnbd3_priv_to_cmd(req);
		dnbd3_request.size = 0;
		break;

	default:
		return -EIO;
	}

	dnbd3_request.handle = (uint64_t)(uintptr_t)req; // Double cast to prevent warning on 32bit
	fixup_request(dnbd3_request);
	iov.iov_base = &dnbd3_request;
	iov.iov_len = sizeof(dnbd3_request);
	if (kernel_sendmsg(dev->sock, &msg, &iov, 1, sizeof(dnbd3_request)) != sizeof(dnbd3_request)) {
		printk(KERN_ERR "dnbd3: connection to server lost\n");
		result = -EAGAIN;
		goto error;
	}

	// receive net reply
	iov.iov_base = &dnbd3_reply;
	iov.iov_len = sizeof(dnbd3_reply);
	result = kernel_recvmsg(dev->sock, &msg, &iov, 1, sizeof(dnbd3_reply), msg.msg_flags);
	if (!result) {
		printk(KERN_ERR "dnbd3: connection to server lost\n");
		result = -EIO;
		goto error;

	}
	fixup_reply(dnbd3_reply);

	// check error
	if (dnbd3_reply.magic != dnbd3_packet_magic) {
		printk(KERN_ERR "dnbd3: wrong magic packet\n");
		result = -EIO;
		goto error;
	}
//	if (dnbd3_reply.cmd == 0) {
//		printk(KERN_ERR "dnbd3: command was 0\n");
//		result = -EIO;
//		goto error;
//	}

	if (dnbd3_reply.cmd != CMD_GET_BLOCK) {
		printk(KERN_ERR "dnbd3: command was %d\n", dnbd3_reply.cmd);
		result = -EIO;
		goto error;
	}



	rq_for_each_segment(bvec_inst, req, iter) {
		siginitsetinv(&blocked, sigmask(SIGKILL));
		sigprocmask(SIG_SETMASK, &blocked, &oldset);

		kaddr = kmap(bvec->bv_page) + bvec->bv_offset;
		iov.iov_base = kaddr;
		iov.iov_len = bvec->bv_len;
		if (kernel_recvmsg(dev->sock, &msg, &iov, 1, bvec->bv_len, msg.msg_flags) != bvec->bv_len) {
			kunmap(bvec->bv_page);
			sigprocmask(SIG_SETMASK, &oldset, NULL );
			printk(KERN_ERR "dnbd3: could not receive form net to block layer\n");
			goto error;
		}
		kunmap(bvec->bv_page);

		sigprocmask(SIG_SETMASK, &oldset, NULL );
	}
//
//	__blk_end_request_all(req, 0);
	blk_mq_end_request(req, 0);
	dev->pending = NULL;
error:
	return result;
}

static void dnbd3_requeue_cmd(struct dnbd3_cmd *cmd)
{
	struct request *req = blk_mq_rq_from_pdu(cmd);

	if (!test_and_set_bit(DNBD3_CMD_REQUEUED, &cmd->flags))
		blk_mq_requeue_request(req, true);
}

static int dnbd3_handle_cmd(struct dnbd3_cmd *cmd, int index)
{
	struct request *req = blk_mq_rq_from_pdu(cmd);
	struct dnbd3_device_t *dev = cmd->dnbd3;
	int ret = -1;

	printk(KERN_DEBUG "dnbd3: handle request at position %lu and size %d, device %i\n", blk_rq_pos(req), blk_rq_bytes(req), dev->minor);

	if (!refcount_inc_not_zero(&dev->config_refs)) {
		dev_err_ratelimited(disk_to_dev(dev->disk), "socks array is empty\n");
		blk_mq_start_request(req);
		return -EINVAL;
	}

	if (index >= 1) { // TODO use next server with good rtt for this request
		printk(KERN_INFO "dnbd3: index is %d", index);
		dev_err_ratelimited(disk_to_dev(dev->disk), "attempted send on invalid socket\n");
//		nbd_config_put(nbd);
//		TODO what to do here?
		blk_mq_start_request(req);
		return -EINVAL;
	}
	cmd->status = BLK_STS_OK;

again:


	mutex_lock(&dev->socket_lock);
	if (!dev->sock) {
		mutex_unlock(&dev->socket_lock);
		printk(KERN_DEBUG "dnbd3: not connected, try to reconnect\n");
		if (!dnbd3_net_connect(dev)) {
			printk(KERN_ERR "dnbd3: failed to reconnect\n");
			blk_mq_start_request(req);
			return -EIO;
		}
		goto again;
	}

	blk_mq_start_request(req);
	if (unlikely(dev->pending && dev->pending != req)) {
		dnbd3_requeue_cmd(cmd);
		ret = 0;
		goto out;
	}

	ret = dnbd3_send_cmd(dev, cmd, index);
	if (ret == -EAGAIN) {
		dev_err_ratelimited(disk_to_dev(dev->disk), "request send failed, requeueing\n");
		dnbd3_requeue_cmd(cmd);
		ret = 0;
	}
out:
	mutex_unlock(&dev->socket_lock);
//	nbd_config_put(nbd);
	return ret;
}

static blk_status_t dnbd3_queue_rq(struct blk_mq_hw_ctx *hctx, const struct blk_mq_queue_data *bd)
{
	struct dnbd3_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
	int ret;
	struct dnbd3_device_t *dev = cmd->dnbd3;

	printk(KERN_DEBUG "dnbd3: queue request device %i\n", dev->minor);


	mutex_lock(&cmd->lock);
	clear_bit(DNBD3_CMD_REQUEUED, &cmd->flags);


	ret = dnbd3_handle_cmd(cmd, hctx->queue_num);
	if (ret < 0)
		ret = BLK_STS_IOERR;
	else if (!ret)
		ret = BLK_STS_OK;
	mutex_unlock(&cmd->lock);

	return ret;
}

static void dnbd3_complete_rq(struct request *req)
{
	printk(KERN_DEBUG "dnbd3: dnbd3_complete_rq\n");

}

static int dnbd3_init_request(struct blk_mq_tag_set *set, struct request *rq, unsigned int hctx_idx, unsigned int numa_node)
{
	struct dnbd3_cmd *cmd = blk_mq_rq_to_pdu(rq);
	cmd->dnbd3 = set->driver_data;
	cmd->flags = 0;
	mutex_init(&cmd->lock);
	return 0;
}
static enum blk_eh_timer_return dnbd3_xmit_timeout(struct request *req, bool reserved)
{
	printk(KERN_DEBUG "dnbd3: dnbd3_xmit_timeout\n");
	return BLK_EH_DONE;
}


struct blk_mq_ops dnbd3_mq_ops = {
	.queue_rq = dnbd3_queue_rq,
	.complete = dnbd3_complete_rq,
	.init_request = dnbd3_init_request,
	.timeout = dnbd3_xmit_timeout,
};