splitted receiver

3 files changed, 321 insertions, 206 deletions

diff --git a/src/kernel/core.c b/src/kernel/core.c
index 1347bcd..8f02900 100644
--- a/src/kernel/core.c
+++ b/src/kernel/core.c
@@ -168,7 +168,26 @@ static int dnbd3_init_request(struct blk_mq_tag_set *set, struct request *rq, un
 }
 static enum blk_eh_timer_return dnbd3_xmit_timeout(struct request *req, bool reserved)
 {
-	printk(KERN_DEBUG "dnbd3: dnbd3_xmit_timeout\n");
+	struct dnbd3_cmd *cmd = blk_mq_rq_to_pdu(req);
+	struct dnbd3_device *dev = cmd->dnbd3;
+	int i;
+	printk(KERN_WARNING "dnbd3: received timeout\n");
+
+	if (!mutex_trylock(&cmd->lock)) {
+		return BLK_EH_RESET_TIMER;
+	}
+
+	for (i = 0; i < NUMBER_CONNECTIONS; i++) {
+		if (dev->socks[i].sock && dev->socks[i].server) {
+			printk(KERN_INFO "dnbd3: reset request\n");
+			dnbd3_requeue_cmd(cmd);
+			return BLK_EH_DONE;
+		}
+	}
+
+	dev_err_ratelimited(disk_to_dev(dev->disk), "connection timed out\n");
+	cmd->status = BLK_STS_IOERR;
+	blk_mq_complete_request(req);
 	return BLK_EH_DONE;
 }
 
diff --git a/src/kernel/dnbd3.h b/src/kernel/dnbd3.h
index f3f848a..ae00cca 100644
--- a/src/kernel/dnbd3.h
+++ b/src/kernel/dnbd3.h
@@ -52,7 +52,7 @@ struct dnbd3_sock {
 	struct dnbd3_server *server;
 	uint32_t heartbeat_count;
 	uint32_t cookie;
-//	uint8_t panic, discover, panic_count;
+	uint8_t panic;//, discover, panic_count;
 	struct dnbd3_device *device;
 	struct work_struct keepalive;
 	struct timer_list keepalive_timer;
@@ -84,6 +84,7 @@ struct dnbd3_device {
 	uint16_t rid;
 	uint64_t reported_size;
 
+	struct work_struct panic_worker;
 	struct work_struct discovery; // if in irq and need to send request
 	struct timer_list discovery_timer;
 };
diff --git a/src/kernel/net.c b/src/kernel/net.c
index 0f875c5..0b83a43 100644
--- a/src/kernel/net.c
+++ b/src/kernel/net.c
@@ -38,7 +38,7 @@
 
 #define KEEPALIVE_TIMER (jiffies + (HZ * TIMER_INTERVAL_KEEPALIVE_PACKET))
 #define DISCOVERY_TIMER (jiffies + (HZ * TIMER_INTERVAL_PROBE_NORMAL))
-#define REQUEST_TIMEOUT (jiffies + (HZ * SOCKET_TIMEOUT_CLIENT_DATA))
+#define REQUEST_TIMEOUT (HZ * SOCKET_TIMEOUT_CLIENT_DATA)
 
 #define init_msghdr(h) do { \
         h.msg_name = NULL; \
@@ -49,7 +49,7 @@
 	} while (0)
 
 static DECLARE_WAIT_QUEUE_HEAD(send_wq);
-static volatile uint64_t send_wq_signal;
+static volatile uint64_t send_wq_signal; //TODO make atomic atomic_64_t
 
 static int dnbd3_socket_connect(struct dnbd3_device *dev, struct dnbd3_server *server);
 static int __dnbd3_socket_connect(struct dnbd3_server * server, struct dnbd3_sock *sock);
@@ -199,8 +199,8 @@ int dnbd3_send_request_blocking(struct dnbd3_sock *sock, int dnbd3_cmd)
 
 	mutex_unlock(&sock->lock);
 
-	printk(KERN_DEBUG "dnbd3: blocking request going to sleep wait for handle %llu\n", handle);
-	if (wait_event_interruptible_timeout(send_wq, handle == send_wq_signal, 10 /* REQUEST_TIMEOUT*/) <= 0) { // timeout or interrupt
+	printk(KERN_DEBUG "dnbd3: blocking request going to sleep for %d  wait for handle %llu\n", REQUEST_TIMEOUT, handle);
+	if (wait_event_interruptible_timeout(send_wq, handle == send_wq_signal, REQUEST_TIMEOUT) <= 0) { // timeout or interrupt
 		printk(KERN_WARNING "dndbd3: request timed out\n");
 		result = -EIO;
 		goto error;
@@ -215,14 +215,38 @@ error:
 	return result;
 }
 
-static void dnbd3_receive_worker(struct work_struct *work)
+static int dnbd3_receive_cmd(struct dnbd3_sock *sock, dnbd3_reply_t *reply)
+{
+	int result;
+	struct msghdr msg;
+	struct kvec iov;
+	init_msghdr(msg);
+	iov.iov_base = reply;
+	iov.iov_len = sizeof(dnbd3_reply_t);
+	result = kernel_recvmsg(sock->sock, &msg, &iov, 1, iov.iov_len, msg.msg_flags);
+	if (result <= 0) {
+		return result;
+	}
+	fixup_reply(dnbd3_reply);
+
+	// check error
+	if (reply->magic != dnbd3_packet_magic) {
+		printk(KERN_ERR "dnbd3: receive cmd wrong magic packet\n");
+		return -EIO;
+	}
+
+	if (reply->cmd == 0) {
+		printk(KERN_ERR "dnbd3: receive command was 0\n");
+		return -EIO;
+	}
+	return result;
+}
+
+static int dnbd3_receive_cmd_get_block_mq(struct dnbd3_device *dev, struct dnbd3_sock *sock, dnbd3_reply_t *reply)
 {
-	struct dnbd3_sock *sock = container_of(work, struct dnbd3_sock, receive);
-	struct dnbd3_device *dev = sock->device;
-	struct request *req = NULL;
-	dnbd3_reply_t dnbd3_reply;
 	struct dnbd3_cmd *cmd;
 	struct msghdr msg;
+	struct request *req = NULL;
 	struct kvec iov;
 	struct req_iterator iter;
 	struct bio_vec bvec_inst;
@@ -230,17 +254,201 @@ static void dnbd3_receive_worker(struct work_struct *work)
 	sigset_t blocked, oldset;
 	void *kaddr;
 	uint32_t tag, cookie;
-	uint16_t hwq, rid;
-	int result, count, remaining;
-	uint64_t reported_size, handle;
+	uint16_t hwq;
+	int result = 0;
+	uint64_t handle;
+	init_msghdr(msg);
+
+	printk(KERN_DEBUG "dnbd3: handle is %llu\n", reply->handle);
+	memcpy(&handle, &reply->handle, sizeof(handle));
+	cookie = dnbd3_arg1_from_handle(handle);
+	tag = dnbd3_arg0_from_handle(handle);
+
+	hwq = blk_mq_unique_tag_to_hwq(tag);
+	if (hwq < dev->tag_set.nr_hw_queues) {
+		req = blk_mq_tag_to_rq(dev->tag_set.tags[hwq], blk_mq_unique_tag_to_tag(tag));
+	}
+	if (!req || !blk_mq_request_started(req)) {
+		dev_err(disk_to_dev(dev->disk), "unexpected reply (%d) %p\n", tag, req);
+		return -EIO;
+	}
+	cmd = blk_mq_rq_to_pdu(req);
+
+	mutex_lock(&cmd->lock);
+	if (cmd->cookie != cookie) {
+		dev_err(disk_to_dev(dev->disk), "double reply on req %p, cookie %u, handle cookie %u\n",
+			req, cmd->cookie, cookie);
+		mutex_unlock(&cmd->lock);
+		return -EIO;
+	}
+
+
+	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;
+		result = kernel_recvmsg(sock->sock, &msg, &iov, 1, bvec->bv_len, msg.msg_flags);
+		if (result != 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");
+			mutex_unlock(&cmd->lock);
+			return result;
+		}
+		kunmap(bvec->bv_page);
+
+		sigprocmask(SIG_SETMASK, &oldset, NULL );
+	}
+	mutex_unlock(&cmd->lock);
+	blk_mq_end_request(req, 0);
+	return result;
+}
+
+static int dnbd3_receive_cmd_get_servers(struct dnbd3_device *dev, struct dnbd3_sock *sock, dnbd3_reply_t *reply)
+{
+	struct msghdr msg;
+	struct kvec iov;
+	/* return true if did not receive servers, not an error*/
+	int result = 1;
+	int count, remaining;
+	init_msghdr(msg);
+
+	printk(KERN_DEBUG "dnbd3: get servers received\n");
+	mutex_lock(&dev->device_lock);
+	if (!dev->use_server_provided_alts) {
+		remaining = reply->size;
+		goto consume_payload;
+	}
+	dev->new_servers_num = 0;
+	count = MIN(NUMBER_SERVERS, reply->size / sizeof(dnbd3_server_entry_t));
+
+	if (count != 0) {
+		iov.iov_base = dev->new_servers;
+		iov.iov_len = count * sizeof(dnbd3_server_entry_t);
+		result = kernel_recvmsg(sock->sock, &msg, &iov, 1, (count * sizeof(dnbd3_server_entry_t)), msg.msg_flags);
+		if (result <= 0) {
+			printk(KERN_ERR "dnbd3: failed to receive get servers %d\n", result);
+			return result;
+		} else if (result != (count * sizeof(dnbd3_server_entry_t))) {
+			printk(KERN_ERR "dnbd3: failed to get servers\n");
+			mutex_unlock(&dev->device_lock);
+			return -EIO;
+		}
+		dev->new_servers_num = count;
+	}
+	// If there were more servers than accepted, remove the remaining data from the socket buffer
+	remaining = reply->size - (count * sizeof(dnbd3_server_entry_t));
+consume_payload:
+	while (remaining > 0) {
+		count = MIN(sizeof(dnbd3_reply_t), remaining); // Abuse the reply struct as the receive buffer
+		iov.iov_base = reply;
+		iov.iov_len = count;
+		result = kernel_recvmsg(sock->sock, &msg, &iov, 1, count, msg.msg_flags);
+		if (result <= 0) {
+			printk(KERN_ERR "dnbd3: failed to receive payload from get servers\n");
+			mutex_unlock(&dev->device_lock);
+			return result;
+		}
+	}
+	mutex_unlock(&dev->device_lock);
+	return result;
+}
+static int dnbd3_receive_cmd_latest_rid(struct dnbd3_device *dev, struct dnbd3_sock *sock, dnbd3_reply_t *reply)
+{
+	struct kvec iov;
+	uint16_t rid;
+	int result;
+	struct msghdr msg;
+	init_msghdr(msg);
+	printk(KERN_DEBUG "dnbd3: latest rid received\n");
+
+	if (reply->size != 2) {
+		printk(KERN_ERR "dnbd3: failed to get latest rid, wrong size\n");
+		return -EIO;
+	}
+	iov.iov_base = &rid;
+	iov.iov_len = sizeof(rid);
+	result = kernel_recvmsg(sock->sock, &msg, &iov, 1, iov.iov_len, msg.msg_flags);
+	if (result <= 0) {
+		printk(KERN_ERR "dnbd3: failed to receive latest rid\n");
+		return result;
+	}
+	rid = net_order_16(rid);
+	printk("Latest rid of %s is %d (currently using %d)\n", dev->imgname, (int)rid, (int)dev->rid);
+	dev->update_available = (rid > dev->rid ? 1 : 0);
+	return result;
+}
+
+static int dnbd3_receive_cmd_select_image(struct dnbd3_device *dev, struct dnbd3_sock *sock, dnbd3_reply_t *reply)
+{
+	struct kvec iov;
+	uint16_t rid;
 	char *name;
+	int result;
+	struct msghdr msg;
 	serialized_buffer_t payload_buffer;
+	uint64_t reported_size;
 	init_msghdr(msg);
+	printk(KERN_DEBUG "dnbd3: select image received\n");
+	// receive reply payload
+	iov.iov_base = &payload_buffer;
+	iov.iov_len = reply->size;
+	result = kernel_recvmsg(sock->sock, &msg, &iov, 1, iov.iov_len, msg.msg_flags);
+	if (result <= 0) {
+		printk(KERN_ERR "dnbd3: failed to receive select image %d\n", result);
+		return result;
+	} else if (result != reply->size) {
+		printk(KERN_ERR "dnbd3: could not read CMD_SELECT_IMAGE payload on handshake, size is %d and should be%d\n",
+				result, reply->size);
+		return -EIO;
+	}
+
+	// handle/check reply payload
+	serializer_reset_read(&payload_buffer, reply->size);
+	sock->server->protocol_version = serializer_get_uint16(&payload_buffer);
+	if (sock->server->protocol_version < MIN_SUPPORTED_SERVER) {
+		printk(KERN_ERR "dnbd3: server version is lower than min supported version\n");
+		return -EIO;
+	}
+
+	//TODO compare RID
+
+	name = serializer_get_string(&payload_buffer);
+	rid = serializer_get_uint16(&payload_buffer);
+	if (dev->rid != rid && strcmp(name, dev->imgname) != 0) {
+		printk(KERN_ERR "dnbd3: server offers image '%s', requested '%s'\n", name, dev->imgname);
+		return -EIO;
+	}
+
+	reported_size = serializer_get_uint64(&payload_buffer);
+	if (!dev->reported_size) {
+		if (reported_size < 4096) {
+			printk(KERN_ERR "dnbd3: reported size by server is < 4096\n");
+			return -EIO;
+		}
+		dev->reported_size = reported_size;
+		set_capacity(dev->disk, dev->reported_size >> 9); /* 512 Byte blocks */
+	} else if (dev->reported_size != reported_size) {
+		printk(KERN_ERR "dnbd3: reported size by server is %llu but should be %llu\n", reported_size, dev->reported_size);
+		return -EIO;
+	}
+	return result;
+
+}
+static void dnbd3_receive_worker(struct work_struct *work)
+{
+	struct dnbd3_sock *sock = container_of(work, struct dnbd3_sock, receive);
+	struct dnbd3_device *dev = sock->device;
+	dnbd3_reply_t dnbd3_reply;
+	uint64_t handle;
+	int result;
 
 	while(sock->sock && sock->server) {
-		iov.iov_base = &dnbd3_reply;
-		iov.iov_len = sizeof(dnbd3_reply);
-		result = kernel_recvmsg(sock->sock, &msg, &iov, 1, sizeof(dnbd3_reply), msg.msg_flags);
+		result = dnbd3_receive_cmd(sock, &dnbd3_reply);
+//				kernel_recvmsg(sock->sock, &msg, &iov, 1, sizeof(dnbd3_reply), msg.msg_flags);
 		if (result == -EAGAIN) {
 			continue;
 		} else if (result <= 0) {
@@ -248,122 +456,28 @@ static void dnbd3_receive_worker(struct work_struct *work)
 			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;
-		}
-
 
 		switch (dnbd3_reply.cmd) {
 		case CMD_GET_BLOCK:
-			printk(KERN_DEBUG "dnbd3: handle is %llu\n", dnbd3_reply.handle);
-			memcpy(&handle, &dnbd3_reply.handle, sizeof(handle));
-			cookie = dnbd3_arg1_from_handle(handle);
-			tag = dnbd3_arg0_from_handle(handle);
-
-			hwq = blk_mq_unique_tag_to_hwq(tag);
-			if (hwq < dev->tag_set.nr_hw_queues)
-				req = blk_mq_tag_to_rq(dev->tag_set.tags[hwq], blk_mq_unique_tag_to_tag(tag));
-			if (!req || !blk_mq_request_started(req)) {
-				dev_err(disk_to_dev(dev->disk), "Unexpected reply (%d) %p\n", tag, req);
-				continue;
-			}
-			cmd = blk_mq_rq_to_pdu(req);
-
-			mutex_lock(&cmd->lock);
-			if (cmd->cookie != cookie) {
-				dev_err(disk_to_dev(dev->disk), "Double reply on req %p, cookie %u, handle cookie %u\n",
-					req, cmd->cookie, cookie);
-				mutex_unlock(&cmd->lock);
-				continue;
-			}
-
-
-			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;
-				result = kernel_recvmsg(sock->sock, &msg, &iov, 1, bvec->bv_len, msg.msg_flags);
-				if (result != 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");
-					mutex_unlock(&cmd->lock);
-					continue;
-				}
-				kunmap(bvec->bv_page);
-
-				sigprocmask(SIG_SETMASK, &oldset, NULL );
+			result = dnbd3_receive_cmd_get_block_mq(dev, sock, &dnbd3_reply);
+			if (result <= 0) {
+				printk(KERN_ERR "dnbd3: receive cmd get block mq failed %d\n", result);
+				goto error;
 			}
-			mutex_unlock(&cmd->lock);
-			blk_mq_end_request(req, 0);
 			break;
 		case CMD_GET_SERVERS:
-			printk(KERN_DEBUG "dnbd3: get servers received\n");
-			mutex_lock(&dev->device_lock);
-			if (!dev->use_server_provided_alts) {
-				remaining = dnbd3_reply.size;
-				goto consume_payload;
-			}
-			dev->new_servers_num = 0;
-			count = MIN(NUMBER_SERVERS, dnbd3_reply.size / sizeof(dnbd3_server_entry_t));
-
-			if (count != 0) {
-				iov.iov_base = dev->new_servers;
-				iov.iov_len = count * sizeof(dnbd3_server_entry_t);
-				result = kernel_recvmsg(sock->sock, &msg, &iov, 1, (count * sizeof(dnbd3_server_entry_t)), msg.msg_flags);
-				if (result != (count * sizeof(dnbd3_server_entry_t))) {
-					printk(KERN_ERR "dnbd3: failed to get servers\n");
-					mutex_unlock(&dev->device_lock);
-					goto error;
-				}
-				dev->new_servers_num = count;
-			}
-			// If there were more servers than accepted, remove the remaining data from the socket buffer
-			remaining = dnbd3_reply.size - (count * sizeof(dnbd3_server_entry_t));
-consume_payload:
-			while (remaining > 0) {
-				count = MIN(sizeof(dnbd3_reply), remaining); // Abuse the reply struct as the receive buffer
-				iov.iov_base = &dnbd3_reply;
-				iov.iov_len = count;
-				result = kernel_recvmsg(sock->sock, &msg, &iov, 1, count, msg.msg_flags);
-				if (result <= 0) {
-					printk(KERN_ERR "dnbd3: failed to receive payload from get servers\n");
-					mutex_unlock(&dev->device_lock);
-					goto error;
-				}
+			result = dnbd3_receive_cmd_get_servers(dev, sock, &dnbd3_reply);
+			if (result <= 0) {
+				printk(KERN_ERR "dnbd3: receive cmd get servers failed %d\n", result);
+				goto error;
 			}
-			mutex_unlock(&dev->device_lock);
 			break;
 		case CMD_LATEST_RID:
-			if (dnbd3_reply.size != 2) {
-				printk(KERN_ERR "dnbd3: failed to get latest rid, wrong size\n");
-				goto error;
-			}
-			printk(KERN_DEBUG "dnbd3: latest rid received\n");
-			iov.iov_base = &rid;
-			iov.iov_len = sizeof(rid);
-			result = kernel_recvmsg(sock->sock, &msg, &iov, 1, iov.iov_len, msg.msg_flags);
+			result = dnbd3_receive_cmd_latest_rid(dev, sock, &dnbd3_reply);
 			if (result <= 0) {
-				printk(KERN_ERR "dnbd3: failed to get latest rid\n");
+				printk(KERN_ERR "dnbd3: receive cmd latest rid failed %d\n", result);
 				goto error;
 			}
-			rid = net_order_16(rid);
-			printk("Latest rid of %s is %d (currently using %d)\n", dev->imgname, (int)rid, (int)dev->rid);
-			dev->update_available = (rid > dev->rid ? 1 : 0);
 			break;
 		case CMD_KEEPALIVE:
 			if (dnbd3_reply.size != 0) {
@@ -373,44 +487,11 @@ consume_payload:
 			printk(KERN_DEBUG "dnbd3: keep alive received\n");
 			break;
 		case CMD_SELECT_IMAGE:
-			printk(KERN_DEBUG "dnbd3: select image received\n");
-			// receive reply payload
-			iov.iov_base = &payload_buffer;
-			iov.iov_len = dnbd3_reply.size;
-			result = kernel_recvmsg(sock->sock, &msg, &iov, 1, dnbd3_reply.size, msg.msg_flags);
-			if (result != dnbd3_reply.size) {
-				printk(KERN_ERR "dnbd3: could not read CMD_SELECT_IMAGE payload on handshake, size is %d and should be%d\n",
-						result, dnbd3_reply.size);
-				goto error;
-			}
-
-			// handle/check reply payload
-			serializer_reset_read(&payload_buffer, dnbd3_reply.size);
-			sock->server->protocol_version = serializer_get_uint16(&payload_buffer);
-			if (sock->server->protocol_version < MIN_SUPPORTED_SERVER) {
-				printk(KERN_ERR "dnbd3: server version is lower than min supported version\n");
-				goto error;
-			}
-
-			name = serializer_get_string(&payload_buffer);
-			rid = serializer_get_uint16(&payload_buffer);
-			if (dev->rid != rid && strcmp(name, dev->imgname) != 0) {
-				printk(KERN_ERR "dnbd3: server offers image '%s', requested '%s'\n", name, dev->imgname);
+			result = dnbd3_receive_cmd_select_image(dev, sock, &dnbd3_reply);
+			if (result <= 0) {
+				printk(KERN_ERR "dnbd3: receive cmd select image failed %d\n", result);
 				goto error;
 			}
-
-			reported_size = serializer_get_uint64(&payload_buffer);
-			if (!dev->reported_size) {
-				if (reported_size < 4096) {
-					printk(KERN_ERR "dnbd3: reported size by server is < 4096\n");
-					goto error;
-				}
-				dev->reported_size = reported_size;
-				set_capacity(dev->disk, dev->reported_size >> 9); /* 512 Byte blocks */
-			} else if (dev->reported_size != reported_size) {
-				printk(KERN_ERR "dnbd3: reported size by server is %llu but should be %llu\n", reported_size, dev->reported_size);
-			}
-
 			break;
 		default:
 			printk(KERN_WARNING "dnbd3: Unknown command (Receive)\n");
@@ -459,6 +540,59 @@ static void dnbd3_discovery_timer(struct timer_list *arg)
 	add_timer(&dev->discovery_timer);
 }
 
+static struct dnbd3_server *dnbd3_find_best_alt_server(struct dnbd3_device *dev) {
+	int i, j;
+	uint64_t rtt = 0;
+	uint64_t best_rtt = RTT_UNREACHABLE;
+	struct dnbd3_server *best_alt_server = NULL;
+	for (i = 0; i < NUMBER_SERVERS; i++) {
+		rtt = (dev->alt_servers[i].rtts[0] + dev->alt_servers[i].rtts[1]
+				+ dev->alt_servers[i].rtts[2] + dev->alt_servers[i].rtts[3]) / 4;
+		if (rtt < best_rtt) {
+			best_alt_server = &dev->alt_servers[i];
+			for (j = 0; j < NUMBER_CONNECTIONS; j++) {
+				if (best_alt_server == dev->socks[j].server) {
+					best_alt_server = NULL; // found already connected server
+					break;
+				}
+			}
+		}
+	}
+	return best_alt_server;
+}
+
+static void dnbd3_panic_worker(struct work_struct *work)
+{
+	struct dnbd3_device *dev = container_of(work, struct dnbd3_device, panic_worker);
+	struct dnbd3_sock *panicked_sock = NULL;
+	struct dnbd3_server *new_server, *panicked_server;
+	int i;
+	int sock_alive = 0;
+	for (i = 0; i < NUMBER_CONNECTIONS; i++) {
+		if (dev->socks[i].panic) {
+			panicked_sock = &dev->socks[i];
+		} else if (dev->socks[i].sock && dev->socks[i].server) {
+			sock_alive++;
+		}
+	}
+	if (panicked_sock) {
+		printk(KERN_WARNING "dnbd3: socket %d panicked, connections still alive %d\n", panicked_sock->sock_nr, sock_alive);
+		panicked_server = panicked_sock->server;
+		dnbd3_socket_disconnect(dev, panicked_server, panicked_sock, true);
+
+		new_server = dnbd3_find_best_alt_server(dev);
+		if (new_server != NULL && new_server != panicked_server) {
+			printk(KERN_INFO "dnbd3: found replacement server");
+			dnbd3_socket_connect(dev, new_server);
+		} else if (sock_alive > 0) {
+			printk(KERN_INFO "dnbd3: found no replacement server but still connected to %d servers\n", sock_alive);
+		} else {
+			printk(KERN_ERR "dnbd3: could not reconnect to server\n");
+		}
+	}
+}
+
+
 static void dnbd3_discovery_worker(struct work_struct *work)
 {
 	struct dnbd3_device *dev = container_of(work, struct dnbd3_device, discovery);
@@ -471,12 +605,10 @@ static void dnbd3_discovery_worker(struct work_struct *work)
 	dnbd3_request_t dnbd3_request;
 	dnbd3_reply_t dnbd3_reply;
 	struct msghdr msg;
-	char *buf, *name;
+	char *buf;
 	struct request *req = NULL;
 	uint64_t rtt;
 	serialized_buffer_t *payload;
-	uint64_t filesize;
-	uint16_t rid;
 	printk(KERN_DEBUG "dnbd3: starting discovery worker\n");
 
 	dnbd3_send_request_blocking(&dev->socks[0], CMD_GET_SERVERS);
@@ -514,8 +646,7 @@ static void dnbd3_discovery_worker(struct work_struct *work)
 						dnbd3_socket_disconnect(dev, existing_server, NULL, true); // TODO what to do when only one connection?
 						existing_server->host.type = 0;
 					}
-					// ADD, so just reset fail counter
-//					existing_server->failures = 0; makes no sense?
+					existing_server->failures = 0; // reset failure count
 					continue;
 				} else if (free_server) {
 					free_server->host = new_server->host;
@@ -557,6 +688,8 @@ static void dnbd3_discovery_worker(struct work_struct *work)
 		if (existing_server->host.type) {
 			sock->sock = NULL;
 			sock->device = dev;
+			sock->server = existing_server;
+
 			init_msghdr(msg);
 			if (__dnbd3_socket_connect(existing_server, sock)) {
 				printk(KERN_ERR "dnbd3: socket connect failed in rtt measurement\n");
@@ -567,55 +700,16 @@ static void dnbd3_discovery_worker(struct work_struct *work)
 				printk(KERN_ERR "dnbd3: request select image failed in rtt measurement\n");
 				goto rtt_error;
 			}
-			iov.iov_base = &dnbd3_reply;
-			iov.iov_len = sizeof(dnbd3_reply);
-			if (kernel_recvmsg(sock->sock, &msg, &iov, 1, sizeof(dnbd3_reply), msg.msg_flags) != sizeof(dnbd3_reply)) {
-				printk(KERN_ERR "dnbd3: receive select image failed in rtt measurement %d\n", j);
-				goto rtt_error;
-			}
-			fixup_reply(dnbd3_reply);
-			if (dnbd3_reply.magic != dnbd3_packet_magic || dnbd3_reply.cmd != CMD_SELECT_IMAGE || dnbd3_reply.size < 4) {
-				printk(KERN_ERR "dnbd3: receive select image wrong header in rtt measurement\n");
-				goto rtt_error;
-			}
-
-			// receive data
-			iov.iov_base = payload;
-			iov.iov_len = dnbd3_reply.size;
-			if (kernel_recvmsg(sock->sock, &msg, &iov, 1, dnbd3_reply.size, msg.msg_flags) != dnbd3_reply.size) {
-				printk(KERN_ERR "dnbd3: receive data select image failed in rtt measurement\n");
-				goto rtt_error;
-			}
-			serializer_reset_read(payload, dnbd3_reply.size);
-
-			existing_server->protocol_version = serializer_get_uint16(payload);
-			if (existing_server->protocol_version < MIN_SUPPORTED_SERVER) {
-				printk(KERN_ERR "dnbd3: server version to old in rtt measurement\n");
+			if (dnbd3_receive_cmd(sock, &dnbd3_reply) <= 0) {
+				printk(KERN_ERR "dnbd3: receive cmd failed in rtt measurement %d\n", j);
 				goto rtt_error;
 			}
 
-			name = serializer_get_string(payload);
-			if (name == NULL ) {
-				printk(KERN_ERR "dnbd3: server did not supply an image name in rtt measurement\n");
+			if (dnbd3_receive_cmd_select_image(dev, sock, &dnbd3_reply) <= 0) {
+				printk(KERN_ERR "dnbd3: receive select image failed in rtt measurement %d\n", j);
 				goto rtt_error;
 			}
 
-			if (strcmp(name, dev->imgname) != 0) {
-				printk(KERN_ERR "dnbd3: image name %s does not match requested %s in rtt measurement\n", name, dev->imgname);
-				goto rtt_error;
-			}
-//			TODO rid is 1 but dev->rid is 0
-			rid = serializer_get_uint16(payload);
-//			if (rid != dev->rid) {
-//				printk(KERN_ERR "dnbd3: rid %d does not match requested %d in rtt measurement\n", rid, dev->rid);
-//				goto rtt_error;
-//			}
-
-			filesize = serializer_get_uint64(payload);
-			if (filesize != dev->reported_size) {
-				printk(KERN_ERR "dnbd3: image size %llu does not match requested %llu in rtt measurement\n", filesize, dev->reported_size);
-				goto rtt_error;
-			}
 			// Request block
 			dnbd3_request.cmd = CMD_GET_BLOCK;
 			// Do *NOT* pick a random block as it has proven to cause severe
@@ -673,7 +767,6 @@ rtt_error:
 			}
 		}
 	}
-
 error:
 	if (buf) {
 		kfree(buf);
@@ -861,6 +954,8 @@ static int dnbd3_socket_disconnect(struct dnbd3_device *dev, struct dnbd3_server
 		sock_release(sock->sock);
 		sock->sock = NULL;
 	}
+	sock->panic = 0;
+	sock->heartbeat_count = 0;
 	return 0;
 }