1 files changed, 184 insertions, 2 deletions

diff --git a/src/kernel/mq.c b/src/kernel/mq.c
index ee60574..e3407ab 100644
--- a/src/kernel/mq.c
+++ b/src/kernel/mq.c
@@ -5,24 +5,206 @@
  *      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;
-	struct dnbd3_sock *nsock;
 	int ret = -1;
 
-	printk(KERN_DEBUG "dnbd3: handle command %i device %i\n", cmd->cmd_cookie, dev->minor);
+	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;
 }