usr/fileio/common.c

   1 /*
   2  *  common.c
   3  *
   4  *  Copyright (C) 2007 - 2008 Vladislav Bolkhovitin <vst@vlnb.net>
   5  *  Copyright (C) 2007 - 2008 CMS Distribution Limited
   6  *
   7  *  This program is free software; you can redistribute it and/or
   8  *  modify it under the terms of the GNU General Public License
   9  *  as published by the Free Software Foundation, version 2
  10  *  of the License.
  11  *
  12  *  This program is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  15  *  GNU General Public License for more details.
  16  */
  17
  18 #include <ctype.h>
  19 #include <errno.h>
  20 #include <fcntl.h>
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <unistd.h>
  25 #include <stdint.h>
  26 #include <getopt.h>
  27 #include <inttypes.h>
  28
  29 #include <sys/ioctl.h>
  30 #include <sys/poll.h>
  31
  32 #include <arpa/inet.h>
  33
  34 #include <pthread.h>
  35
  36 #include "common.h"
  37
  38 static unsigned int random_values[256] = {
  39             9862592UL,  3744545211UL,  2348289082UL,  4036111983UL,
  40           435574201UL,  3110343764UL,  2383055570UL,  1826499182UL,
  41          4076766377UL,  1549935812UL,  3696752161UL,  1200276050UL,
  42          3878162706UL,  1783530428UL,  2291072214UL,   125807985UL,
  43          3407668966UL,   547437109UL,  3961389597UL,   969093968UL,
  44            56006179UL,  2591023451UL,     1849465UL,  1614540336UL,
  45          3699757935UL,   479961779UL,  3768703953UL,  2529621525UL,
  46          4157893312UL,  3673555386UL,  4091110867UL,  2193909423UL,
  47          2800464448UL,  3052113233UL,   450394455UL,  3424338713UL,
  48          2113709130UL,  4082064373UL,  3708640918UL,  3841182218UL,
  49          3141803315UL,  1032476030UL,  1166423150UL,  1169646901UL,
  50          2686611738UL,   575517645UL,  2829331065UL,  1351103339UL,
  51          2856560215UL,  2402488288UL,   867847666UL,     8524618UL,
  52           704790297UL,  2228765657UL,   231508411UL,  1425523814UL,
  53          2146764591UL,  1287631730UL,  4142687914UL,  3879884598UL,
  54           729945311UL,   310596427UL,  2263511876UL,  1983091134UL,
  55          3500916580UL,  1642490324UL,  3858376049UL,   695342182UL,
  56           780528366UL,  1372613640UL,  1100993200UL,  1314818946UL,
  57           572029783UL,  3775573540UL,   776262915UL,  2684520905UL,
  58          1007252738UL,  3505856396UL,  1974886670UL,  3115856627UL,
  59          4194842288UL,  2135793908UL,  3566210707UL,     7929775UL,
  60          1321130213UL,  2627281746UL,  3587067247UL,  2025159890UL,
  61          2587032000UL,  3098513342UL,  3289360258UL,   130594898UL,
  62          2258149812UL,  2275857755UL,  3966929942UL,  1521739999UL,
  63          4191192765UL,   958953550UL,  4153558347UL,  1011030335UL,
  64           524382185UL,  4099757640UL,   498828115UL,  2396978754UL,
  65           328688935UL,   826399828UL,  3174103611UL,  3921966365UL,
  66          2187456284UL,  2631406787UL,  3930669674UL,  4282803915UL,
  67          1776755417UL,   374959755UL,  2483763076UL,   844956392UL,
  68          2209187588UL,  3647277868UL,   291047860UL,  3485867047UL,
  69          2223103546UL,  2526736133UL,  3153407604UL,  3828961796UL,
  70          3355731910UL,  2322269798UL,  2752144379UL,   519897942UL,
  71          3430536488UL,  1801511593UL,  1953975728UL,  3286944283UL,
  72          1511612621UL,  1050133852UL,   409321604UL,  1037601109UL,
  73          3352316843UL,  4198371381UL,   617863284UL,   994672213UL,
  74          1540735436UL,  2337363549UL,  1242368492UL,   665473059UL,
  75          2330728163UL,  3443103219UL,  2291025133UL,  3420108120UL,
  76          2663305280UL,  1608969839UL,  2278959931UL,  1389747794UL,
  77          2226946970UL,  2131266900UL,  3856979144UL,  1894169043UL,
  78          2692697628UL,  3797290626UL,  3248126844UL,  3922786277UL,
  79           343705271UL,  3739749888UL,  2191310783UL,  2962488787UL,
  80          4119364141UL,  1403351302UL,  2984008923UL,  3822407178UL,
  81          1932139782UL,  2323869332UL,  2793574182UL,  1852626483UL,
  82          2722460269UL,  1136097522UL,  1005121083UL,  1805201184UL,
  83          2212824936UL,  2979547931UL,  4133075915UL,  2585731003UL,
  84          2431626071UL,   134370235UL,  3763236829UL,  1171434827UL,
  85          2251806994UL,  1289341038UL,  3616320525UL,   392218563UL,
  86          1544502546UL,  2993937212UL,  1957503701UL,  3579140080UL,
  87          4270846116UL,  2030149142UL,  1792286022UL,   366604999UL,
  88          2625579499UL,   790898158UL,   770833822UL,   815540197UL,
  89          2747711781UL,  3570468835UL,  3976195842UL,  1257621341UL,
  90          1198342980UL,  1860626190UL,  3247856686UL,   351473955UL,
  91           993440563UL,   340807146UL,  1041994520UL,  3573925241UL,
  92           480246395UL,  2104806831UL,  1020782793UL,  3362132583UL,
  93          2272911358UL,  3440096248UL,  2356596804UL,   259492703UL,
  94          3899500740UL,   252071876UL,  2177024041UL,  4284810959UL,
  95          2775999888UL,  2653420445UL,  2876046047UL,  1025771859UL,
  96          1994475651UL,  3564987377UL,  4112956647UL,  1821511719UL,
  97          3113447247UL,   455315102UL,  1585273189UL,  2311494568UL,
  98           774051541UL,  1898115372UL,  2637499516UL,   247231365UL,
  99          1475014417UL,   803585727UL,  3911097303UL,  1714292230UL,
 100           476579326UL,  2496900974UL,  3397613314UL,   341202244UL,
 101           807790202UL,  4221326173UL,   499979741UL,  1301488547UL,
 102          1056807896UL,  3525009458UL,  1174811641UL,  3049738746UL,
 103 };
 104
 105 static void exec_inquiry(struct vdisk_cmd *vcmd);
 106 static void exec_request_sense(struct vdisk_cmd *vcmd);
 107 static void exec_mode_sense(struct vdisk_cmd *vcmd);
 108 static void exec_mode_select(struct vdisk_cmd *vcmd);
 109 static void exec_read_capacity(struct vdisk_cmd *vcmd);
 110 static void exec_read_capacity16(struct vdisk_cmd *vcmd);
 111 static void exec_read_toc(struct vdisk_cmd *vcmd);
 112 static void exec_prevent_allow_medium_removal(struct vdisk_cmd *vcmd);
 113 static int exec_fsync(struct vdisk_cmd *vcmd);
 114 static void exec_read(struct vdisk_cmd *vcmd, loff_t loff);
 115 static void exec_write(struct vdisk_cmd *vcmd, loff_t loff);
 116 static void exec_verify(struct vdisk_cmd *vcmd, loff_t loff);
 117
 118 static inline void set_cmd_error_status(struct scst_user_scsi_cmd_reply_exec *reply,
 119         int status)
 120 {
 121         reply->status = status;
 122         reply->resp_data_len = 0;
 123         return;
 124 }
 125
 126 static int set_sense(uint8_t *buffer, int len, int key, int asc, int ascq)
 127 {
 128         int res = 14;
 129         EXTRACHECKS_BUG_ON(len < res);
 130         memset(buffer, 0, res);
 131         buffer[0] = 0x70;       /* Error Code                   */
 132         buffer[2] = key;        /* Sense Key                    */
 133         buffer[7] = 0x0a;       /* Additional Sense Length      */
 134         buffer[12] = asc;       /* ASC                          */
 135         buffer[13] = ascq;      /* ASCQ                         */
 136         TRACE_BUFFER("Sense set", buffer, res);
 137         return res;
 138 }
 139
 140 void set_cmd_error(struct vdisk_cmd *vcmd, int key, int asc, int ascq)
 141 {
 142         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
 143
 144         TRACE_ENTRY();
 145
 146         EXTRACHECKS_BUG_ON(vcmd->cmd->subcode != SCST_USER_EXEC);
 147
 148         set_cmd_error_status(reply, SAM_STAT_CHECK_CONDITION);
 149         reply->sense_len = set_sense(vcmd->sense, sizeof(vcmd->sense), key,
 150                 asc, ascq);
 151         reply->psense_buffer = (unsigned long)vcmd->sense;
 152
 153         TRACE_EXIT();
 154         return;
 155 }
 156
 157 void set_busy(struct scst_user_scsi_cmd_reply_exec *reply)
 158 {
 159         TRACE_ENTRY();
 160
 161         set_cmd_error_status(reply, SAM_STAT_TASK_SET_FULL);
 162         TRACE_MGMT_DBG("%s", "Sending QUEUE_FULL status");
 163
 164         TRACE_EXIT();
 165         return;
 166 }
 167
 168 static int do_parse(struct vdisk_cmd *vcmd)
 169 {
 170         int res = 0;
 171         struct scst_user_scsi_cmd_parse *cmd = &vcmd->cmd->parse_cmd;
 172         struct scst_user_scsi_cmd_reply_parse *reply = &vcmd->reply->parse_reply;
 173
 174         TRACE_ENTRY();
 175
 176         memset(reply, 0, sizeof(*reply));
 177         vcmd->reply->cmd_h = vcmd->cmd->cmd_h;
 178         vcmd->reply->subcode = vcmd->cmd->subcode;
 179
 180         if (cmd->expected_values_set == 0) {
 181                 PRINT_ERROR("%s", "Oops, expected values are not set");
 182                 reply->bufflen = -1; /* invalid value */
 183                 goto out;
 184         }
 185
 186         reply->queue_type = cmd->queue_type;
 187         reply->data_direction = cmd->expected_data_direction;
 188         reply->data_len = cmd->expected_transfer_len;
 189         reply->bufflen = cmd->expected_transfer_len;
 190
 191 out:
 192         TRACE_EXIT_RES(res);
 193         return res;
 194 }
 195
 196 struct vdisk_tgt_dev *find_tgt_dev(struct vdisk_dev *dev, uint64_t sess_h)
 197 {
 198         unsigned int i;
 199         struct vdisk_tgt_dev *res = NULL;
 200
 201         for(i = 0; i < ARRAY_SIZE(dev->tgt_devs); i++) {
 202                 if (dev->tgt_devs[i].sess_h == sess_h) {
 203                         res = &dev->tgt_devs[i];
 204                         break;
 205                 }
 206         }
 207         return res;
 208 }
 209
 210 struct vdisk_tgt_dev *find_empty_tgt_dev(struct vdisk_dev *dev)
 211 {
 212         unsigned int i;
 213         struct vdisk_tgt_dev *res = NULL;
 214
 215         for(i = 0; i < ARRAY_SIZE(dev->tgt_devs); i++) {
 216                 if (dev->tgt_devs[i].sess_h == 0) {
 217                         res = &dev->tgt_devs[i];
 218                         break;
 219                 }
 220         }
 221         return res;
 222 }
 223
 224 static inline int sync_queue_type(enum scst_cmd_queue_type qt)
 225 {
 226         switch(qt) {
 227                 case SCST_CMD_QUEUE_ORDERED:
 228                 case SCST_CMD_QUEUE_HEAD_OF_QUEUE:
 229                         return 1;
 230                 default:
 231                         return 0;
 232         }
 233 }
 234
 235 static inline int need_pre_sync(enum scst_cmd_queue_type cur,
 236         enum scst_cmd_queue_type last)
 237 {
 238         if (sync_queue_type(cur))
 239                 if (!sync_queue_type(last))
 240                         return 1;
 241         return 0;
 242 }
 243
 244 static int do_exec(struct vdisk_cmd *vcmd)
 245 {
 246         int res = 0;
 247         struct vdisk_dev *dev = vcmd->dev;
 248         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
 249         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
 250         uint64_t lba_start = 0;
 251         loff_t data_len = 0;
 252         uint8_t *cdb = cmd->cdb;
 253         int opcode = cdb[0];
 254         loff_t loff;
 255         int fua = 0;
 256
 257         TRACE_ENTRY();
 258
 259         switch(cmd->queue_type) {
 260         case SCST_CMD_QUEUE_ORDERED:
 261                 TRACE(TRACE_ORDER, "ORDERED cmd_h %d", vcmd->cmd->cmd_h);
 262                 break;
 263         case SCST_CMD_QUEUE_HEAD_OF_QUEUE:
 264                 TRACE(TRACE_ORDER, "HQ cmd_h %d", vcmd->cmd->cmd_h);
 265                 break;
 266         default:
 267                 break;
 268         }
 269
 270         memset(reply, 0, sizeof(*reply));
 271         vcmd->reply->cmd_h = vcmd->cmd->cmd_h;
 272         vcmd->reply->subcode = vcmd->cmd->subcode;
 273         reply->reply_type = SCST_EXEC_REPLY_COMPLETED;
 274
 275 #ifdef DEBUG_SENSE
 276         if ((random() % 100000) == 75) {
 277                 set_cmd_error(vcmd, SCST_LOAD_SENSE(scst_sense_hardw_error));
 278                 goto out;
 279         }
 280 #endif
 281
 282 #ifdef DEBUG_TM_IGNORE
 283         if (dev->debug_tm_ignore && (random() % 10000) == 75) {
 284                 TRACE_MGMT_DBG("Ignore cmd op %x (h=%d)", cdb[0],
 285                         vcmd->cmd->cmd_h);
 286                 res = 150;
 287                 goto out;
 288         }
 289 #endif
 290
 291         if ((cmd->pbuf == 0) && (cmd->alloc_len != 0)) {
 292 #ifdef DEBUG_NOMEM
 293                 if ((random() % 100) == 75)
 294                         cmd->pbuf = 0;
 295                 else
 296 #endif
 297                         cmd->pbuf = (unsigned long)dev->alloc_fn(cmd->alloc_len);
 298                 TRACE_MEM("Buf %"PRIx64" alloced, len %d", cmd->pbuf,
 299                         cmd->alloc_len);
 300                 reply->pbuf = cmd->pbuf;
 301                 if (cmd->pbuf == 0) {
 302                         TRACE(TRACE_OUT_OF_MEM, "Unable to allocate buffer "
 303                                 "(len %d)", cmd->alloc_len);
 304 #ifndef DEBUG_NOMEM
 305                         set_busy(reply);
 306 #endif
 307                         goto out;
 308                 }
 309         }
 310
 311         switch (opcode) {
 312         case READ_6:
 313         case WRITE_6:
 314         case VERIFY_6:
 315                 lba_start = (((cdb[1] & 0x1f) << (BYTE * 2)) +
 316                              (cdb[2] << (BYTE * 1)) +
 317                              (cdb[3] << (BYTE * 0)));
 318                 data_len = cmd->bufflen;
 319                 break;
 320         case READ_10:
 321         case READ_12:
 322         case WRITE_10:
 323         case WRITE_12:
 324         case VERIFY:
 325         case WRITE_VERIFY:
 326         case WRITE_VERIFY_12:
 327         case VERIFY_12:
 328                 lba_start |= ((uint64_t)cdb[2]) << 24;
 329                 lba_start |= ((uint64_t)cdb[3]) << 16;
 330                 lba_start |= ((uint64_t)cdb[4]) << 8;
 331                 lba_start |= ((uint64_t)cdb[5]);
 332                 data_len = cmd->bufflen;
 333                 break;
 334         case SYNCHRONIZE_CACHE:
 335                 lba_start |= ((uint64_t)cdb[2]) << 24;
 336                 lba_start |= ((uint64_t)cdb[3]) << 16;
 337                 lba_start |= ((uint64_t)cdb[4]) << 8;
 338                 lba_start |= ((uint64_t)cdb[5]);
 339                 data_len = ((cdb[7] << (BYTE * 1)) + (cdb[8] << (BYTE * 0)))
 340                                 << dev->block_shift;
 341                 if (data_len == 0)
 342                         data_len = dev->file_size -
 343                                 ((loff_t)lba_start << dev->block_shift);
 344                 break;
 345         case READ_16:
 346         case WRITE_16:
 347         case WRITE_VERIFY_16:
 348         case VERIFY_16:
 349                 lba_start |= ((uint64_t)cdb[2]) << 56;
 350                 lba_start |= ((uint64_t)cdb[3]) << 48;
 351                 lba_start |= ((uint64_t)cdb[4]) << 40;
 352                 lba_start |= ((uint64_t)cdb[5]) << 32;
 353                 lba_start |= ((uint64_t)cdb[6]) << 16;
 354                 lba_start |= ((uint64_t)cdb[7]) << 8;
 355                 lba_start |= ((uint64_t)cdb[8]);
 356                 data_len = cmd->bufflen;
 357                 break;
 358         }
 359
 360         loff = (loff_t)lba_start << dev->block_shift;
 361         TRACE_DBG("cmd %d, buf %"PRIx64", lba_start %"PRId64", loff %"PRId64
 362                 ", data_len %"PRId64, vcmd->cmd->cmd_h, cmd->pbuf, lba_start,
 363                 (uint64_t)loff, (uint64_t)data_len);
 364         if ((loff < 0) || (data_len < 0) || ((loff + data_len) > dev->file_size)) {
 365                 PRINT_INFO("Access beyond the end of the device "
 366                         "(%"PRId64" of %"PRId64", len %"PRId64")", (uint64_t)loff,
 367                         (uint64_t)dev->file_size, (uint64_t)data_len);
 368                 set_cmd_error(vcmd, SCST_LOAD_SENSE(
 369                                 scst_sense_block_out_range_error));
 370                 goto out;
 371         }
 372
 373         switch (opcode) {
 374         case WRITE_10:
 375         case WRITE_12:
 376         case WRITE_16:
 377                 fua = (cdb[1] & 0x8);
 378                 if (cdb[1] & 0x8) {
 379                         TRACE(TRACE_ORDER, "FUA(%d): loff=%"PRId64", "
 380                                 "data_len=%"PRId64, fua, (uint64_t)loff,
 381                                 (uint64_t)data_len);
 382                 }
 383                 break;
 384         }
 385
 386         switch (opcode) {
 387         case READ_6:
 388         case READ_10:
 389         case READ_12:
 390         case READ_16:
 391                 exec_read(vcmd, loff);
 392                 break;
 393         case WRITE_6:
 394         case WRITE_10:
 395         case WRITE_12:
 396         case WRITE_16:
 397                 if (!dev->rd_only_flag) {
 398                         int do_fsync = sync_queue_type(cmd->queue_type);
 399                         struct vdisk_tgt_dev *tgt_dev;
 400                         enum scst_cmd_queue_type last_queue_type;
 401
 402                         tgt_dev = find_tgt_dev(dev, cmd->sess_h);
 403                         if (tgt_dev == NULL) {
 404                                 PRINT_ERROR("Session %"PRIx64" not found",
 405                                         cmd->sess_h);
 406                                 set_cmd_error(vcmd,
 407                                     SCST_LOAD_SENSE(scst_sense_hardw_error));
 408                                 goto out;
 409                         }
 410
 411                         last_queue_type = tgt_dev->last_write_cmd_queue_type;
 412                         tgt_dev->last_write_cmd_queue_type = cmd->queue_type;
 413                         if (need_pre_sync(cmd->queue_type, last_queue_type)) {
 414                                 TRACE(TRACE_ORDER, "ORDERED "
 415                                         "WRITE(%d): loff=%"PRId64", data_len=%"
 416                                         PRId64, cmd->queue_type, (uint64_t)loff,
 417                                         (uint64_t)data_len);
 418                                 do_fsync = 1;
 419                                 if (exec_fsync(vcmd) != 0)
 420                                         goto out;
 421                         }
 422                         exec_write(vcmd, loff);
 423                         /* O_SYNC flag is used for WT devices */
 424                         if (do_fsync || fua)
 425                                 exec_fsync(vcmd);
 426                 } else {
 427                         TRACE(TRACE_MINOR, "Attempt to write to read-only "
 428                                 "device %s", dev->name);
 429                         set_cmd_error(vcmd,
 430                                 SCST_LOAD_SENSE(scst_sense_data_protect));
 431                 }
 432                 break;
 433         case WRITE_VERIFY:
 434         case WRITE_VERIFY_12:
 435         case WRITE_VERIFY_16:
 436                 if (!dev->rd_only_flag) {
 437                         int do_fsync = sync_queue_type(cmd->queue_type);
 438                         struct vdisk_tgt_dev *tgt_dev;
 439                         enum scst_cmd_queue_type last_queue_type;
 440
 441                         tgt_dev = find_tgt_dev(dev, cmd->sess_h);
 442                         if (tgt_dev == NULL) {
 443                                 PRINT_ERROR("Session %"PRIx64" not found",
 444                                         cmd->sess_h);
 445                                 set_cmd_error(vcmd,
 446                                     SCST_LOAD_SENSE(scst_sense_hardw_error));
 447                                 goto out;
 448                         }
 449
 450                         last_queue_type = tgt_dev->last_write_cmd_queue_type;
 451                         tgt_dev->last_write_cmd_queue_type = cmd->queue_type;
 452                         if (need_pre_sync(cmd->queue_type, last_queue_type)) {
 453                                 TRACE(TRACE_ORDER, "ORDERED "
 454                                         "WRITE_VERIFY(%d): loff=%"PRId64", "
 455                                         "data_len=%"PRId64, cmd->queue_type,
 456                                         (uint64_t)loff, (uint64_t)data_len);
 457                                 do_fsync = 1;
 458                                 if (exec_fsync(vcmd) != 0)
 459                                         goto out;
 460                         }
 461                         exec_write(vcmd, loff);
 462                         /* O_SYNC flag is used for WT devices */
 463                         if (reply->status == 0)
 464                                 exec_verify(vcmd, loff);
 465                         else if (do_fsync)
 466                                 exec_fsync(vcmd);
 467                 } else {
 468                         TRACE(TRACE_MINOR, "Attempt to write to read-only "
 469                                 "device %s", dev->name);
 470                         set_cmd_error(vcmd,
 471                                 SCST_LOAD_SENSE(scst_sense_data_protect));
 472                 }
 473                 break;
 474         case SYNCHRONIZE_CACHE:
 475         {
 476                 int immed = cdb[1] & 0x2;
 477                 TRACE(TRACE_ORDER, "SYNCHRONIZE_CACHE: "
 478                         "loff=%"PRId64", data_len=%"PRId64", immed=%d",
 479                         (uint64_t)loff, (uint64_t)data_len, immed);
 480                 if (immed) {
 481                         /* ToDo: backgroung exec */
 482                         exec_fsync(vcmd);
 483                         break;
 484                 } else {
 485                         exec_fsync(vcmd);
 486                         break;
 487                 }
 488         }
 489         case VERIFY_6:
 490         case VERIFY:
 491         case VERIFY_12:
 492         case VERIFY_16:
 493                 exec_verify(vcmd, loff);
 494                 break;
 495         case MODE_SENSE:
 496         case MODE_SENSE_10:
 497                 exec_mode_sense(vcmd);
 498                 break;
 499         case MODE_SELECT:
 500         case MODE_SELECT_10:
 501                 exec_mode_select(vcmd);
 502                 break;
 503         case ALLOW_MEDIUM_REMOVAL:
 504                 exec_prevent_allow_medium_removal(vcmd);
 505                 break;
 506         case READ_TOC:
 507                 exec_read_toc(vcmd);
 508                 break;
 509         case START_STOP:
 510                 exec_fsync(vcmd/*, 0, dev->file_size*/);
 511                 break;
 512         case RESERVE:
 513         case RESERVE_10:
 514         case RELEASE:
 515         case RELEASE_10:
 516         case TEST_UNIT_READY:
 517                 break;
 518         case INQUIRY:
 519                 exec_inquiry(vcmd);
 520                 break;
 521         case REQUEST_SENSE:
 522                 exec_request_sense(vcmd);
 523                 break;
 524         case READ_CAPACITY:
 525                 exec_read_capacity(vcmd);
 526                 break;
 527         case SERVICE_ACTION_IN:
 528                 if ((cmd->cdb[1] & 0x1f) == SAI_READ_CAPACITY_16) {
 529                         exec_read_capacity16(vcmd);
 530                         break;
 531                 }
 532                 /* else go through */
 533         case REPORT_LUNS:
 534         default:
 535                 TRACE_DBG("Invalid opcode %d", opcode);
 536                 set_cmd_error(vcmd, SCST_LOAD_SENSE(scst_sense_invalid_opcode));
 537                 break;
 538         }
 539
 540 out:
 541         TRACE_EXIT();
 542         return res;
 543 }
 544
 545 static int do_alloc_mem(struct vdisk_cmd *vcmd)
 546 {
 547         struct scst_user_get_cmd *cmd = vcmd->cmd;
 548         struct scst_user_reply_cmd *reply = vcmd->reply;
 549         int res = 0;
 550
 551         TRACE_ENTRY();
 552
 553         TRACE_MEM("Alloc mem (cmd %d, sess_h %"PRIx64", cdb_len %d, "
 554                 "alloc_len %d, queue_type %d, data_direction %d)", cmd->cmd_h,
 555                 cmd->alloc_cmd.sess_h, cmd->alloc_cmd.cdb_len,
 556                 cmd->alloc_cmd.alloc_len, cmd->alloc_cmd.queue_type,
 557                 cmd->alloc_cmd.data_direction);
 558
 559         TRACE_BUFF_FLAG(TRACE_MEMORY, "CDB", cmd->alloc_cmd.cdb,
 560                 cmd->alloc_cmd.cdb_len);
 561
 562         memset(reply, 0, sizeof(*reply));
 563         reply->cmd_h = cmd->cmd_h;
 564         reply->subcode = cmd->subcode;
 565 #ifdef DEBUG_NOMEM
 566         if ((random() % 100) == 75)
 567                 reply->alloc_reply.pbuf = 0;
 568         else
 569 #endif
 570                 reply->alloc_reply.pbuf = (unsigned long)vcmd->dev->alloc_fn(
 571                                                 cmd->alloc_cmd.alloc_len);
 572         TRACE_MEM("Buf %"PRIx64" alloced, len %d", reply->alloc_reply.pbuf,
 573                 cmd->alloc_cmd.alloc_len);
 574         if (reply->alloc_reply.pbuf == 0) {
 575                 TRACE(TRACE_OUT_OF_MEM, "Unable to allocate buffer (len %d)",
 576                         cmd->alloc_cmd.alloc_len);
 577         }
 578
 579         TRACE_EXIT_RES(res);
 580         return res;
 581 }
 582
 583 static int do_cached_mem_free(struct vdisk_cmd *vcmd)
 584 {
 585         struct scst_user_get_cmd *cmd = vcmd->cmd;
 586         struct scst_user_reply_cmd *reply = vcmd->reply;
 587         int res = 0;
 588
 589         TRACE_ENTRY();
 590
 591         TRACE_MEM("Cached mem free (cmd %d, buf %"PRIx64")", cmd->cmd_h,
 592                 cmd->on_cached_mem_free.pbuf);
 593
 594         free((void *)(unsigned long)cmd->on_cached_mem_free.pbuf);
 595
 596         memset(reply, 0, sizeof(*reply));
 597         reply->cmd_h = cmd->cmd_h;
 598         reply->subcode = cmd->subcode;
 599
 600         TRACE_EXIT_RES(res);
 601         return res;
 602 }
 603
 604 static int do_on_free_cmd(struct vdisk_cmd *vcmd)
 605 {
 606         struct scst_user_get_cmd *cmd = vcmd->cmd;
 607         struct scst_user_reply_cmd *reply = vcmd->reply;
 608         int res = 0;
 609
 610         TRACE_ENTRY();
 611
 612         TRACE_DBG("On free cmd (cmd %d, resp_data_len %d, aborted %d, "
 613                 "status %d, delivery_status %d)", cmd->cmd_h,
 614                 cmd->on_free_cmd.resp_data_len, cmd->on_free_cmd.aborted,
 615                 cmd->on_free_cmd.status, cmd->on_free_cmd.delivery_status);
 616
 617         TRACE_MEM("On free cmd (cmd %d, buf %"PRIx64", buffer_cached %d)",
 618                 cmd->cmd_h, cmd->on_free_cmd.pbuf,
 619                 cmd->on_free_cmd.buffer_cached);
 620
 621         if (!cmd->on_free_cmd.buffer_cached && (cmd->on_free_cmd.pbuf != 0)) {
 622                 TRACE_MEM("Freeing buf %"PRIx64, cmd->on_free_cmd.pbuf);
 623                 free((void *)(unsigned long)cmd->on_free_cmd.pbuf);
 624         }
 625
 626         memset(reply, 0, sizeof(*reply));
 627         reply->cmd_h = cmd->cmd_h;
 628         reply->subcode = cmd->subcode;
 629
 630         TRACE_EXIT_RES(res);
 631         return res;
 632 }
 633
 634 static int do_tm(struct vdisk_cmd *vcmd)
 635 {
 636         struct scst_user_get_cmd *cmd = vcmd->cmd;
 637         struct scst_user_reply_cmd *reply = vcmd->reply;
 638         int res = 0;
 639
 640         TRACE_ENTRY();
 641
 642         TRACE((cmd->tm_cmd.fn == SCST_ABORT_TASK) ? TRACE_MGMT_MINOR : TRACE_MGMT,
 643                 "TM fn %d (sess_h %"PRIx64", cmd_h_to_abort %d)", cmd->tm_cmd.fn,
 644                 cmd->tm_cmd.sess_h, cmd->tm_cmd.cmd_h_to_abort);
 645
 646         memset(reply, 0, sizeof(*reply));
 647         reply->cmd_h = cmd->cmd_h;
 648         reply->subcode = cmd->subcode;
 649         reply->result = 0;
 650
 651         TRACE_EXIT_RES(res);
 652         return res;
 653 }
 654
 655 static int do_sess(struct vdisk_cmd *vcmd)
 656 {
 657         struct scst_user_get_cmd *cmd = vcmd->cmd;
 658         struct scst_user_reply_cmd *reply = vcmd->reply;
 659         int res = 0;
 660         struct vdisk_tgt_dev *tgt_dev;
 661
 662         TRACE_ENTRY();
 663
 664         /*
 665          * We are guaranteed to have one and only one command at this point,
 666          * which is ATTACH_SESS/DETACH_SESS, so no protection is needed
 667          */
 668
 669         tgt_dev = find_tgt_dev(vcmd->dev, cmd->sess.sess_h);
 670
 671         if (cmd->subcode == SCST_USER_ATTACH_SESS) {
 672                 if (tgt_dev != NULL) {
 673                         PRINT_ERROR("Session %"PRIx64" already exists)",
 674                                 cmd->sess.sess_h);
 675                         res = EEXIST;
 676                         goto reply;
 677                 }
 678
 679                 tgt_dev = find_empty_tgt_dev(vcmd->dev);
 680                 if (tgt_dev == NULL) {
 681                         PRINT_ERROR("Too many initiators, session %"PRIx64
 682                                 " refused)", cmd->sess.sess_h);
 683                         res = ENOMEM;
 684                         goto reply;
 685                 }
 686
 687                 tgt_dev->sess_h = cmd->sess.sess_h;
 688                 tgt_dev->last_write_cmd_queue_type = SCST_CMD_QUEUE_SIMPLE;
 689
 690                 PRINT_INFO("Session from initiator %s (target %s) attached "
 691                         "(LUN %"PRIx64", threads_num %d, rd_only %d, sess_h "
 692                         "%"PRIx64")", cmd->sess.initiator_name,
 693                         cmd->sess.target_name, cmd->sess.lun,
 694                         cmd->sess.threads_num, cmd->sess.rd_only,
 695                         cmd->sess.sess_h);
 696         } else {
 697                 if (tgt_dev == NULL) {
 698                         PRINT_ERROR("Session %"PRIx64" not found)",
 699                                 cmd->sess.sess_h);
 700                         res = ESRCH;
 701                         goto reply;
 702                 }
 703                 tgt_dev->sess_h = 0;
 704                 PRINT_INFO("Session detached (sess_h %"PRIx64")",
 705                         cmd->sess.sess_h);
 706         }
 707
 708 reply:
 709         memset(reply, 0, sizeof(*reply));
 710         reply->cmd_h = cmd->cmd_h;
 711         reply->subcode = cmd->subcode;
 712         reply->result = res;
 713
 714         TRACE_EXIT_RES(res);
 715         return res;
 716 }
 717
 718 static int open_dev_fd(struct vdisk_dev *dev)
 719 {
 720         int res;
 721         int open_flags = O_LARGEFILE;
 722
 723         if (dev->rd_only_flag)
 724                 open_flags |= O_RDONLY;
 725         else
 726                 open_flags |= O_RDWR;
 727         if (dev->o_direct_flag)
 728                 open_flags |= O_DIRECT;
 729         if (dev->wt_flag)
 730                 open_flags |= O_SYNC;
 731
 732         TRACE_DBG("Opening file %s, flags 0x%x", dev->file_name, open_flags);
 733         res = open(dev->file_name, open_flags);
 734
 735         return res;
 736 }
 737
 738 void *main_loop(void *arg)
 739 {
 740         int res = 0;
 741         struct vdisk_dev *dev = (struct vdisk_dev *)arg;
 742         struct scst_user_get_cmd cmd;
 743         struct scst_user_reply_cmd reply;
 744         struct vdisk_cmd vcmd = { -1, &cmd, dev, &reply, {0}};
 745         int scst_usr_fd = dev->scst_usr_fd;
 746         struct pollfd pl;
 747
 748         TRACE_ENTRY();
 749
 750         vcmd.fd = open_dev_fd(dev);
 751         if (vcmd.fd < 0) {
 752                 res = -errno;
 753                 PRINT_ERROR("Unable to open file %s (%s)", dev->file_name,
 754                         strerror(-res));
 755                 goto out;
 756         }
 757
 758         memset(&pl, 0, sizeof(pl));
 759         pl.fd = scst_usr_fd;
 760         pl.events = POLLIN;
 761
 762         cmd.preply = 0;
 763
 764         while(1) {
 765 #ifdef DEBUG_TM_IGNORE_ALL
 766                 if (dev->debug_tm_ignore && (random() % 50000) == 55) {
 767                         TRACE_MGMT_DBG("%s", "Ignore ALL");
 768                         dev->debug_tm_ignore_all = 1;
 769                 }
 770                 if (dev->debug_tm_ignore_all) {
 771                         /* Go Astral */
 772                         while(1) {
 773                                 sleep(60);
 774                         }
 775                 }
 776 #endif
 777
 778                 res = ioctl(scst_usr_fd, SCST_USER_REPLY_AND_GET_CMD, &cmd);
 779                 if (res != 0) {
 780                         res = errno;
 781                         switch(res) {
 782                         case ESRCH:
 783                         case EBUSY:
 784                                 TRACE_MGMT_DBG("SCST_USER_REPLY_AND_GET_CMD returned "
 785                                         "%d (%s)", res, strerror(res));
 786                                 cmd.preply = 0;
 787                         case EINTR:
 788                                 continue;
 789                         case EAGAIN:
 790                                 TRACE_DBG("SCST_USER_REPLY_AND_GET_CMD returned "
 791                                         "EAGAIN (%d)", res);
 792                                 cmd.preply = 0;
 793                                 if (dev->non_blocking)
 794                                         break;
 795                                 else
 796                                         continue;
 797                         default:
 798                                 PRINT_ERROR("SCST_USER_REPLY_AND_GET_CMD failed: "
 799                                         "%s (%d)", strerror(res), res);
 800 #if 1
 801                                 continue;
 802 #else
 803                                 goto out_close;
 804 #endif
 805                         }
 806 again_poll:
 807                         res = poll(&pl, 1, 2000);
 808                         if (res > 0)
 809                                 continue;
 810                         else if (res == 0)
 811                                 goto again_poll;
 812                         else {
 813                                 res = errno;
 814                                 switch(res) {
 815                                 case ESRCH:
 816                                 case EBUSY:
 817                                 case EAGAIN:
 818                                         TRACE_MGMT_DBG("poll() returned %d "
 819                                                 "(%s)", res, strerror(res));
 820                                 case EINTR:
 821                                         goto again_poll;
 822                                 default:
 823                                         PRINT_ERROR("poll() failed: %s", strerror(res));
 824 #if 1
 825                                         goto again_poll;
 826 #else
 827                                         goto out_close;
 828 #endif
 829                                 }
 830                         }
 831                 }
 832
 833                 TRACE_BUFFER("Received cmd", &cmd, sizeof(cmd));
 834
 835                 switch(cmd.subcode) {
 836                 case SCST_USER_EXEC:
 837                         if (cmd.exec_cmd.data_direction == SCST_DATA_WRITE) {
 838                                 TRACE_BUFFER("Received cmd data",
 839                                         (void *)(unsigned long)cmd.exec_cmd.pbuf,
 840                                         cmd.exec_cmd.bufflen);
 841                         }
 842                         res = do_exec(&vcmd);
 843 #ifdef DEBUG_TM_IGNORE
 844                         if (res == 150) {
 845                                 cmd.preply = 0;
 846                                 continue;
 847                         }
 848 #endif
 849                         if (reply.exec_reply.resp_data_len != 0) {
 850                                 TRACE_BUFFER("Reply data",
 851                                         (void *)(unsigned long)reply.exec_reply.pbuf,
 852                                         reply.exec_reply.resp_data_len);
 853                         }
 854                         break;
 855
 856                 case SCST_USER_ALLOC_MEM:
 857                         res = do_alloc_mem(&vcmd);
 858                         break;
 859
 860                 case SCST_USER_PARSE:
 861                         res = do_parse(&vcmd);
 862                         break;
 863
 864                 case SCST_USER_ON_CACHED_MEM_FREE:
 865                         res = do_cached_mem_free(&vcmd);
 866                         break;
 867
 868                 case SCST_USER_ON_FREE_CMD:
 869                         res = do_on_free_cmd(&vcmd);
 870                         break;
 871
 872                 case SCST_USER_TASK_MGMT:
 873                         res = do_tm(&vcmd);
 874 #if DEBUG_TM_FN_IGNORE
 875                         if (dev->debug_tm_ignore) {
 876                                 sleep(15);
 877                         }
 878 #endif
 879                         break;
 880
 881                 case SCST_USER_ATTACH_SESS:
 882                 case SCST_USER_DETACH_SESS:
 883                         res = do_sess(&vcmd);
 884                         break;
 885
 886                 default:
 887                         PRINT_ERROR("Unknown or wrong cmd subcode %x",
 888                                 cmd.subcode);
 889                         goto out_close;
 890                 }
 891
 892                 if (res != 0)
 893                         goto out_close;
 894
 895                 cmd.preply = (unsigned long)&reply;
 896                 TRACE_BUFFER("Sending reply", &reply, sizeof(reply));
 897         }
 898
 899 out_close:
 900         close(vcmd.fd);
 901
 902 out:
 903         PRINT_INFO("Thread %d exiting (res=%d)", gettid(), res);
 904
 905         TRACE_EXIT_RES(res);
 906         return (void *)(long)res;
 907 }
 908
 909 uint64_t gen_dev_id_num(const struct vdisk_dev *dev)
 910 {
 911         unsigned int dev_id_num, i;
 912
 913         for (dev_id_num = 0, i = 0; i < (int)strlen(dev->name); i++) {
 914                 unsigned int rv = random_values[(int)(dev->name[i])];
 915                 /* do some rotating of the bits */
 916                 dev_id_num ^= ((rv << i) | (rv >> (32 - i)));
 917         }
 918
 919         return ((uint64_t)vdisk_ID << 32) | dev_id_num;
 920 }
 921
 922 static void exec_inquiry(struct vdisk_cmd *vcmd)
 923 {
 924         struct vdisk_dev *dev = vcmd->dev;
 925         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
 926         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
 927         int resp_len = 0;
 928         int length = cmd->bufflen;
 929         unsigned int i;
 930         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
 931         uint8_t buf[INQ_BUF_SZ];
 932
 933         TRACE_ENTRY();
 934
 935         if (cmd->cdb[1] & CMDDT) {
 936                 TRACE_DBG("%s", "INQUIRY: CMDDT is unsupported");
 937                 set_cmd_error(vcmd,
 938                     SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
 939                 goto out;
 940         }
 941
 942         memset(buf, 0, sizeof(buf));
 943         buf[0] = dev->type;      /* type dev */
 944         if (buf[0] == TYPE_ROM)
 945                 buf[1] = 0x80;      /* removable */
 946         /* Vital Product */
 947         if (cmd->cdb[1] & EVPD) {
 948                 uint64_t dev_id_num;
 949                 int dev_id_len;
 950                 char dev_id_str[17];
 951
 952                 dev_id_num  = gen_dev_id_num(dev);
 953                 dev_id_len = snprintf(dev_id_str, sizeof(dev_id_str), "%llx",
 954                                         dev_id_num);
 955                 if (dev_id_len >= (signed)sizeof(dev_id_str))
 956                         dev_id_len = sizeof(dev_id_str) - 1;
 957                 TRACE_DBG("dev_id num %llx, str %s, len %d", dev_id_num,
 958                         dev_id_str, dev_id_len);
 959                 if (0 == cmd->cdb[2]) { /* supported vital product data pages */
 960                         buf[3] = 3;
 961                         buf[4] = 0x0; /* this page */
 962                         buf[5] = 0x80; /* unit serial number */
 963                         buf[6] = 0x83; /* device identification */
 964                         resp_len = buf[3] + 4;
 965                 } else if (0x80 == cmd->cdb[2]) { /* unit serial number */
 966                         int usn_len = strlen(dev->usn);
 967                         buf[1] = 0x80;
 968                         buf[3] = usn_len;
 969                         strncpy((char *)&buf[4], dev->usn, usn_len);
 970                         resp_len = buf[3] + 4;
 971                 } else if (0x83 == cmd->cdb[2]) { /* device identification */
 972                         int num = 4;
 973
 974                         buf[1] = 0x83;
 975                         /* Two identification descriptors: */
 976                         /* T10 vendor identifier field format (faked) */
 977                         buf[num + 0] = 0x2;     /* ASCII */
 978                         buf[num + 1] = 0x1;     /* Vendor ID */
 979                         memcpy(&buf[num + 4], VENDOR, 8);
 980                         i = strlen(dev->name) + 1; /* for ' ' */
 981                         memset(&buf[num + 12], ' ', i + dev_id_len);
 982                         memcpy(&buf[num + 12], dev->name, i-1);
 983                         memcpy(&buf[num + 12 + i], dev_id_str, dev_id_len);
 984                         buf[num + 3] = 8 + i + dev_id_len;
 985                         num += buf[num + 3];
 986
 987 #if 0 /* This isn't required and can be misleading, so let's disable it */
 988                         num += 4;
 989
 990                         /* NAA IEEE registered identifier (faked) */
 991                         buf[num] = 0x1; /* binary */
 992                         buf[num + 1] = 0x3;
 993                         buf[num + 2] = 0x0;
 994                         buf[num + 3] = 0x8;
 995                         buf[num + 4] = 0x51;    /* ieee company id=0x123456 (faked) */
 996                         buf[num + 5] = 0x23;
 997                         buf[num + 6] = 0x45;
 998                         buf[num + 7] = 0x60;
 999                         buf[num + 8] = (dev_id_num >> 24);
1000                         buf[num + 9] = (dev_id_num >> 16) & 0xff;
1001                         buf[num + 10] = (dev_id_num >> 8) & 0xff;
1002                         buf[num + 11] = dev_id_num & 0xff;
1003                         num = num + 12 - 4;
1004 #endif
1005
1006                         resp_len = num;
1007                         buf[2] = (resp_len >> 8) & 0xFF;
1008                         buf[3] = resp_len & 0xFF;
1009                         resp_len += 4;
1010                 } else {
1011                         TRACE_DBG("INQUIRY: Unsupported EVPD page %x",
1012                                 cmd->cdb[2]);
1013                         set_cmd_error(vcmd,
1014                             SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1015                         goto out;
1016                 }
1017         } else {
1018                 int len;
1019
1020                 if (cmd->cdb[2] != 0) {
1021                         TRACE_DBG("INQUIRY: Unsupported page %x", cmd->cdb[2]);
1022                         set_cmd_error(vcmd,
1023                             SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1024                         goto out;
1025                 }
1026
1027                 buf[2] = 4;     /* Device complies to this standard - SPC-2  */
1028                 buf[3] = 0x12;  /* HiSup + data in format specified in SPC-2 */
1029                 buf[4] = 31;/* n - 4 = 35 - 4 = 31 for full 36 byte data */
1030                 buf[6] = 1; /* MultiP 1 */
1031                 buf[7] = 2; /* CMDQUE 1, BQue 0 => commands queuing supported */
1032
1033                 /* 8 byte ASCII Vendor Identification of the target - left aligned */
1034                 memcpy(&buf[8], VENDOR, 8);
1035
1036                 /* 16 byte ASCII Product Identification of the target - left aligned */
1037                 memset(&buf[16], ' ', 16);
1038                 len = min(strlen(dev->name), (size_t)16);
1039                 memcpy(&buf[16], dev->name, len);
1040
1041                 /* 4 byte ASCII Product Revision Level of the target - left aligned */
1042                 memcpy(&buf[32], FIO_REV, 4);
1043                 resp_len = buf[4] + 5;
1044         }
1045
1046         sBUG_ON(resp_len >= (int)sizeof(buf));
1047         if (length > resp_len)
1048                 length = resp_len;
1049         memcpy(address, buf, length);
1050         reply->resp_data_len = length;
1051
1052 out:
1053         TRACE_EXIT();
1054         return;
1055 }
1056
1057 static void exec_request_sense(struct vdisk_cmd *vcmd)
1058 {
1059         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1060         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1061         int length = cmd->bufflen;
1062         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1063
1064         TRACE_ENTRY();
1065
1066         if (length < SCST_STANDARD_SENSE_LEN) {
1067                 PRINT_ERROR("too small requested buffer for REQUEST SENSE "
1068                         "(len %d)", length);
1069                 set_cmd_error(vcmd,
1070                     SCST_LOAD_SENSE(scst_sense_invalid_field_in_parm_list));
1071                 goto out;
1072         }
1073
1074         set_sense(address, length, SCST_LOAD_SENSE(scst_sense_no_sense));
1075         reply->resp_data_len = length;
1076
1077 out:
1078         TRACE_EXIT();
1079         return;
1080 }
1081
1082 /*
1083  * <<Following mode pages info copied from ST318451LW with some corrections>>
1084  *
1085  * ToDo: revise them
1086  */
1087
1088 static int err_recov_pg(unsigned char *p, int pcontrol)
1089 {       /* Read-Write Error Recovery page for mode_sense */
1090         const unsigned char err_recov_pg[] = {0x1, 0xa, 0xc0, 11, 240, 0, 0, 0,
1091                                               5, 0, 0xff, 0xff};
1092
1093         memcpy(p, err_recov_pg, sizeof(err_recov_pg));
1094         if (1 == pcontrol)
1095                 memset(p + 2, 0, sizeof(err_recov_pg) - 2);
1096         return sizeof(err_recov_pg);
1097 }
1098
1099 static int disconnect_pg(unsigned char *p, int pcontrol)
1100 {       /* Disconnect-Reconnect page for mode_sense */
1101         const unsigned char disconnect_pg[] = {0x2, 0xe, 128, 128, 0, 10, 0, 0,
1102                                                0, 0, 0, 0, 0, 0, 0, 0};
1103
1104         memcpy(p, disconnect_pg, sizeof(disconnect_pg));
1105         if (1 == pcontrol)
1106                 memset(p + 2, 0, sizeof(disconnect_pg) - 2);
1107         return sizeof(disconnect_pg);
1108 }
1109
1110 static int rigid_geo_pg(unsigned char *p, int pcontrol,
1111         struct vdisk_dev *dev)
1112 {
1113         unsigned char geo_m_pg[] = {0x04, 0x16, 0, 0, 0, DEF_HEADS, 0, 0,
1114                                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1115                                     0x3a, 0x98/* 15K RPM */, 0, 0};
1116         int32_t ncyl, n;
1117
1118         memcpy(p, geo_m_pg, sizeof(geo_m_pg));
1119         ncyl = dev->nblocks / (DEF_HEADS * DEF_SECTORS);
1120         if ((dev->nblocks % (DEF_HEADS * DEF_SECTORS)) != 0)
1121                 ncyl++;
1122         memcpy(&n, p + 2, sizeof(uint32_t));
1123         n = n | (htonl(ncyl) >> 8);
1124         memcpy(p + 2, &n, sizeof(uint32_t));
1125         if (1 == pcontrol)
1126                 memset(p + 2, 0, sizeof(geo_m_pg) - 2);
1127         return sizeof(geo_m_pg);
1128 }
1129
1130 static int format_pg(unsigned char *p, int pcontrol,
1131                              struct vdisk_dev *dev)
1132 {       /* Format device page for mode_sense */
1133         const unsigned char format_pg[] = {0x3, 0x16, 0, 0, 0, 0, 0, 0,
1134                                            0, 0, 0, 0, 0, 0, 0, 0,
1135                                            0, 0, 0, 0, 0x40, 0, 0, 0};
1136
1137         memcpy(p, format_pg, sizeof(format_pg));
1138         p[10] = (DEF_SECTORS >> 8) & 0xff;
1139         p[11] = DEF_SECTORS & 0xff;
1140         p[12] = (dev->block_size >> 8) & 0xff;
1141         p[13] = dev->block_size & 0xff;
1142         if (1 == pcontrol)
1143                 memset(p + 2, 0, sizeof(format_pg) - 2);
1144         return sizeof(format_pg);
1145 }
1146
1147 static int caching_pg(unsigned char *p, int pcontrol,
1148                              struct vdisk_dev *dev)
1149 {       /* Caching page for mode_sense */
1150         const unsigned char caching_pg[] = {0x8, 18, 0x10, 0, 0xff, 0xff, 0, 0,
1151                 0xff, 0xff, 0xff, 0xff, 0x80, 0x14, 0, 0, 0, 0, 0, 0};
1152
1153         memcpy(p, caching_pg, sizeof(caching_pg));
1154         p[2] |= !(dev->wt_flag) ? WCE : 0;
1155         if (1 == pcontrol)
1156                 memset(p + 2, 0, sizeof(caching_pg) - 2);
1157         return sizeof(caching_pg);
1158 }
1159
1160 static int ctrl_m_pg(unsigned char *p, int pcontrol,
1161                             struct vdisk_dev *dev)
1162 {       /* Control mode page for mode_sense */
1163         const unsigned char ctrl_m_pg[] = {0xa, 0xa, 0x20, 0, 0, 0x40, 0, 0,
1164                                            0, 0, 0x2, 0x4b};
1165
1166         memcpy(p, ctrl_m_pg, sizeof(ctrl_m_pg));
1167         if (!dev->wt_flag && !dev->nv_cache)
1168                 p[3] |= 0x10; /* Enable unrestricted reordering */
1169         if (1 == pcontrol)
1170                 memset(p + 2, 0, sizeof(ctrl_m_pg) - 2);
1171         return sizeof(ctrl_m_pg);
1172 }
1173
1174 static int iec_m_pg(unsigned char *p, int pcontrol)
1175 {       /* Informational Exceptions control mode page for mode_sense */
1176         const unsigned char iec_m_pg[] = {0x1c, 0xa, 0x08, 0, 0, 0, 0, 0,
1177                                           0, 0, 0x0, 0x0};
1178         memcpy(p, iec_m_pg, sizeof(iec_m_pg));
1179         if (1 == pcontrol)
1180                 memset(p + 2, 0, sizeof(iec_m_pg) - 2);
1181         return sizeof(iec_m_pg);
1182 }
1183
1184 static void exec_mode_sense(struct vdisk_cmd *vcmd)
1185 {
1186         struct vdisk_dev *dev = vcmd->dev;
1187         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1188         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1189         int length = cmd->bufflen;
1190         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1191         uint8_t buf[MSENSE_BUF_SZ];
1192         int blocksize;
1193         uint64_t nblocks;
1194         unsigned char dbd, type;
1195         int pcontrol, pcode, subpcode;
1196         unsigned char dev_spec;
1197         int msense_6, offset = 0, len;
1198         unsigned char *bp;
1199
1200         TRACE_ENTRY();
1201
1202         blocksize = dev->block_size;
1203         nblocks = dev->nblocks;
1204
1205         type = dev->type;    /* type dev */
1206         dbd = cmd->cdb[1] & DBD;
1207         pcontrol = (cmd->cdb[2] & 0xc0) >> 6;
1208         pcode = cmd->cdb[2] & 0x3f;
1209         subpcode = cmd->cdb[3];
1210         msense_6 = (MODE_SENSE == cmd->cdb[0]);
1211         dev_spec = (dev->rd_only_flag ? WP : 0) | DPOFUA;
1212
1213         memset(buf, 0, sizeof(buf));
1214
1215         if (0x3 == pcontrol) {
1216                 TRACE_DBG("%s", "MODE SENSE: Saving values not supported");
1217                 set_cmd_error(vcmd,
1218                     SCST_LOAD_SENSE(scst_sense_saving_params_unsup));
1219                 goto out;
1220         }
1221
1222         if (msense_6) {
1223                 buf[1] = type;
1224                 buf[2] = dev_spec;
1225                 offset = 4;
1226         } else {
1227                 buf[2] = type;
1228                 buf[3] = dev_spec;
1229                 offset = 8;
1230         }
1231
1232         if (0 != subpcode) { /* TODO: Control Extension page */
1233                 TRACE_DBG("%s", "MODE SENSE: Only subpage 0 is supported");
1234                 set_cmd_error(vcmd,
1235                     SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1236                 goto out;
1237         }
1238
1239         if (!dbd) {
1240                 /* Create block descriptor */
1241                 buf[offset - 1] = 0x08;         /* block descriptor length */
1242                 if (nblocks >> 32) {
1243                         buf[offset + 0] = 0xFF;
1244                         buf[offset + 1] = 0xFF;
1245                         buf[offset + 2] = 0xFF;
1246                         buf[offset + 3] = 0xFF;
1247                 } else {
1248                         buf[offset + 0] = (nblocks >> (BYTE * 3)) & 0xFF;/* num blks */
1249                         buf[offset + 1] = (nblocks >> (BYTE * 2)) & 0xFF;
1250                         buf[offset + 2] = (nblocks >> (BYTE * 1)) & 0xFF;
1251                         buf[offset + 3] = (nblocks >> (BYTE * 0)) & 0xFF;
1252                 }
1253                 buf[offset + 4] = 0;                    /* density code */
1254                 buf[offset + 5] = (blocksize >> (BYTE * 2)) & 0xFF;/* blklen */
1255                 buf[offset + 6] = (blocksize >> (BYTE * 1)) & 0xFF;
1256                 buf[offset + 7] = (blocksize >> (BYTE * 0)) & 0xFF;
1257
1258                 offset += 8;                    /* increment offset */
1259         }
1260
1261         bp = buf + offset;
1262
1263         switch (pcode) {
1264         case 0x1:       /* Read-Write error recovery page, direct access */
1265                 len = err_recov_pg(bp, pcontrol);
1266                 break;
1267         case 0x2:       /* Disconnect-Reconnect page, all devices */
1268                 len = disconnect_pg(bp, pcontrol);
1269                 break;
1270         case 0x3:       /* Format device page, direct access */
1271                 len = format_pg(bp, pcontrol, dev);
1272                 break;
1273         case 0x4:       /* Rigid disk geometry */
1274                 len = rigid_geo_pg(bp, pcontrol, dev);
1275                 break;
1276         case 0x8:       /* Caching page, direct access */
1277                 len = caching_pg(bp, pcontrol, dev);
1278                 break;
1279         case 0xa:       /* Control Mode page, all devices */
1280                 len = ctrl_m_pg(bp, pcontrol, dev);
1281                 break;
1282         case 0x1c:      /* Informational Exceptions Mode page, all devices */
1283                 len = iec_m_pg(bp, pcontrol);
1284                 break;
1285         case 0x3f:      /* Read all Mode pages */
1286                 len = err_recov_pg(bp, pcontrol);
1287                 len += disconnect_pg(bp + len, pcontrol);
1288                 len += format_pg(bp + len, pcontrol, dev);
1289                 len += caching_pg(bp + len, pcontrol, dev);
1290                 len += ctrl_m_pg(bp + len, pcontrol, dev);
1291                 len += iec_m_pg(bp + len, pcontrol);
1292                 len += rigid_geo_pg(bp + len, pcontrol, dev);
1293                 break;
1294         default:
1295                 TRACE_DBG("MODE SENSE: Unsupported page %x", pcode);
1296                 set_cmd_error(vcmd,
1297                     SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1298                 goto out;
1299         }
1300
1301         offset += len;
1302
1303         if (msense_6)
1304                 buf[0] = offset - 1;
1305         else {
1306                 buf[0] = ((offset - 2) >> 8) & 0xff;
1307                 buf[1] = (offset - 2) & 0xff;
1308         }
1309
1310         sBUG_ON(offset >= (int)sizeof(buf));
1311         if (offset > length)
1312                 offset = length;
1313         memcpy(address, buf, offset);
1314         reply->resp_data_len = offset;
1315
1316 out:
1317         TRACE_EXIT();
1318         return;
1319 }
1320
1321 static int set_wt(struct vdisk_dev *dev, int wt)
1322 {
1323         int res = 0;
1324
1325         TRACE_ENTRY();
1326
1327         if ((dev->wt_flag == wt) || dev->nullio)
1328                 goto out;
1329
1330         pthread_mutex_lock(&dev->dev_mutex);
1331         dev->wt_flag = wt;
1332         pthread_mutex_unlock(&dev->dev_mutex);
1333
1334 out:
1335         TRACE_EXIT_RES(res);
1336         return res;
1337 }
1338
1339 static void exec_mode_select(struct vdisk_cmd *vcmd)
1340 {
1341         struct vdisk_dev *dev = vcmd->dev;
1342         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1343         int length = cmd->bufflen;
1344         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1345         int mselect_6, offset;
1346
1347         TRACE_ENTRY();
1348
1349         mselect_6 = (MODE_SELECT == cmd->cdb[0]);
1350
1351         if (!(cmd->cdb[1] & PF) || (cmd->cdb[1] & SP)) {
1352                 PRINT_ERROR("MODE SELECT: PF and/or SP are wrongly set "
1353                         "(cdb[1]=%x)", cmd->cdb[1]);
1354                 set_cmd_error(vcmd,
1355                     SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1356                 goto out;
1357         }
1358
1359         if (mselect_6) {
1360                 offset = 4;
1361         } else {
1362                 offset = 8;
1363         }
1364
1365         if (address[offset - 1] == 8) {
1366                 offset += 8;
1367         } else if (address[offset - 1] != 0) {
1368                 PRINT_ERROR("%s", "MODE SELECT: Wrong parameters list "
1369                         "lenght");
1370                 set_cmd_error(vcmd,
1371                     SCST_LOAD_SENSE(scst_sense_invalid_field_in_parm_list));
1372                 goto out;
1373         }
1374
1375         while (length > offset + 2) {
1376                 if (address[offset] & PS) {
1377                         PRINT_ERROR("%s", "MODE SELECT: Illegal PS bit");
1378                         set_cmd_error(vcmd, SCST_LOAD_SENSE(
1379                                 scst_sense_invalid_field_in_parm_list));
1380                         goto out;
1381                 }
1382                 if ((address[offset] & 0x3f) == 0x8) {  /* Caching page */
1383                         if (address[offset + 1] != 18) {
1384                                 PRINT_ERROR("%s", "MODE SELECT: Invalid "
1385                                         "caching page request");
1386                                 set_cmd_error(vcmd, SCST_LOAD_SENSE(
1387                                         scst_sense_invalid_field_in_parm_list));
1388                                 goto out;
1389                         }
1390                         if (set_wt(dev,
1391                               (address[offset + 2] & WCE) ? 0 : 1) != 0) {
1392                                 set_cmd_error(vcmd,
1393                                     SCST_LOAD_SENSE(scst_sense_hardw_error));
1394                                 goto out;
1395                         }
1396                         break;
1397                 }
1398                 offset += address[offset + 1];
1399         }
1400
1401 out:
1402         TRACE_EXIT();
1403         return;
1404 }
1405
1406 static void exec_read_capacity(struct vdisk_cmd *vcmd)
1407 {
1408         struct vdisk_dev *dev = vcmd->dev;
1409         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1410         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1411         int length = cmd->bufflen;
1412         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1413         uint32_t blocksize;
1414         uint64_t nblocks;
1415         uint8_t buffer[READ_CAP_LEN];
1416
1417         TRACE_ENTRY();
1418
1419         blocksize = dev->block_size;
1420         nblocks = dev->nblocks;
1421
1422         /* last block on the dev is (nblocks-1) */
1423         memset(buffer, 0, sizeof(buffer));
1424         if (nblocks >> 32) {
1425                 buffer[0] = 0xFF;
1426                 buffer[1] = 0xFF;
1427                 buffer[2] = 0xFF;
1428                 buffer[3] = 0xFF;
1429         } else {
1430                 buffer[0] = ((nblocks - 1) >> (BYTE * 3)) & 0xFF;
1431                 buffer[1] = ((nblocks - 1) >> (BYTE * 2)) & 0xFF;
1432                 buffer[2] = ((nblocks - 1) >> (BYTE * 1)) & 0xFF;
1433                 buffer[3] = ((nblocks - 1) >> (BYTE * 0)) & 0xFF;
1434         }
1435         buffer[4] = (blocksize >> (BYTE * 3)) & 0xFF;
1436         buffer[5] = (blocksize >> (BYTE * 2)) & 0xFF;
1437         buffer[6] = (blocksize >> (BYTE * 1)) & 0xFF;
1438         buffer[7] = (blocksize >> (BYTE * 0)) & 0xFF;
1439
1440         if (length > READ_CAP_LEN)
1441                 length = READ_CAP_LEN;
1442         memcpy(address, buffer, length);
1443
1444         reply->resp_data_len = length;
1445
1446         TRACE_EXIT();
1447         return;
1448 }
1449
1450 static void exec_read_capacity16(struct vdisk_cmd *vcmd)
1451 {
1452         struct vdisk_dev *dev = vcmd->dev;
1453         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1454         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1455         int length = cmd->bufflen;
1456         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1457         uint32_t blocksize;
1458         uint64_t nblocks;
1459         uint8_t buffer[READ_CAP16_LEN];
1460
1461         TRACE_ENTRY();
1462
1463         blocksize = dev->block_size;
1464         nblocks = dev->nblocks - 1;
1465
1466         memset(buffer, 0, sizeof(buffer));
1467         buffer[0] = nblocks >> 56;
1468         buffer[1] = (nblocks >> 48) & 0xFF;
1469         buffer[2] = (nblocks >> 40) & 0xFF;
1470         buffer[3] = (nblocks >> 32) & 0xFF;
1471         buffer[4] = (nblocks >> 24) & 0xFF;
1472         buffer[5] = (nblocks >> 16) & 0xFF;
1473         buffer[6] = (nblocks >> 8) & 0xFF;
1474         buffer[7] = nblocks& 0xFF;
1475
1476         buffer[8] = (blocksize >> (BYTE * 3)) & 0xFF;
1477         buffer[9] = (blocksize >> (BYTE * 2)) & 0xFF;
1478         buffer[10] = (blocksize >> (BYTE * 1)) & 0xFF;
1479         buffer[11] = (blocksize >> (BYTE * 0)) & 0xFF;
1480
1481         if (length > READ_CAP16_LEN)
1482                 length = READ_CAP16_LEN;
1483         memcpy(address, buffer, length);
1484
1485         reply->resp_data_len = length;
1486
1487         TRACE_EXIT();
1488         return;
1489 }
1490
1491 static void exec_read_toc(struct vdisk_cmd *vcmd)
1492 {
1493         struct vdisk_dev *dev = vcmd->dev;
1494         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1495         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1496         int32_t off = 0;
1497         int length = cmd->bufflen;
1498         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1499         uint32_t nblocks;
1500         uint8_t buffer[4+8+8] = { 0x00, 0x0a, 0x01, 0x01, 0x00, 0x14,
1501                                   0x01, 0x00, 0x00, 0x00, 0x00, 0x00 };
1502
1503         TRACE_ENTRY();
1504
1505         if (dev->type != TYPE_ROM) {
1506                 PRINT_ERROR("%s", "READ TOC for non-CDROM device");
1507                 set_cmd_error(vcmd,
1508                         SCST_LOAD_SENSE(scst_sense_invalid_opcode));
1509                 goto out;
1510         }
1511
1512         if (cmd->cdb[2] & 0x0e/*Format*/) {
1513                 PRINT_ERROR("%s", "READ TOC: invalid requested data format");
1514                 set_cmd_error(vcmd,
1515                         SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1516                 goto out;
1517         }
1518
1519         if ((cmd->cdb[6] != 0 && (cmd->cdb[2] & 0x01)) ||
1520             (cmd->cdb[6] > 1 && cmd->cdb[6] != 0xAA)) {
1521                 PRINT_ERROR("READ TOC: invalid requested track number %x",
1522                         cmd->cdb[6]);
1523                 set_cmd_error(vcmd,
1524                         SCST_LOAD_SENSE(scst_sense_invalid_field_in_cdb));
1525                 goto out;
1526         }
1527
1528         /* ToDo when you have > 8TB ROM device. */
1529         nblocks = (uint32_t)dev->nblocks;
1530
1531         /* Header */
1532         memset(buffer, 0, sizeof(buffer));
1533         buffer[2] = 0x01;    /* First Track/Session */
1534         buffer[3] = 0x01;    /* Last Track/Session */
1535         off = 4;
1536         if (cmd->cdb[6] <= 1)
1537         {
1538                 /* Fistr TOC Track Descriptor */
1539                 buffer[off+1] = 0x14; /* ADDR    0x10 - Q Sub-channel encodes current position data
1540                                          CONTROL 0x04 - Data track, recoreded uninterrupted */
1541                 buffer[off+2] = 0x01; /* Track Number */
1542                 off += 8;
1543         }
1544         if (!(cmd->cdb[2] & 0x01))
1545         {
1546                 /* Lead-out area TOC Track Descriptor */
1547                 buffer[off+1] = 0x14;
1548                 buffer[off+2] = 0xAA;     /* Track Number */
1549                 buffer[off+4] = (nblocks >> (BYTE * 3)) & 0xFF; /* Track Start Address */
1550                 buffer[off+5] = (nblocks >> (BYTE * 2)) & 0xFF;
1551                 buffer[off+6] = (nblocks >> (BYTE * 1)) & 0xFF;
1552                 buffer[off+7] = (nblocks >> (BYTE * 0)) & 0xFF;
1553                 off += 8;
1554         }
1555
1556         buffer[1] = off - 2;    /* Data  Length */
1557
1558         if (off > length)
1559                 off = length;
1560         memcpy(address, buffer, off);
1561         reply->resp_data_len = off;
1562
1563 out:
1564         TRACE_EXIT();
1565         return;
1566 }
1567
1568 static void exec_prevent_allow_medium_removal(struct vdisk_cmd *vcmd)
1569 {
1570         struct vdisk_dev *dev = vcmd->dev;
1571         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1572
1573         TRACE_DBG("PERSIST/PREVENT 0x%02x", cmd->cdb[4]);
1574
1575         pthread_mutex_lock(&dev->dev_mutex);
1576         if (dev->type == TYPE_ROM)
1577                 dev->prevent_allow_medium_removal =
1578                         cmd->cdb[4] & 0x01 ? 1 : 0;
1579         else {
1580                 PRINT_ERROR("%s", "Prevent allow medium removal for "
1581                         "non-CDROM device");
1582                 set_cmd_error(vcmd,
1583                         SCST_LOAD_SENSE(scst_sense_invalid_opcode));
1584         }
1585         pthread_mutex_unlock(&dev->dev_mutex);
1586
1587         return;
1588 }
1589
1590 static int exec_fsync(struct vdisk_cmd *vcmd)
1591 {
1592         int res = 0;
1593         struct vdisk_dev *dev = vcmd->dev;
1594
1595         /* Hopefully, the compiler will generate the single comparison */
1596         if (dev->nv_cache || dev->wt_flag || dev->rd_only_flag ||
1597             dev->o_direct_flag || dev->nullio)
1598                 goto out;
1599
1600         /* ToDo: use sync_file_range() instead */
1601         fsync(vcmd->fd);
1602
1603 out:
1604         TRACE_EXIT_RES(res);
1605         return res;
1606 }
1607
1608 static void exec_read(struct vdisk_cmd *vcmd, loff_t loff)
1609 {
1610         struct vdisk_dev *dev = vcmd->dev;
1611         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1612         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1613         int length = cmd->bufflen;
1614         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1615         int fd = vcmd->fd;
1616         loff_t err;
1617
1618         TRACE_ENTRY();
1619
1620         TRACE_DBG("reading off %"PRId64", len %d", loff, length);
1621         if (dev->nullio)
1622                 err = length;
1623         else {
1624                 /* SEEK */
1625                 err = lseek64(fd, loff, 0/*SEEK_SET*/);
1626                 if (err != loff) {
1627                         PRINT_ERROR("lseek trouble %"PRId64" != %"PRId64
1628                                 " (errno %d)", (uint64_t)err, (uint64_t)loff,
1629                                 errno);
1630                         set_cmd_error(vcmd, SCST_LOAD_SENSE(scst_sense_hardw_error));
1631                         goto out;
1632                 }
1633                 /* READ */
1634                 err = read(fd, address, length);
1635         }
1636
1637         if ((err < 0) || (err < length)) {
1638                 PRINT_ERROR("read() returned %"PRId64" from %d (errno %d)",
1639                         (uint64_t)err, length, errno);
1640                 if (err == -EAGAIN)
1641                         set_busy(reply);
1642                 else {
1643                         set_cmd_error(vcmd,
1644                             SCST_LOAD_SENSE(scst_sense_read_error));
1645                 }
1646                 goto out;
1647         }
1648
1649         reply->resp_data_len = cmd->bufflen;
1650
1651 out:
1652         TRACE_EXIT();
1653         return;
1654 }
1655
1656 static void exec_write(struct vdisk_cmd *vcmd, loff_t loff)
1657 {
1658         struct vdisk_dev *dev = vcmd->dev;
1659         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1660         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1661         loff_t err;
1662         int length = cmd->bufflen;
1663         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1664         int fd = vcmd->fd;
1665
1666         TRACE_ENTRY();
1667
1668 restart:
1669         TRACE_DBG("writing off %"PRId64", len %d", loff, length);
1670
1671         if (dev->nullio)
1672                 err = length;
1673         else {
1674                 /* SEEK */
1675                 err = lseek64(fd, loff, 0/*SEEK_SET*/);
1676                 if (err != loff) {
1677                         PRINT_ERROR("lseek trouble %"PRId64" != %"PRId64
1678                                 " (errno %d)", (uint64_t)err, (uint64_t)loff,
1679                                 errno);
1680                         set_cmd_error(vcmd,
1681                             SCST_LOAD_SENSE(scst_sense_hardw_error));
1682                         goto out;
1683                 }
1684
1685                 /* WRITE */
1686                 err = write(fd, address, length);
1687         }
1688
1689         if (err < 0) {
1690                 PRINT_ERROR("write() returned %"PRId64" from %d (errno %d, "
1691                         "cmd_h %x)", err, length, errno, vcmd->cmd->cmd_h);
1692                 if (err == -EAGAIN)
1693                         set_busy(reply);
1694                 else {
1695                         set_cmd_error(vcmd,
1696                             SCST_LOAD_SENSE(scst_sense_write_error));
1697                 }
1698                 goto out;
1699         } else if (err < length) {
1700                 /*
1701                  * Probably that's wrong, but sometimes write() returns
1702                  * value less, than requested. Let's restart.
1703                  */
1704                 TRACE_MGMT_DBG("write() returned %d from %d", (int)err, length);
1705                 if (err == 0) {
1706                         PRINT_INFO("Suspicious: write() returned 0 from "
1707                                 "%d", length);
1708                 }
1709                 length -= err;
1710                 goto restart;
1711         }
1712
1713 out:
1714         TRACE_EXIT();
1715         return;
1716 }
1717
1718 static void exec_verify(struct vdisk_cmd *vcmd, loff_t loff)
1719 {
1720         struct vdisk_dev *dev = vcmd->dev;
1721         struct scst_user_scsi_cmd_exec *cmd = &vcmd->cmd->exec_cmd;
1722         struct scst_user_scsi_cmd_reply_exec *reply = &vcmd->reply->exec_reply;
1723         loff_t err;
1724         int length = cmd->bufflen;
1725         uint8_t *address = (uint8_t*)(unsigned long)cmd->pbuf;
1726         int compare;
1727         int fd = vcmd->fd;
1728         uint8_t mem_verify[128*1024];
1729
1730         TRACE_ENTRY();
1731
1732         if (exec_fsync(vcmd) != 0)
1733                 goto out;
1734
1735         /*
1736          * Until the cache is cleared prior the verifying, there is not
1737          * much point in this code. ToDo.
1738          *
1739          * Nevertherless, this code is valuable if the data have not read
1740          * from the file/disk yet.
1741          */
1742
1743         /* SEEK */
1744         if (!dev->nullio) {
1745                 err = lseek64(fd, loff, 0/*SEEK_SET*/);
1746                 if (err != loff) {
1747                         PRINT_ERROR("lseek trouble %"PRId64" != %"PRId64
1748                                 " (errno %d)", (uint64_t)err,
1749                                 (uint64_t)loff, errno);
1750                         set_cmd_error(vcmd, SCST_LOAD_SENSE(scst_sense_hardw_error));
1751                         goto out;
1752                 }
1753         }
1754
1755         if ((length == 0) && (cmd->data_len != 0)) {
1756                 length = cmd->data_len;
1757                 compare = 0;
1758         } else
1759                 compare = 1;
1760
1761         while (length > 0) {
1762                 int len_mem = (length > (int)sizeof(mem_verify)) ?
1763                                         (int)sizeof(mem_verify) : length;
1764                 TRACE_DBG("Verify: length %d - len_mem %d", length, len_mem);
1765
1766                 if (!dev->nullio)
1767                         err = read(fd, (char *)mem_verify, len_mem);
1768                 else
1769                         err = len_mem;
1770                 if ((err < 0) || (err < len_mem)) {
1771                         PRINT_ERROR("read() returned %"PRId64" from %d "
1772                                 "(errno %d)", (uint64_t)err, len_mem, errno);
1773                         if (err == -EAGAIN)
1774                                 set_busy(reply);
1775                         else {
1776                                 set_cmd_error(vcmd,
1777                                     SCST_LOAD_SENSE(scst_sense_read_error));
1778                         }
1779                         goto out;
1780                 }
1781                 if (compare && memcmp(address, mem_verify, len_mem) != 0) {
1782                         TRACE_DBG("Verify: error memcmp length %d", length);
1783                         set_cmd_error(vcmd,
1784                             SCST_LOAD_SENSE(scst_sense_miscompare_error));
1785                         goto out;
1786                 }
1787                 length -= len_mem;
1788                 address += len_mem;
1789         }
1790
1791         if (length < 0) {
1792                 PRINT_ERROR("Failure: %d", length);
1793                 set_cmd_error(vcmd, SCST_LOAD_SENSE(scst_sense_hardw_error));
1794         }
1795
1796 out:
1797         TRACE_EXIT();
1798         return;
1799 }