From 5222aaf223e52961cabeb7cabc579892ccd8bc59 Mon Sep 17 00:00:00 2001
From: Paolo Bonzini <pbonzini@redhat.com>
Date: Thu, 9 Aug 2012 11:00:22 +0200
Subject: [PATCH] scsi-disk: add support for the UNMAP command

The unmap command can reuse the same infrastructure as MODE SELECT
for reading the descriptor list into memory.  The descriptors are
processed sequentially.

Signed-off-by: Paolo Bonzini <pbonzini@redhat.com>
---
 hw/scsi-disk.c | 93 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 92 insertions(+), 1 deletion(-)

diff --git a/hw/scsi-disk.c b/hw/scsi-disk.c
index 584aec13fb..e71809e091 100644
--- a/hw/scsi-disk.c
+++ b/hw/scsi-disk.c
@@ -637,7 +637,7 @@ static int scsi_disk_emulate_inquiry(SCSIRequest *req, uint8_t *outbuf)
         {
             buflen = 8;
             outbuf[4] = 0;
-            outbuf[5] = 0x60; /* write_same 10/16 supported */
+            outbuf[5] = 0xe0; /* unmap & write_same 10/16 all supported */
             outbuf[6] = s->qdev.conf.discard_granularity ? 2 : 1;
             outbuf[7] = 0;
             break;
@@ -1449,6 +1449,89 @@ invalid_field:
     return;
 }
 
+typedef struct UnmapCBData {
+    SCSIDiskReq *r;
+    uint8_t *inbuf;
+    int count;
+} UnmapCBData;
+
+static void scsi_unmap_complete(void *opaque, int ret)
+{
+    UnmapCBData *data = opaque;
+    SCSIDiskReq *r = data->r;
+    SCSIDiskState *s = DO_UPCAST(SCSIDiskState, qdev, r->req.dev);
+    uint64_t sector_num;
+    uint32 nb_sectors;
+
+    r->req.aiocb = NULL;
+    if (ret < 0) {
+        if (scsi_handle_rw_error(r, -ret)) {
+            goto done;
+        }
+    }
+
+    if (data->count > 0 && !r->req.io_canceled) {
+        sector_num = ldq_be_p(&data->inbuf[0]);
+        nb_sectors = ldl_be_p(&data->inbuf[8]) & 0xffffffffULL;
+        if (sector_num > sector_num + nb_sectors ||
+            sector_num + nb_sectors - 1 > s->qdev.max_lba) {
+            scsi_check_condition(r, SENSE_CODE(LBA_OUT_OF_RANGE));
+            goto done;
+        }
+
+        r->req.aiocb = bdrv_aio_discard(s->qdev.conf.bs,
+                                        sector_num * (s->qdev.blocksize / 512),
+                                        nb_sectors * (s->qdev.blocksize / 512),
+                                        scsi_unmap_complete, data);
+        data->count--;
+        data->inbuf += 16;
+        return;
+    }
+
+done:
+    if (data->count == 0) {
+        scsi_req_complete(&r->req, GOOD);
+    }
+    if (!r->req.io_canceled) {
+        scsi_req_unref(&r->req);
+    }
+    g_free(data);
+}
+
+static void scsi_disk_emulate_unmap(SCSIDiskReq *r, uint8_t *inbuf)
+{
+    uint8_t *p = inbuf;
+    int len = r->req.cmd.xfer;
+    UnmapCBData *data;
+
+    if (len < 8) {
+        goto invalid_param_len;
+    }
+    if (len < lduw_be_p(&p[0]) + 2) {
+        goto invalid_param_len;
+    }
+    if (len < lduw_be_p(&p[2]) + 8) {
+        goto invalid_param_len;
+    }
+    if (lduw_be_p(&p[2]) & 15) {
+        goto invalid_param_len;
+    }
+
+    data = g_new0(UnmapCBData, 1);
+    data->r = r;
+    data->inbuf = &p[8];
+    data->count = lduw_be_p(&p[2]) >> 4;
+
+    /* The matching unref is in scsi_unmap_complete, before data is freed.  */
+    scsi_req_ref(&r->req);
+    scsi_unmap_complete(data, 0);
+    return;
+
+invalid_param_len:
+    scsi_check_condition(r, SENSE_CODE(INVALID_PARAM_LEN));
+    return;
+}
+
 static void scsi_disk_emulate_write_data(SCSIRequest *req)
 {
     SCSIDiskReq *r = DO_UPCAST(SCSIDiskReq, req, req);
@@ -1468,6 +1551,10 @@ static void scsi_disk_emulate_write_data(SCSIRequest *req)
         scsi_disk_emulate_mode_select(r, r->iov.iov_base);
         break;
 
+    case UNMAP:
+        scsi_disk_emulate_unmap(r, r->iov.iov_base);
+        break;
+
     default:
         abort();
     }
@@ -1702,6 +1789,9 @@ static int32_t scsi_disk_emulate_command(SCSIRequest *req, uint8_t *buf)
     case MODE_SELECT_10:
         DPRINTF("Mode Select(10) (len %lu)\n", (long)r->req.cmd.xfer);
         break;
+    case UNMAP:
+        DPRINTF("Unmap (len %lu)\n", (long)r->req.cmd.xfer);
+        break;
     case WRITE_SAME_10:
         nb_sectors = lduw_be_p(&req->cmd.buf[7]);
         goto write_same;
@@ -2067,6 +2157,7 @@ static const SCSIReqOps *const scsi_disk_reqops_dispatch[256] = {
     [SEEK_10]                         = &scsi_disk_emulate_reqops,
     [MODE_SELECT]                     = &scsi_disk_emulate_reqops,
     [MODE_SELECT_10]                  = &scsi_disk_emulate_reqops,
+    [UNMAP]                           = &scsi_disk_emulate_reqops,
     [WRITE_SAME_10]                   = &scsi_disk_emulate_reqops,
     [WRITE_SAME_16]                   = &scsi_disk_emulate_reqops,