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