struct iscsi_tcp_conn *tcp_conn = conn->dd_data;
 
        crypto_hash_digest(&tcp_conn->tx_hash, &buf->sg, buf->sg.length, crc);
-       buf->sg.length += sizeof(u32);
+       buf->sg.length += ISCSI_DIGEST_SIZE;
 }
 
 /*
 iscsi_tcp_chunk_done(struct iscsi_chunk *chunk)
 {
        static unsigned char padbuf[ISCSI_PAD_LEN];
+       unsigned int pad;
 
        if (chunk->copied < chunk->size) {
                iscsi_tcp_chunk_map(chunk);
        }
 
        /* Do we need to handle padding? */
-       if (chunk->total_copied & (ISCSI_PAD_LEN-1)) {
-               unsigned int pad;
-
-               pad = ISCSI_PAD_LEN - (chunk->total_copied & (ISCSI_PAD_LEN-1));
+       pad = iscsi_padding(chunk->total_copied);
+       if (pad != 0) {
                debug_tcp("consume %d pad bytes\n", pad);
                chunk->total_size += pad;
                chunk->size = pad;
                }
 
                iscsi_buf_init_iov(&tcp_ctask->headbuf, (char*)ctask->hdr,
-                                 sizeof(struct iscsi_hdr));
+                                 ctask->hdr_len);
 
                if (conn->hdrdgst_en)
                        iscsi_hdr_digest(conn, &tcp_ctask->headbuf,
-                                        (u8*)tcp_ctask->hdrext);
+                                        iscsi_next_hdr(ctask));
                tcp_ctask->xmstate &= ~XMSTATE_CMD_HDR_INIT;
                tcp_ctask->xmstate |= XMSTATE_CMD_HDR_XMIT;
        }
                struct iscsi_cmd_task *ctask = session->cmds[cmd_i];
                struct iscsi_tcp_cmd_task *tcp_ctask = ctask->dd_data;
 
-               ctask->hdr = &tcp_ctask->hdr;
+               ctask->hdr = &tcp_ctask->hdr.cmd_hdr;
+               ctask->hdr_max = sizeof(tcp_ctask->hdr) - ISCSI_DIGEST_SIZE;
        }
 
        for (cmd_i = 0; cmd_i < session->mgmtpool_max; cmd_i++) {
 
 #define XMSTATE_IMM_HDR_INIT           0x1000
 #define XMSTATE_SOL_HDR_INIT           0x2000
 
-#define ISCSI_PAD_LEN                  4
 #define ISCSI_SG_TABLESIZE             SG_ALL
 #define ISCSI_TCP_MAX_CMD_LEN          16
 
 
 struct iscsi_data_task {
        struct iscsi_data       hdr;                    /* PDU */
-       char                    hdrext[sizeof(__u32)];  /* Header-Digest */
+       char                    hdrext[ISCSI_DIGEST_SIZE];/* Header-Digest */
        struct iscsi_buf        digestbuf;              /* digest buffer */
        uint32_t                digest;                 /* data digest */
 };
 
 struct iscsi_tcp_mgmt_task {
        struct iscsi_hdr        hdr;
-       char                    hdrext[sizeof(__u32)]; /* Header-Digest */
+       char                    hdrext[ISCSI_DIGEST_SIZE]; /* Header-Digest */
        int                     xmstate;        /* mgmt xmit progress */
        struct iscsi_buf        headbuf;        /* header buffer */
        struct iscsi_buf        sendbuf;        /* in progress buffer */
 };
 
 struct iscsi_tcp_cmd_task {
-       struct iscsi_cmd        hdr;
-       char                    hdrext[4*sizeof(__u16)+ /* AHS */
-                                   sizeof(__u32)];     /* HeaderDigest */
+       struct iscsi_hdr_buff {
+               struct iscsi_cmd        cmd_hdr;
+               char                    hdrextbuf[ISCSI_MAX_AHS_SIZE +
+                                                 ISCSI_DIGEST_SIZE];
+       } hdr;
        char                    pad[ISCSI_PAD_LEN];
        int                     pad_count;              /* padded bytes */
        struct iscsi_buf        headbuf;                /* header buf (xmit) */
 
 #include <scsi/scsi_transport_iscsi.h>
 #include <scsi/libiscsi.h>
 
+static void fail_command(struct iscsi_conn *conn, struct iscsi_cmd_task *ctask,
+                        int err);
+
 struct iscsi_session *
 class_to_transport_session(struct iscsi_cls_session *cls_session)
 {
 }
 EXPORT_SYMBOL_GPL(iscsi_prep_unsolicit_data_pdu);
 
+static int iscsi_add_hdr(struct iscsi_cmd_task *ctask, unsigned len)
+{
+       unsigned exp_len = ctask->hdr_len + len;
+
+       if (exp_len > ctask->hdr_max) {
+               WARN_ON(1);
+               return -EINVAL;
+       }
+
+       WARN_ON(len & (ISCSI_PAD_LEN - 1)); /* caller must pad the AHS */
+       ctask->hdr_len = exp_len;
+       return 0;
+}
+
 /**
  * iscsi_prep_scsi_cmd_pdu - prep iscsi scsi cmd pdu
  * @ctask: iscsi cmd task
  * Prep basic iSCSI PDU fields for a scsi cmd pdu. The LLD should set
  * fields like dlength or final based on how much data it sends
  */
-static void iscsi_prep_scsi_cmd_pdu(struct iscsi_cmd_task *ctask)
+static int iscsi_prep_scsi_cmd_pdu(struct iscsi_cmd_task *ctask)
 {
        struct iscsi_conn *conn = ctask->conn;
        struct iscsi_session *session = conn->session;
        struct iscsi_cmd *hdr = ctask->hdr;
        struct scsi_cmnd *sc = ctask->sc;
+       unsigned hdrlength;
+       int rc;
 
+       ctask->hdr_len = 0;
+       rc = iscsi_add_hdr(ctask, sizeof(*hdr));
+       if (rc)
+               return rc;
         hdr->opcode = ISCSI_OP_SCSI_CMD;
         hdr->flags = ISCSI_ATTR_SIMPLE;
         int_to_scsilun(sc->device->lun, (struct scsi_lun *)hdr->lun);
                        hdr->flags |= ISCSI_FLAG_CMD_READ;
        }
 
+       /* calculate size of additional header segments (AHSs) */
+       hdrlength = ctask->hdr_len - sizeof(*hdr);
+
+       WARN_ON(hdrlength & (ISCSI_PAD_LEN-1));
+       hdrlength /= ISCSI_PAD_LEN;
+
+       WARN_ON(hdrlength >= 256);
+       hdr->hlength = hdrlength & 0xFF;
+
        conn->scsicmd_pdus_cnt++;
 
         debug_scsi("iscsi prep [%s cid %d sc %p cdb 0x%x itt 0x%x len %d "
                 sc->sc_data_direction == DMA_TO_DEVICE ? "write" : "read",
                conn->id, sc, sc->cmnd[0], ctask->itt, scsi_bufflen(sc),
                 session->cmdsn, session->max_cmdsn - session->exp_cmdsn + 1);
+       return 0;
 }
 
 /**
 
                conn->ctask = list_entry(conn->xmitqueue.next,
                                         struct iscsi_cmd_task, running);
-               iscsi_prep_scsi_cmd_pdu(conn->ctask);
+               if (iscsi_prep_scsi_cmd_pdu(conn->ctask)) {
+                       fail_command(conn, conn->ctask, DID_ABORT << 16);
+                       continue;
+               }
                conn->session->tt->init_cmd_task(conn->ctask);
                conn->ctask->state = ISCSI_TASK_RUNNING;
                list_move_tail(conn->xmitqueue.next, &conn->run_list);
                if (cmd_task_size)
                        ctask->dd_data = &ctask[1];
                ctask->itt = cmd_i;
+               ctask->hdr_max = sizeof(struct iscsi_cmd);
                INIT_LIST_HEAD(&ctask->running);
        }
 
 
 #define ISCSI_LISTEN_PORT      3260
 
 /* Padding word length */
-#define PAD_WORD_LEN           4
+#define ISCSI_PAD_LEN          4
 
 /*
  * useful common(control and data pathes) macro
        __be32 read_length;
 };
 
+/* Extended CDB AHS */
+struct iscsi_ecdb_ahdr {
+       __be16 ahslength;       /* CDB length - 15, including reserved byte */
+       uint8_t ahstype;
+       uint8_t reserved;
+       uint8_t ecdb[260 - 16]; /* 4-byte aligned extended CDB spillover */
+};
+
 /* SCSI Response Header */
 struct iscsi_cmd_rsp {
        uint8_t opcode;
 
 #define ISCSI_ADDRESS_BUF_LEN          64
 
 enum {
+       /* this is the maximum possible storage for AHSs */
+       ISCSI_MAX_AHS_SIZE = sizeof(struct iscsi_ecdb_ahdr) +
+                               sizeof(struct iscsi_rlength_ahdr),
        ISCSI_DIGEST_SIZE = sizeof(__u32),
 };
 
 
 struct iscsi_cmd_task {
        /*
-        * Becuae LLDs allocate their hdr differently, this is a pointer to
-        * that storage. It must be setup at session creation time.
+        * Because LLDs allocate their hdr differently, this is a pointer
+        * and length to that storage. It must be setup at session
+        * creation time.
         */
        struct iscsi_cmd        *hdr;
+       unsigned short          hdr_max;
+       unsigned short          hdr_len;        /* accumulated size of hdr used */
        int                     itt;            /* this ITT */
 
        uint32_t                unsol_datasn;
        void                    *dd_data;       /* driver/transport data */
 };
 
+static inline void* iscsi_next_hdr(struct iscsi_cmd_task *ctask)
+{
+       return (void*)ctask->hdr + ctask->hdr_len;
+}
+
 struct iscsi_conn {
        struct iscsi_cls_conn   *cls_conn;      /* ptr to class connection */
        void                    *dd_data;       /* iscsi_transport data */
 extern void iscsi_pool_free(struct iscsi_queue *, void **);
 extern int iscsi_pool_init(struct iscsi_queue *, int, void ***, int);
 
+/*
+ * inline functions to deal with padding.
+ */
+static inline unsigned int
+iscsi_padded(unsigned int len)
+{
+       return (len + ISCSI_PAD_LEN - 1) & ~(ISCSI_PAD_LEN - 1);
+}
+
+static inline unsigned int
+iscsi_padding(unsigned int len)
+{
+       len &= (ISCSI_PAD_LEN - 1);
+       if (len)
+               len = ISCSI_PAD_LEN - len;
+       return len;
+}
+
 #endif