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