vhost/scsi: Respond to control queue operations

	mutex_unlock(&vq->mutex);

}

static void

vhost_scsi_send_tmf_resp(struct vhost_scsi *vs,

			   struct vhost_virtqueue *vq,

			   int head, unsigned int out)

{

	struct virtio_scsi_ctrl_tmf_resp __user *resp;

	struct virtio_scsi_ctrl_tmf_resp rsp;

	int ret;

	pr_debug("%s\n", __func__);

	memset(&rsp, 0, sizeof(rsp));

	rsp.response = VIRTIO_SCSI_S_FUNCTION_REJECTED;

	resp = vq->iov[out].iov_base;

	ret = __copy_to_user(resp, &rsp, sizeof(rsp));

	if (!ret)

		vhost_add_used_and_signal(&vs->dev, vq, head, 0);

	else

		pr_err("Faulted on virtio_scsi_ctrl_tmf_resp\n");

}

static void

vhost_scsi_send_an_resp(struct vhost_scsi *vs,

			   struct vhost_virtqueue *vq,

			   int head, unsigned int out)

{

	struct virtio_scsi_ctrl_an_resp __user *resp;

	struct virtio_scsi_ctrl_an_resp rsp;

	int ret;

	pr_debug("%s\n", __func__);

	memset(&rsp, 0, sizeof(rsp));	/* event_actual = 0 */

	rsp.response = VIRTIO_SCSI_S_OK;

	resp = vq->iov[out].iov_base;

	ret = __copy_to_user(resp, &rsp, sizeof(rsp));

	if (!ret)

		vhost_add_used_and_signal(&vs->dev, vq, head, 0);

	else

		pr_err("Faulted on virtio_scsi_ctrl_an_resp\n");

}

static void

vhost_scsi_ctl_handle_vq(struct vhost_scsi *vs, struct vhost_virtqueue *vq)

{

	union {

		__virtio32 type;

		struct virtio_scsi_ctrl_an_req an;

		struct virtio_scsi_ctrl_tmf_req tmf;

	} v_req;

	struct iov_iter out_iter;

	unsigned int out = 0, in = 0;

	int head;

	size_t req_size, rsp_size, typ_size;

	size_t out_size, in_size;

	u8 *lunp;

	void *req;

	mutex_lock(&vq->mutex);

	/*

	 * We can handle the vq only after the endpoint is setup by calling the

	 * VHOST_SCSI_SET_ENDPOINT ioctl.

	 */

	if (!vq->private_data)

		goto out;

	vhost_disable_notify(&vs->dev, vq);

	for (;;) {

		head = vhost_get_vq_desc(vq, vq->iov,

					 ARRAY_SIZE(vq->iov), &out, &in,

					 NULL, NULL);

		pr_debug("vhost_get_vq_desc: head: %d, out: %u in: %u\n",

			 head, out, in);

		/* On error, stop handling until the next kick. */

		if (unlikely(head < 0))

			break;

		/* Nothing new?  Wait for eventfd to tell us they refilled. */

		if (head == vq->num) {

			if (unlikely(vhost_enable_notify(&vs->dev, vq))) {

				vhost_disable_notify(&vs->dev, vq);

				continue;

			}

			break;

		}

		/*

		 * Get the size of request and response buffers.

		 */

		out_size = iov_length(vq->iov, out);

		in_size = iov_length(&vq->iov[out], in);

		/*

		 * Copy over the virtio-scsi request header, which for a

		 * ANY_LAYOUT enabled guest may span multiple iovecs, or a

		 * single iovec may contain both the header + outgoing

		 * WRITE payloads.

		 *

		 * copy_from_iter() will advance out_iter, so that it will

		 * point at the start of the outgoing WRITE payload, if

		 * DMA_TO_DEVICE is set.

		 */

		iov_iter_init(&out_iter, WRITE, vq->iov, out, out_size);

		req = &v_req.type;

		typ_size = sizeof(v_req.type);

		if (unlikely(!copy_from_iter_full(req, typ_size, &out_iter))) {

			vq_err(vq, "Faulted on copy_from_iter tmf type\n");

			/*

			 * The size of the response buffer varies based on

			 * the request type and must be validated against it.

			 * Since the request type is not known, don't send

			 * a response.

			 */

			continue;

		}

		switch (v_req.type) {

		case VIRTIO_SCSI_T_TMF:

			req = &v_req.tmf;

			lunp = &v_req.tmf.lun[0];

			req_size = sizeof(struct virtio_scsi_ctrl_tmf_req);

			rsp_size = sizeof(struct virtio_scsi_ctrl_tmf_resp);

			break;

		case VIRTIO_SCSI_T_AN_QUERY:

		case VIRTIO_SCSI_T_AN_SUBSCRIBE:

			req = &v_req.an;

			lunp = &v_req.an.lun[0];

			req_size = sizeof(struct virtio_scsi_ctrl_an_req);

			rsp_size = sizeof(struct virtio_scsi_ctrl_an_resp);

			break;

		default:

			vq_err(vq, "Unknown control request %d", v_req.type);

			continue;

		}

		/*

		 * Check for a sane response buffer so we can report early

		 * errors back to the guest.

		 */

		if (unlikely(in_size < rsp_size)) {

			vq_err(vq,

			       "Resp buf too small, need min %zu bytes got %zu",

			       rsp_size, in_size);

			/*

			 * Notifications are disabled at this point;

			 * continue so they can be eventually enabled

			 * when processing terminates.

			 */

			continue;

		}

		if (unlikely(out_size < req_size)) {

			vq_err(vq,

			       "Req buf too small, need min %zu bytes got %zu",

			       req_size, out_size);

			vhost_scsi_send_bad_target(vs, vq, head, out);

			continue;

		}

		req += typ_size;

		req_size -= typ_size;

		if (unlikely(!copy_from_iter_full(req, req_size, &out_iter))) {

			vq_err(vq, "Faulted on copy_from_iter\n");

			vhost_scsi_send_bad_target(vs, vq, head, out);

			continue;

		}

		/* virtio-scsi spec requires byte 0 of the lun to be 1 */

		if (unlikely(*lunp != 1)) {

			vq_err(vq, "Illegal virtio-scsi lun: %u\n", *lunp);

			vhost_scsi_send_bad_target(vs, vq, head, out);

			continue;

		}

		if (v_req.type == VIRTIO_SCSI_T_TMF) {

			pr_debug("%s tmf %d\n", __func__, v_req.tmf.subtype);

			vhost_scsi_send_tmf_resp(vs, vq, head, out);

		} else

			vhost_scsi_send_an_resp(vs, vq, head, out);

	}

out:

	mutex_unlock(&vq->mutex);

}

static void vhost_scsi_ctl_handle_kick(struct vhost_work *work)

{

	struct vhost_virtqueue *vq = container_of(work, struct vhost_virtqueue,

						poll.work);

	struct vhost_scsi *vs = container_of(vq->dev, struct vhost_scsi, dev);

	pr_debug("%s: The handling func for control queue.\n", __func__);

	vhost_scsi_ctl_handle_vq(vs, vq);

}

static void