1 files changed, 50 insertions, 5 deletions

diff --git a/drivers/scsi/libiscsi.c b/drivers/scsi/libiscsi.c
index bdd7de7da39a..2f6b0955ff01 100644
--- a/drivers/scsi/libiscsi.c
+++ b/drivers/scsi/libiscsi.c
@@ -137,6 +137,45 @@ static int iscsi_add_hdr(struct iscsi_cmd_task *ctask, unsigned len)
         return 0;
 }
 
+/*
+ * make an extended cdb AHS
+ */
+static int iscsi_prep_ecdb_ahs(struct iscsi_cmd_task *ctask)
+{
+        struct scsi_cmnd *cmd = ctask->sc;
+        unsigned rlen, pad_len;
+        unsigned short ahslength;
+        struct iscsi_ecdb_ahdr *ecdb_ahdr;
+        int rc;
+
+        ecdb_ahdr = iscsi_next_hdr(ctask);
+        rlen = cmd->cmd_len - ISCSI_CDB_SIZE;
+
+        BUG_ON(rlen > sizeof(ecdb_ahdr->ecdb));
+        ahslength = rlen + sizeof(ecdb_ahdr->reserved);
+
+        pad_len = iscsi_padding(rlen);
+
+        rc = iscsi_add_hdr(ctask, sizeof(ecdb_ahdr->ahslength) +
+                           sizeof(ecdb_ahdr->ahstype) + ahslength + pad_len);
+        if (rc)
+                return rc;
+
+        if (pad_len)
+                memset(&ecdb_ahdr->ecdb[rlen], 0, pad_len);
+
+        ecdb_ahdr->ahslength = cpu_to_be16(ahslength);
+        ecdb_ahdr->ahstype = ISCSI_AHSTYPE_CDB;
+        ecdb_ahdr->reserved = 0;
+        memcpy(ecdb_ahdr->ecdb, cmd->cmnd + ISCSI_CDB_SIZE, rlen);
+
+        debug_scsi("iscsi_prep_ecdb_ahs: varlen_cdb_len %d "
+                   "rlen %d pad_len %d ahs_length %d iscsi_headers_size %u\n",
+                   cmd->cmd_len, rlen, pad_len, ahslength, ctask->hdr_len);
+
+        return 0;
+}
+
 /**
  * iscsi_prep_scsi_cmd_pdu - prep iscsi scsi cmd pdu
  * @ctask: iscsi cmd task
@@ -150,7 +189,7 @@ static int iscsi_prep_scsi_cmd_pdu(struct iscsi_cmd_task *ctask)
         struct iscsi_session *session = conn->session;
         struct iscsi_cmd *hdr = ctask->hdr;
         struct scsi_cmnd *sc = ctask->sc;
-        unsigned hdrlength;
+        unsigned hdrlength, cmd_len;
         int rc;
 
         ctask->hdr_len = 0;
@@ -165,10 +204,16 @@ static int iscsi_prep_scsi_cmd_pdu(struct iscsi_cmd_task *ctask)
         hdr->cmdsn = cpu_to_be32(session->cmdsn);
         session->cmdsn++;
         hdr->exp_statsn = cpu_to_be32(conn->exp_statsn);
-        memcpy(hdr->cdb, sc->cmnd, sc->cmd_len);
-        if (sc->cmd_len < MAX_COMMAND_SIZE)
-                memset(&hdr->cdb[sc->cmd_len], 0,
-                        MAX_COMMAND_SIZE - sc->cmd_len);
+        cmd_len = sc->cmd_len;
+        if (cmd_len < ISCSI_CDB_SIZE)
+                memset(&hdr->cdb[cmd_len], 0, ISCSI_CDB_SIZE - cmd_len);
+        else if (cmd_len > ISCSI_CDB_SIZE) {
+                rc = iscsi_prep_ecdb_ahs(ctask);
+                if (rc)
+                        return rc;
+                cmd_len = ISCSI_CDB_SIZE;
+        }
+        memcpy(hdr->cdb, sc->cmnd, cmd_len);
 
         ctask->imm_count = 0;
         if (sc->sc_data_direction == DMA_TO_DEVICE) {