diff --git a/src/GreenSCSI.ino b/src/GreenSCSI.ino
deleted file mode 100644
index a91457a..0000000
--- a/src/GreenSCSI.ino
+++ /dev/null
@@ -1,1440 +0,0 @@
-/*  
- *  GreenSCSI
- *  Copyright (c) 2021  David Kuder
- *  Copyright (c) 2021  Eric Helgeson, Androda
- *  
- *  This file is free software: you may copy, redistribute and/or modify it  
- *  under the terms of the GNU General Public License as published by the  
- *  Free Software Foundation, either version 2 of the License, or (at your  
- *  option) any later version.  
- *  
- *  This file is distributed in the hope that it will be useful, but  
- *  WITHOUT ANY WARRANTY; without even the implied warranty of  
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU  
- *  General Public License for more details.  
- *  
- *  You should have received a copy of the GNU General Public License  
- *  along with this program.  If not, see https://github.com/erichelgeson/bluescsi.  
- *  
- * This file incorporates work covered by the following copyright and  
- * permission notice:  
- *  
- *     Copyright (c) 2019 komatsu   
- *  
- *     Permission to use, copy, modify, and/or distribute this software  
- *     for any purpose with or without fee is hereby granted, provided  
- *     that the above copyright notice and this permission notice appear  
- *     in all copies.  
- *  
- *     THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL  
- *     WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED  
- *     WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE  
- *     AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR  
- *     CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS  
- *     OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,  
- *     NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN  
- *     CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.  
- */
-
-#include <Arduino.h> // For Platform.IO
-#include <SPI.h>
-#include <SdFat.h>
-#include "sdios.h"
-
-#define DEBUG            0      // 0:No debug information output
-                                // 1: Debug information output to USB Serial
-                                // 2: Debug information output to LOG.txt (slow)
-
-#define SCSI_SELECT      0      // 0 for STANDARD
-                                // 1 for SHARP X1turbo
-                                // 2 for NEC PC98
-#define READ_SPEED_OPTIMIZE  1 // Faster reads
-#define WRITE_SPEED_OPTIMIZE 1 // Speeding up writes
-#define USE_DB2ID_TABLE      1 // Use table to get ID from SEL-DB
-
-// SCSI config
-#define NUM_SCSIID  7          // Maximum number of supported SCSI-IDs (The minimum is 0)
-#define NUM_SCSILUN 7          // Maximum number of LUNs supported     (The minimum is 0)
-#define READ_PARITY_CHECK 0    // Perform read parity check (unverified)
-
-// HDD format
-#define MAX_BLOCKSIZE 4096     // Maximum BLOCK size
-
-// SDFAT
-SdFs sd;
-
-#if DEBUG == 1
-#define LOG(XX)     Serial.print(XX)
-#define LOGHEX(XX)  Serial.print(XX, HEX)
-#define LOGN(XX)    Serial.println(XX)
-#define LOGHEXN(XX) Serial.println(XX, HEX)
-#elif DEBUG == 2
-#define LOG(XX)     LOG_FILE.println(XX); LOG_FILE.sync();
-#define LOGHEX(XX)  LOG_FILE.println(XX, HEX); LOG_FILE.sync();
-#define LOGN(XX)    LOG_FILE.println(XX); LOG_FILE.sync();
-#define LOGHEXN(XX) LOG_FILE.println(XX, HEX); LOG_FILE.sync();
-#else
-#define LOG(XX)     //Serial.print(XX)
-#define LOGHEX(XX)  //Serial.print(XX, HEX)
-#define LOGN(XX)    //Serial.println(XX)
-#define LOGHEXN(XX) //Serial.println(XX, HEX)
-#endif
-
-#define DB0       0     // SCSI:DB0
-#define DB1       1     // SCSI:DB1
-#define DB2       29    // SCSI:DB2
-#define DB3       30    // SCSI:DB3
-#define DB4       43    // SCSI:DB4
-#define DB5       46    // SCSI:DB5
-#define DB6       44    // SCSI:DB6
-#define DB7       45    // SCSI:DB7
-#define DB8       32     // SCSI:DBP
-#define ATN       12    // SCSI:ATN
-#define BSY       10    // SCSI:BSY
-#define ACK       9     // SCSI:ACK
-#define RST       8     // SCSI:RST
-#define MSG       7     // SCSI:MSG
-#define SEL       6     // SCSI:SEL
-#define CD        5     // SCSI:C/D
-#define REQ       4     // SCSI:REQ
-#define IO        3     // SCSI:I/O
-
-#define SD_CS     BUILTIN_SDCARD      // SDCARD:CS
-#define LED       13     // LED
-
-// LED control
-#define LED_ON()       digitalWrite(LED, HIGH);
-#define LED_OFF()      digitalWrite(LED, LOW);
-
-// SCSI output pin control: opendrain active LOW (direct pin drive)
-inline void SCSI_OUT(int VPIN, boolean ACTIVE) {
-  if(ACTIVE) {
-    pinMode(VPIN, OUTPUT_OPENDRAIN);
-    digitalWrite(VPIN, LOW);
-  } else {
-    pinMode(VPIN, INPUT);
-  }
-};
-
-// SCSI input pin check (invert)
-inline boolean SCSI_IN(int VPIN) {
-  pinMode(VPIN, INPUT_PULLUP);
-  return !digitalRead(VPIN);
-};
-
-#define SCSI_DB_MASK 0x00ff0800
-
-// Put DB and DP in output mode
-inline void SCSI_DB_OUTPUT() {
-  GPIOB_PDOR = SCSI_DB_MASK;
-  GPIOB_PDDR = 0x00000000;
-}
-
-// Put DB and DP in input mode
-inline void SCSI_DB_INPUT()  {
-  GPIOB_PDDR = 0x00000000;
-}
-
-// Turn on the output only for BSY
-#define SCSI_BSY_ACTIVE()      { SCSI_OUT(BSY, true); }
-// BSY,REQ,MSG,CD,IO Turn on the output (no change required for OD)
-#define SCSI_TARGET_ACTIVE()   { }
-// BSY,REQ,MSG,CD,IO Turn off output, BSY is the last input
-#define SCSI_TARGET_INACTIVE() { SCSI_OUT(REQ, false); SCSI_OUT(MSG, false); SCSI_OUT(CD, false); SCSI_OUT(IO, false); SCSI_OUT(BSY, false); pinMode(BSY, INPUT_PULLUP); }
-
-// HDDiamge file
-#define HDIMG_ID_POS  2                 // Position to embed ID number
-#define HDIMG_LUN_POS 3                 // Position to embed LUN numbers
-#define HDIMG_BLK_POS 5                 // Position to embed block size numbers
-#define MAX_FILE_PATH 32                // Maximum file name length
-
-// HDD image
-typedef struct hddimg_struct
-{
-    FsFile      m_file;                 // File object
-    uint64_t    m_fileSize;             // File size
-    size_t      m_blocksize;            // SCSI BLOCK size
-}HDDIMG;
-HDDIMG  img[NUM_SCSIID][NUM_SCSILUN]; // Maximum number
-
-uint8_t       m_senseKey = 0;         // Sense key
-volatile bool m_isBusReset = false;   // Bus reset
-
-byte          scsi_id_mask;           // Mask list of responding SCSI IDs
-byte          m_id;                   // Currently responding SCSI-ID
-byte          m_lun;                  // Logical unit number currently responding
-byte          m_sts;                  // Status byte
-byte          m_msg;                  // Message bytes
-HDDIMG       *m_img;                  // HDD image for current SCSI-ID, LUN
-byte          m_buf[MAX_BLOCKSIZE+1]; // General purpose buffer + overrun fetch
-int           m_msc;
-byte          m_msb[256];             // Command storage bytes
-
-/*
- *  Data byte to GPIOB register setting value and parity table
-*/
-
-
-// Parity bit generation
-#define PTY(V)   ((1^((V)^((V)>>1)^((V)>>2)^((V)>>3)^((V)>>4)^((V)>>5)^((V)>>6)^((V)>>7)))&1)
-
-// Set DBP
-#define DBP(D)    (((uint32_t)(D)<<16)|(PTY(D)<<11))
-
-#define DBP8(D)   DBP(D),DBP(D+1),DBP(D+2),DBP(D+3),DBP(D+4),DBP(D+5),DBP(D+6),DBP(D+7)
-#define DBP32(D)  DBP8(D),DBP8(D+8),DBP8(D+16),DBP8(D+24)
-
-// BSRR register control value that simultaneously performs DB set, DP set, and REQ = H (inactrive)
-static const uint32_t db_bsrr[256]={
-  DBP32(0x00),DBP32(0x20),DBP32(0x40),DBP32(0x60),
-  DBP32(0x80),DBP32(0xA0),DBP32(0xC0),DBP32(0xE0)
-};
-// Parity bit acquisition
-#define PARITY(DB) ((db_bsrr[DB & 0xff] & 0x0800) >> 11)
-
-// Macro cleaning
-#undef DBP32
-#undef DBP8
-//#undef DBP
-//#undef PTY
-
-#if USE_DB2ID_TABLE
-/* DB to SCSI-ID translation table */
-static const byte db2scsiid[256]={
-  0xff,
-  0,
-  1,1,
-  2,2,2,2,
-  3,3,3,3,3,3,3,3,
-  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
-  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
-  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
-  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7
-};
-#endif
-
-// Log File
-#define VERSION "1.1-SNAPSHOT"
-#define LOG_FILENAME "LOG.txt"
-FsFile LOG_FILE;
-
-// SCSI Drive Vendor information
-byte SCSI_INFO_BUF[36] = {
-  0x00, //device type
-  0x00, //RMB = 0
-  0x01, //ISO, ECMA, ANSI version
-  0x01, //Response data format
-  35 - 4, //Additional data length
-  0, 0, //Reserve
-  0x00, //Support function
-  'Q', 'U', 'A', 'N', 'T', 'U', 'M', ' ', // vendor 8
-  'F', 'I', 'R', 'E', 'B', 'A', 'L', 'L', '1', ' ', ' ',' ', ' ', ' ', ' ', ' ', // product 16
-  '1', '.', '0', ' ' // version 4
-};
-
-void onFalseInit(void);
-void noSDCardFound(void);
-void onBusReset(void);
-void initFileLog(void);
-void finalizeFileLog(void);
-
-/*
- * IO read.
- */
-inline byte readIO(void)
-{
-  // Port input data register
-  uint32_t ret = GPIOB_PDIR;
-  byte bret = (byte)((~ret)>>16);
-#if READ_PARITY_CHECK
-  if((db_bsrr[bret]^ret)&0x800)
-    m_sts |= 0x01; // parity error
-#endif
-
-  return bret;
-}
-
-// If config file exists, read the first three lines and copy the contents.
-// File must be well formed or you will get junk in the SCSI Vendor fields.
-void readSCSIDeviceConfig() {
-  FsFile config_file = sd.open("scsi-config.txt", O_RDONLY);
-  if (!config_file.isOpen()) {
-    return;
-  }
-  char vendor[9];
-  memset(vendor, 0, sizeof(vendor));
-  config_file.readBytes(vendor, sizeof(vendor));
-  LOG_FILE.print("SCSI VENDOR: ");
-  LOG_FILE.println(vendor);
-  memcpy(&(SCSI_INFO_BUF[8]), vendor, 8);
-
-  char product[17];
-  memset(product, 0, sizeof(product));
-  config_file.readBytes(product, sizeof(product));
-  LOG_FILE.print("SCSI PRODUCT: ");
-  LOG_FILE.println(product);
-  memcpy(&(SCSI_INFO_BUF[16]), product, 16);
-
-  char version[5];
-  memset(version, 0, sizeof(version));
-  config_file.readBytes(version, sizeof(version));
-  LOG_FILE.print("SCSI VERSION: ");
-  LOG_FILE.println(version);
-  memcpy(&(SCSI_INFO_BUF[32]), version, 4);
-  config_file.close();
-}
-
-// read SD information and print to logfile
-void readSDCardInfo()
-{
-  cid_t sd_cid;
-
-  if(sd.card()->readCID(&sd_cid))
-  {
-    LOG_FILE.print("Sd MID:");
-    LOG_FILE.print(sd_cid.mid, 16);
-    LOG_FILE.print(" OID:");
-    LOG_FILE.print(sd_cid.oid[0]);
-    LOG_FILE.println(sd_cid.oid[1]);
-
-    LOG_FILE.print("Sd Name:");
-    LOG_FILE.print(sd_cid.pnm[0]);
-    LOG_FILE.print(sd_cid.pnm[1]);
-    LOG_FILE.print(sd_cid.pnm[2]);
-    LOG_FILE.print(sd_cid.pnm[3]);
-    LOG_FILE.println(sd_cid.pnm[4]);
-
-    LOG_FILE.print("Sd Date:");
-    LOG_FILE.print(sd_cid.mdt_month);
-    LOG_FILE.print("/20"); // CID year is 2000 + high/low
-    LOG_FILE.print(sd_cid.mdt_year_high);
-    LOG_FILE.println(sd_cid.mdt_year_low);
-
-    LOG_FILE.print("Sd Serial:");
-    LOG_FILE.println(sd_cid.psn);
-    LOG_FILE.sync();
-  }
-}
-
-/*
- * Open HDD image file
- */
-
-bool hddimageOpen(HDDIMG *h,const char *image_name,int id,int lun,int blocksize)
-{
-  h->m_fileSize = 0;
-  h->m_blocksize = blocksize;
-  h->m_file = sd.open(image_name, O_RDWR);
-  if(h->m_file.isOpen())
-  {
-    h->m_fileSize = h->m_file.size();
-    LOG_FILE.print("Imagefile: ");
-    LOG_FILE.print(image_name);
-    if(h->m_fileSize>0)
-    {
-      // check blocksize dummy file
-      LOG_FILE.print(" / ");
-      LOG_FILE.print(h->m_fileSize);
-      LOG_FILE.print("bytes / ");
-      LOG_FILE.print(h->m_fileSize / 1024);
-      LOG_FILE.print("KiB / ");
-      LOG_FILE.print(h->m_fileSize / 1024 / 1024);
-      LOG_FILE.println("MiB");
-      return true; // File opened
-    }
-    else
-    {
-      h->m_file.close();
-      h->m_fileSize = h->m_blocksize = 0; // no file
-      LOG_FILE.println("FileSizeError");
-    }
-  }
-  return false;
-}
-
-/*
- * Initialization.
- *  Initialize the bus and set the PIN orientation
- */
-void setup()
-{
-  // PIN initialization
-  pinMode(LED, OUTPUT);
-  LED_OFF();
-
-  //GPIO(SCSI BUS)Initialization
-
-  // Input port
-  pinMode(ATN, INPUT_PULLUP);
-  pinMode(BSY, INPUT_PULLUP);
-  pinMode(ACK, INPUT_PULLUP);
-  pinMode(RST, INPUT_PULLUP);
-  pinMode(SEL, INPUT_PULLUP);
-  // Output port
-  pinMode(MSG, OUTPUT_OPENDRAIN);
-  pinMode(CD,  OUTPUT_OPENDRAIN);
-  pinMode(REQ, OUTPUT_OPENDRAIN);
-  pinMode(IO,  OUTPUT_OPENDRAIN);
-
-  pinMode(DB0, OUTPUT_OPENDRAIN);
-  pinMode(DB1, OUTPUT_OPENDRAIN);
-  pinMode(DB2, OUTPUT_OPENDRAIN);
-  pinMode(DB3, OUTPUT_OPENDRAIN);
-  pinMode(DB4, OUTPUT_OPENDRAIN);
-  pinMode(DB5, OUTPUT_OPENDRAIN);
-  pinMode(DB6, OUTPUT_OPENDRAIN);
-  pinMode(DB7, OUTPUT_OPENDRAIN);
-  pinMode(DB8, OUTPUT_OPENDRAIN);
-
-  // DB and DP are input modes
-  SCSI_DB_INPUT();
-  
-  // Turn off the output port
-  SCSI_TARGET_INACTIVE();
-
-  LED_ON();
-
-  if(!sd.begin(SdioConfig(FIFO_SDIO))) {
-#if DEBUG > 0
-    Serial.println("SD initialization failed!");
-#endif
-    noSDCardFound();
-  }
-  initFileLog();
-  readSCSIDeviceConfig();
-  readSDCardInfo();
-
-  //Sector data overrun byte setting
-  m_buf[MAX_BLOCKSIZE] = 0xff; // DB0 all off,DBP off
-  //HD image file open
-  scsi_id_mask = 0x00;
-
-  // Iterate over the root path in the SD card looking for candidate image files.
-  SdFile root;
-  root.open("/");
-  SdFile file;
-  bool imageReady;
-  int usedDefaultId = 0;
-  while (1) {
-    if (!file.openNext(&root, O_READ)) break;
-    char name[MAX_FILE_PATH+1];
-    if(!file.isDir()) {
-      file.getName(name, MAX_FILE_PATH+1);
-      file.close();
-      String file_name = String(name);
-      file_name.toLowerCase();
-      if(file_name.startsWith("hd")) {
-        // Defaults for Hard Disks
-        int id  = 1; // 0 and 3 are common in Macs for physical HD and CD, so avoid them.
-        int lun = 0;
-        int blk = 512;
-
-        // Positionally read in and coerase the chars to integers.
-        // We only require the minimum and read in the next if provided.
-        int file_name_length = file_name.length();
-        if(file_name_length > 2) { // HD[N]
-          int tmp_id = name[HDIMG_ID_POS] - '0';
-
-          if(tmp_id > -1 && tmp_id < 8) {
-            id = tmp_id; // If valid id, set it, else use default
-          } else {
-            usedDefaultId++;
-          }
-        }
-        if(file_name_length > 3) { // HD0[N]
-          int tmp_lun = name[HDIMG_LUN_POS] - '0';
-
-          if(tmp_lun > -1 && tmp_lun < 2) {
-            lun = tmp_lun; // If valid id, set it, else use default
-          }
-        }
-        int blk1 = 0, blk2 = 0, blk3 = 0, blk4 = 0;
-        if(file_name_length > 8) { // HD00_[111]
-          blk1 = name[HDIMG_BLK_POS] - '0';
-          blk2 = name[HDIMG_BLK_POS+1] - '0';
-          blk3 = name[HDIMG_BLK_POS+2] - '0';
-          if(file_name_length > 9) // HD00_NNN[1]
-            blk4 = name[HDIMG_BLK_POS+3] - '0';
-        }
-        if(blk1 == 2 && blk2 == 5 && blk3 == 6) {
-          blk = 256;
-        } else if(blk1 == 1 && blk2 == 0 && blk3 == 2 && blk4 == 4) {
-          blk = 1024;
-        } else if(blk1 == 2 && blk2 == 0 && blk3 == 4 && blk4 == 8) {
-          blk  = 2048;
-        }
-
-        if(id < NUM_SCSIID && lun < NUM_SCSILUN) {
-          HDDIMG *h = &img[id][lun];
-          imageReady = hddimageOpen(h,name,id,lun,blk);
-          if(imageReady) { // Marked as a responsive ID
-            scsi_id_mask |= 1<<id;
-          }
-        } else {
-          LOG_FILE.print("Bad LUN or SCSI id for image: ");
-          LOG_FILE.println(name);
-          LOG_FILE.sync();
-        }
-      } else {
-          LOG_FILE.print("Not an image: ");
-          LOG_FILE.println(name);
-          LOG_FILE.sync();
-      }
-    }
-  }
-  if(usedDefaultId > 0) {
-    LOG_FILE.println("!! More than one image did not specify a SCSI ID. Last file will be used at ID 1. !!");
-    LOG_FILE.sync();
-  }
-  root.close();
-
-  // Error if there are 0 image files
-  if(scsi_id_mask==0) {
-    LOG_FILE.println("ERROR: No valid images found!");
-    onFalseInit();
-  }
-
-  finalizeFileLog();
-  LED_OFF();
-
-  //Occurs when the RST pin state changes from HIGH to LOW
-  attachInterrupt(RST, onBusReset, FALLING);
-}
-
-/*
- * Setup initialization logfile
- */
-void initFileLog() {
-  LOG_FILE = sd.open(LOG_FILENAME, O_WRONLY | O_CREAT | O_TRUNC);
-  LOG_FILE.println("GreenSCSI <-> SD - https://github.com/dkgrizzly/GreenSCSI");
-  LOG_FILE.print("VERSION: ");
-  LOG_FILE.println(VERSION);
-  LOG_FILE.print("DEBUG:");
-  LOG_FILE.print(DEBUG);
-  LOG_FILE.print(" SCSI_SELECT:");
-  LOG_FILE.print(SCSI_SELECT);
-  LOG_FILE.print(" SDFAT_FILE_TYPE:");
-  LOG_FILE.println(SDFAT_FILE_TYPE);
-  LOG_FILE.print("SdFat version: ");
-  LOG_FILE.println(SD_FAT_VERSION_STR);
-  LOG_FILE.print("SdFat Max FileName Length: ");
-  LOG_FILE.println(MAX_FILE_PATH);
-  LOG_FILE.println("Initialized SD Card - lets go!");
-  LOG_FILE.sync();
-}
-
-/*
- * Finalize initialization logfile
- */
-void finalizeFileLog() {
-  // View support drive map
-  LOG_FILE.print("ID");
-  for(int lun=0;lun<NUM_SCSILUN;lun++)
-  {
-    LOG_FILE.print(":LUN");
-    LOG_FILE.print(lun);
-  }
-  LOG_FILE.println(":");
-  //
-  for(int id=0;id<NUM_SCSIID;id++)
-  {
-    LOG_FILE.print(" ");
-    LOG_FILE.print(id);
-    for(int lun=0;lun<NUM_SCSILUN;lun++)
-    {
-      HDDIMG *h = &img[id][lun];
-      if( (lun<NUM_SCSILUN) && (h->m_file))
-      {
-        LOG_FILE.print((h->m_blocksize<1000) ? ": " : ":");
-        LOG_FILE.print(h->m_blocksize);
-      }
-      else      
-        LOG_FILE.print(":----");
-    }
-    LOG_FILE.println(":");
-  }
-  LOG_FILE.println("Finished initialization of SCSI Devices - Entering main loop.");
-  LOG_FILE.sync();
-  LOG_FILE.close();
-}
-
-/*
- * Initialization failed, blink 3x fast
- */
-void onFalseInit(void)
-{
-  LOG_FILE.sync();
-  while(true) {
-    for(int i = 0; i < 3; i++) {
-      LED_ON();
-      delay(250);
-      LED_OFF();
-      delay(250);
-    }
-    delay(3000);
-  }
-}
-
-/*
- * No SC Card found, blink 5x fast
- */
-void noSDCardFound(void)
-{
-  while(true) {
-    for(int i = 0; i < 5; i++) {
-      LED_ON();
-      delay(250);
-      LED_OFF();
-      delay(250);
-    }
-    delay(3000);
-  }
-}
-
-/*
- * Bus reset interrupt.
- */
-void onBusReset(void)
-{
-#if SCSI_SELECT == 1
-  // SASI I / F for X1 turbo has RST pulse write cycle +2 clock ==
-  // I can't filter because it only activates about 1.25us
-  {{
-#else
-  if(!digitalRead(RST))) {
-    delayMicroseconds(20);
-    if(!digitalRead(RST))) {
-#endif  
-  // BUS FREE is done in the main process
-      // Should I enter DB and DBP once?
-      SCSI_DB_INPUT();
-
-      LOGN("BusReset!");
-      m_isBusReset = true;
-    }
-  }
-}
-
-/*
- * Read by handshake.
- */
-inline byte readHandshake(void)
-{
-  SCSI_OUT(REQ,true)
-  //SCSI_DB_INPUT();
-  while( ! SCSI_IN(ACK)) { if(m_isBusReset) return 0; }
-  byte r = readIO();
-  SCSI_OUT(REQ, false);
-  while( SCSI_IN(ACK)) { if(m_isBusReset) return 0; }
-  return r;  
-}
-
-/*
- * Write with a handshake.
- */
-inline void writeHandshake(byte d)
-{
-  SCSI_DB_OUTPUT(); // (180ns)
-  GPIOB_PDDR = db_bsrr[d];
-  GPIOB_PDOR = db_bsrr[d] ^ SCSI_DB_MASK; // setup DB,DBP (160ns)
-  SCSI_OUT(REQ, false);  // ACK.Fall to DB output delay 100ns(MAX)  (DTC-510B)
-
-  SCSI_OUT(REQ, false); // setup wait (30ns)
-  SCSI_OUT(REQ, false); // setup wait (30ns)
-  SCSI_OUT(REQ, false); // setup wait (30ns)
-  SCSI_OUT(REQ, true);   // (30ns)
-  //while(!SCSI_IN(ACK)) { if(m_isBusReset){ SCSI_DB_INPUT() return; }}
-  while(!m_isBusReset && !SCSI_IN(ACK));
-  // ACK.Fall to REQ.Raise delay 500ns(typ.) (DTC-510B)
-  GPIOB_PDDR = DBP(0xff);
-  GPIOB_PDOR = DBP(0xff) ^ SCSI_DB_MASK;  // DB=0xFF
-  SCSI_OUT(REQ, false);
-  // REQ.Raise to DB hold time 0ns
-  SCSI_DB_INPUT(); // (150ns)
-  while( SCSI_IN(ACK)) { if(m_isBusReset) return; }
-}
-
-/*
- * Data in phase.
- *  Send len bytes of data array p.
- */
-void writeDataPhase(int len, const byte* p)
-{
-  LOGN("DATAIN PHASE");
-  SCSI_OUT(MSG, false); //  gpio_write(MSG, low);
-  SCSI_OUT(CD , false); //  gpio_write(CD, low);
-  SCSI_OUT(IO , true); //  gpio_write(IO, high);
-  for (int i = 0; i < len; i++) {
-    if(m_isBusReset) {
-      return;
-    }
-    writeHandshake(p[i]);
-  }
-}
-
-/* 
- * Data in phase.
- *  Send len block while reading from SD card.
- */
-void writeDataPhaseSD(uint32_t adds, uint32_t len)
-{
-  LOGN("DATAIN PHASE(SD)");
-  uint32_t pos = adds * m_img->m_blocksize;
-  m_img->m_file.seek(pos);
-
-  SCSI_OUT(MSG, false); //  gpio_write(MSG, low);
-  SCSI_OUT(CD , false); //  gpio_write(CD, low);
-  SCSI_OUT(IO , true); //  gpio_write(IO, high);
-
-  for(uint32_t i = 0; i < len; i++) {
-      // Asynchronous reads will make it faster ...
-    m_img->m_file.read(m_buf, m_img->m_blocksize);
-
-#if READ_SPEED_OPTIMIZE
-
-#define REQ_ON() SCSI_OUT(REQ, true);
-#define FETCH_SRC()   (src_byte = *srcptr++)
-#define FETCH_BSRR_DB() (bsrr_val = bsrr_tbl[src_byte])
-#define REQ_OFF_DB_SET(BSRR_VAL) { SCSI_OUT(REQ, false); GPIOB_PDDR = BSRR_VAL; GPIOB_PDOR = BSRR_VAL ^ SCSI_DB_MASK; }
-#define WAIT_ACK_ACTIVE()   while(!m_isBusReset && !SCSI_IN(ACK))
-#define WAIT_ACK_INACTIVE() do{ if(m_isBusReset) return; }while(SCSI_IN(ACK)) 
-
-    SCSI_DB_OUTPUT();
-    register byte *srcptr= m_buf;                 // Source buffer
-    register byte *endptr= m_buf +  m_img->m_blocksize; // End pointer
-
-    /*register*/ byte src_byte;                       // Send data bytes
-    register const uint32_t *bsrr_tbl = db_bsrr;  // Table to convert to BSRR
-    register uint32_t bsrr_val;                   // BSRR value to output (DB, DBP, REQ = ACTIVE)
-
-    // prefetch & 1st out
-    FETCH_SRC();
-    FETCH_BSRR_DB();
-    REQ_OFF_DB_SET(bsrr_val);
-    // DB.set to REQ.F setup 100ns max (DTC-510B)
-    // Maybe there should be some weight here
-    //　WAIT_ACK_INACTIVE();
-    do{
-      // 0
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      // ACK.F  to REQ.R       500ns typ. (DTC-510B)
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 1
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 2
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 3
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 4
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 5
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 6
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-      // 7
-      REQ_ON();
-      FETCH_SRC();
-      FETCH_BSRR_DB();
-      WAIT_ACK_ACTIVE();
-      REQ_OFF_DB_SET(bsrr_val);
-      WAIT_ACK_INACTIVE();
-    }while(srcptr < endptr);
-    SCSI_DB_INPUT();
-#else
-    for(unsigned int j = 0; j < m_img->m_blocksize; j++) {
-      if(m_isBusReset) {
-        return;
-      }
-      writeHandshake(m_buf[j]);
-    }
-#endif
-  }
-}
-
-/*
- * Data out phase.
- *  len block read
- */
-void readDataPhase(int len, byte* p)
-{
-  LOGN("DATAOUT PHASE");
-  SCSI_OUT(MSG,false); //  gpio_write(MSG, low);
-  SCSI_OUT(CD ,false); //  gpio_write(CD, low);
-  SCSI_OUT(IO ,false); //  gpio_write(IO, low);
-  for(uint32_t i = 0; i < len; i++)
-    p[i] = readHandshake();
-}
-
-/*
- * Data out phase.
- *  Write to SD card while reading len block.
- */
-void readDataPhaseSD(uint32_t adds, uint32_t len)
-{
-  LOGN("DATAOUT PHASE(SD)");
-  uint32_t pos = adds * m_img->m_blocksize;
-  m_img->m_file.seek(pos);
-  SCSI_OUT(MSG,false); //  gpio_write(MSG, low);
-  SCSI_OUT(CD ,false); //  gpio_write(CD, low);
-  SCSI_OUT(IO ,false); //  gpio_write(IO, low);
-  for(uint32_t i = 0; i < len; i++) {
-#if WRITE_SPEED_OPTIMIZE
-  register byte *dstptr= m_buf;
-    register byte *endptr= m_buf + m_img->m_blocksize;
-
-    for(dstptr=m_buf;dstptr<endptr;dstptr+=8) {
-      dstptr[0] = readHandshake();
-      dstptr[1] = readHandshake();
-      dstptr[2] = readHandshake();
-      dstptr[3] = readHandshake();
-      dstptr[4] = readHandshake();
-      dstptr[5] = readHandshake();
-      dstptr[6] = readHandshake();
-      dstptr[7] = readHandshake();
-      if(m_isBusReset) {
-        return;
-      }
-    }
-#else
-    for(int j = 0; j <  m_img->m_blocksize; j++) {
-      if(m_isBusReset) {
-        return;
-      }
-      m_buf[j] = readHandshake();
-    }
-#endif
-    m_img->m_file.write(m_buf, m_img->m_blocksize);
-  }
-  m_img->m_file.flush();
-}
-
-/*
- * INQUIRY command processing.
- */
-#if SCSI_SELECT == 2
-byte onInquiryCommand(byte len)
-{
-  byte buf[36] = {
-    0x00, //Device type
-    0x00, //RMB = 0
-    0x01, //ISO,ECMA,ANSI version
-    0x01, //Response data format
-    35 - 4, //Additional data length
-    0, 0, //Reserve
-    0x00, //Support function
-    'N', 'E', 'C', 'I', 'T', 'S', 'U', ' ',
-    'A', 'r', 'd', 'S', 'C', 'S', 'i', 'n', 'o', ' ', ' ',' ', ' ', ' ', ' ', ' ',
-    '0', '0', '1', '0',
-  };
-  if(!m_img) buf[0] = 0x7f;
-  writeDataPhase(len < 36 ? len : 36, buf);
-  return 0x00;
-}
-#else
-byte onInquiryCommand(byte len)
-{
-  byte buf[36];
-  memcpy(buf, SCSI_INFO_BUF, 36);
-
-  if(!m_img) buf[0] = 0x7f;
-
-  writeDataPhase(len < 36 ? len : 36, buf);
-  return 0x00;
-}
-#endif
-
-/*
- * REQUEST SENSE command processing.
- */
-void onRequestSenseCommand(byte len)
-{
-  byte buf[18] = {
-    0x70,   //CheckCondition
-    0,      //Segment number
-    0x00,   //Sense key
-    0, 0, 0, 0,  //information
-    17 - 7 ,   //Additional data length
-    0,
-  };
-  if(!m_img) {
-    // Image file absent
-    buf[2] = 0x02; // NOT_READY
-    buf[12] = 0x25; // Logical Unit Not Supported
-  } else {
-    buf[2] = m_senseKey;
-    m_senseKey = 0;
-  }
-  writeDataPhase(len < 18 ? len : 18, buf);  
-}
-
-/*
- * READ CAPACITY command processing.
- */
-byte onReadCapacityCommand(byte pmi)
-{
-  if(!m_img) return 0x02; // Image file absent
-  
-  uint32_t bl = m_img->m_blocksize;
-  uint32_t bc = m_img->m_fileSize / bl;
-  uint8_t buf[8] = {
-    bc >> 24, bc >> 16, bc >> 8, bc,
-    bl >> 24, bl >> 16, bl >> 8, bl    
-  };
-  writeDataPhase(8, buf);
-  return 0x00;
-}
-
-/*
- * READ6 / 10 Command processing.
- */
-byte onReadCommand(uint32_t adds, uint32_t len)
-{
-  LOGN("-R");
-  LOGHEXN(adds);
-  LOGHEXN(len);
-
-  if(!m_img) return 0x02; // Image file absent
-  
-  LED_ON();
-  writeDataPhaseSD(adds, len);
-  LED_OFF();
-  return 0x00; //sts
-}
-
-/*
- * WRITE6 / 10 Command processing.
- */
-byte onWriteCommand(uint32_t adds, uint32_t len)
-{
-  LOGN("-W");
-  LOGHEXN(adds);
-  LOGHEXN(len);
-  
-  if(!m_img) return 0x02; // Image file absent
-  
-  LED_ON();
-  readDataPhaseSD(adds, len);
-  LED_OFF();
-  return 0; //sts
-}
-
-/*
- * MODE SENSE command processing.
- */
-#if SCSI_SELECT == 2
-byte onModeSenseCommand(byte dbd, int cmd2, uint32_t len)
-{
-  if(!m_img) return 0x02; // Image file absent
-
-  int pageCode = cmd2 & 0x3F;
-
-  // Assuming sector size 512, number of sectors 25, number of heads 8 as default settings
-  int size = m_img->m_fileSize;
-  int cylinders = (int)(size >> 9);
-  cylinders >>= 3;
-  cylinders /= 25;
-  int sectorsize = 512;
-  int sectors = 25;
-  int heads = 8;
-  // Sector size
- int disksize = 0;
-  for(disksize = 16; disksize > 0; --(disksize)) {
-    if ((1 << disksize) == sectorsize)
-      break;
-  }
-  // Number of blocks
-  uint32_t diskblocks = (uint32_t)(size >> disksize);
-  memset(m_buf, 0, sizeof(m_buf)); 
-  int a = 4;
-  if(dbd == 0) {
-    uint32_t bl = m_img->m_blocksize;
-    uint32_t bc = m_img->m_fileSize / bl;
-    byte c[8] = {
-      0,// Density code
-      bc >> 16, bc >> 8, bc,
-      0, //Reserve
-      bl >> 16, bl >> 8, bl
-    };
-    memcpy(&m_buf[4], c, 8);
-    a += 8;
-    m_buf[3] = 0x08;
-  }
-  switch(pageCode) {
-  case 0x3F:
-  {
-    m_buf[a + 0] = 0x01;
-    m_buf[a + 1] = 0x06;
-    a += 8;
-  }
-  case 0x03:  // drive parameters
-  {
-    m_buf[a + 0] = 0x80 | 0x03; // Page code
-    m_buf[a + 1] = 0x16; // Page length
-    m_buf[a + 2] = (byte)(heads >> 8);// number of sectors / track
-    m_buf[a + 3] = (byte)(heads);// number of sectors / track
-    m_buf[a + 10] = (byte)(sectors >> 8);// number of sectors / track
-    m_buf[a + 11] = (byte)(sectors);// number of sectors / track
-    int size = 1 << disksize;
-    m_buf[a + 12] = (byte)(size >> 8);// number of sectors / track
-    m_buf[a + 13] = (byte)(size);// number of sectors / track
-    a += 24;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  }
-  case 0x04:  // drive parameters
-  {
-      LOGN("AddDrive");
-      m_buf[a + 0] = 0x04; // Page code
-      m_buf[a + 1] = 0x12; // Page length
-      m_buf[a + 2] = (cylinders >> 16);// Cylinder length
-      m_buf[a + 3] = (cylinders >> 8);
-      m_buf[a + 4] = cylinders;
-      m_buf[a + 5] = heads;   // Number of heads
-      a += 20;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  }
-  default:
-    break;
-  }
-  m_buf[0] = a - 1;
-  writeDataPhase(len < a ? len : a, m_buf);
-  return 0x00;
-}
-#else
-byte onModeSenseCommand(byte dbd, int cmd2, uint32_t len)
-{
-  if(!m_img) return 0x02; // No image file
-
-  memset(m_buf, 0, sizeof(m_buf));
-  int pageCode = cmd2 & 0x3F;
-  int a = 4;
-  if(dbd == 0) {
-    uint32_t bl =  m_img->m_blocksize;
-    uint32_t bc = m_img->m_fileSize / bl;
-
-    byte c[8] = {
-      0,//Density code
-      bc >> 16, bc >> 8, bc,
-      0, //Reserve
-      bl >> 16, bl >> 8, bl    
-    };
-    memcpy(&m_buf[4], c, 8);
-    a += 8;
-    m_buf[3] = 0x08;
-  }
-  switch(pageCode) {
-  case 0x3F:
-  case 0x03:  //Drive parameters
-    m_buf[a + 0] = 0x03; //Page code
-    m_buf[a + 1] = 0x16; // Page length
-    m_buf[a + 11] = 0x3F;//Number of sectors / track
-    a += 24;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  case 0x04:  //Drive parameters
-    {
-      uint32_t bc = m_img->m_fileSize / m_img->m_file;
-      m_buf[a + 0] = 0x04; //Page code
-      m_buf[a + 1] = 0x16; // Page length
-      m_buf[a + 2] = bc >> 16;// Cylinder length
-      m_buf[a + 3] = bc >> 8;
-      m_buf[a + 4] = bc;
-      m_buf[a + 5] = 1;   //Number of heads
-      a += 24;
-    }
-    if(pageCode != 0x3F) {
-      break;
-    }
-  default:
-    break;
-  }
-  m_buf[0] = a - 1;
-  writeDataPhase(len < a ? len : a, m_buf);
-  return 0x00;
-}
-#endif
-    
-#if SCSI_SELECT == 1
-/*
- * dtc510b_setDriveparameter
- */
-#define PACKED  __attribute__((packed))
-typedef struct PACKED dtc500_cmd_c2_param_struct
-{
-  uint8_t StepPlusWidth;        // Default is 13.6usec (11)
-  uint8_t StepPeriod;         // Default is  3  msec.(60)
-  uint8_t StepMode;         // Default is  Bufferd (0)
-  uint8_t MaximumHeadAdress;      // Default is 4 heads (3)
-  uint8_t HighCylinderAddressByte;  // Default set to 0   (0)
-  uint8_t LowCylinderAddressByte;   // Default is 153 cylinders (152)
-  uint8_t ReduceWrietCurrent;     // Default is above Cylinder 128 (127)
-  uint8_t DriveType_SeekCompleteOption;// (0)
-  uint8_t Reserved8;          // (0)
-  uint8_t Reserved9;          // (0)
-} DTC510_CMD_C2_PARAM;
-
-static void logStrHex(const char *msg,uint32_t num)
-{
-    LOG(msg);
-    LOGHEXN(num);
-}
-
-static byte dtc510b_setDriveparameter(void)
-{
-  DTC510_CMD_C2_PARAM DriveParameter;
-  uint16_t maxCylinder;
-  uint16_t numLAD;
-  //uint32_t stepPulseUsec;
-  int StepPeriodMsec;
-
-  // receive paramter
-  writeDataPhase(sizeof(DriveParameter),(byte *)(&DriveParameter));
- 
-  maxCylinder =
-    (((uint16_t)DriveParameter.HighCylinderAddressByte)<<8) |
-    (DriveParameter.LowCylinderAddressByte);
-  numLAD = maxCylinder * (DriveParameter.MaximumHeadAdress+1);
-  //stepPulseUsec  = calcStepPulseUsec(DriveParameter.StepPlusWidth);
-  StepPeriodMsec = DriveParameter.StepPeriod*50;
-  logStrHex (" StepPlusWidth      : ",DriveParameter.StepPlusWidth);
-  logStrHex (" StepPeriod         : ",DriveParameter.StepPeriod   );
-  logStrHex (" StepMode           : ",DriveParameter.StepMode     );
-  logStrHex (" MaximumHeadAdress  : ",DriveParameter.MaximumHeadAdress);
-  logStrHex (" CylinderAddress    : ",maxCylinder);
-  logStrHex (" ReduceWrietCurrent : ",DriveParameter.ReduceWrietCurrent);
-  logStrHex (" DriveType/SeekCompleteOption : ",DriveParameter.DriveType_SeekCompleteOption);
-  logStrHex (" Maximum LAD        : ",numLAD-1);
-  return  0; // error result
-}
-#endif
-
-/*
- * MsgIn2.
- */
-void MsgIn2(int msg)
-{
-  LOGN("MsgIn2");
-  SCSI_OUT(MSG, true); //  gpio_write(MSG, high);
-  SCSI_OUT(CD , true); //  gpio_write(CD, high);
-  SCSI_OUT(IO , true); //  gpio_write(IO, high);
-  writeHandshake(msg);
-}
-
-/*
- * MsgOut2.
- */
-void MsgOut2()
-{
-  LOGN("MsgOut2");
-  SCSI_OUT(MSG, true); //  gpio_write(MSG, high);
-  SCSI_OUT(CD , true); //  gpio_write(CD, high);
-  SCSI_OUT(IO , false); //  gpio_write(IO, low);
-  m_msb[m_msc] = readHandshake();
-  m_msc++;
-  m_msc %= 256;
-}
-
-/*
- * Main loop.
- */
-void loop() 
-{
-  //int msg = 0;
-  m_msg = 0;
-  m_lun = 0xff;
-
-  // Wait until RST = H, BSY = H, SEL = L
-  do {} while( SCSI_IN(BSY) || !SCSI_IN(SEL) || SCSI_IN(RST));
-
-  // BSY+ SEL-
-  // If the ID to respond is not driven, wait for the next
-  //byte db = readIO();
-  //byte scsiid = db & scsi_id_mask;
-  byte scsiid = readIO() & scsi_id_mask;
-  if((scsiid) == 0) {
-    return;
-  }
-  LOGN("Selection");
-  m_isBusReset = false;
-  // Set BSY to-when selected
-  SCSI_BSY_ACTIVE();     // Turn only BSY output ON, ACTIVE
-
-  // Ask for a TARGET-ID to respond
-#if USE_DB2ID_TABLE
-  m_id = db2scsiid[scsiid];
-  //if(m_id==0xff) return;
-#else
-  for(m_id=7;m_id>=0;m_id--)
-    if(scsiid & (1<<m_id)) break;
-  //if(m_id<0) return;
-#endif
-
-  // Wait until SEL becomes false
-  while(!digitalRead(SEL) && digitalRead(BSY)) {
-    if(m_isBusReset) {
-      goto BusFree;
-    }
-  }
-  SCSI_TARGET_ACTIVE()  // (BSY), REQ, MSG, CD, IO output turned on
-  //  
-  if(!digitalRead(ATN)) {
-    bool syncenable = false;
-    int syncperiod = 50;
-    int syncoffset = 0;
-    int loopWait = 0;
-    m_msc = 0;
-    memset(m_msb, 0x00, sizeof(m_msb));
-    while(!digitalRead(ATN) && loopWait < 255) {
-      MsgOut2();
-      loopWait++;
-    }
-    for(int i = 0; i < m_msc; i++) {
-      // ABORT
-      if (m_msb[i] == 0x06) {
-        goto BusFree;
-      }
-      // BUS DEVICE RESET
-      if (m_msb[i] == 0x0C) {
-        syncoffset = 0;
-        goto BusFree;
-      }
-      // IDENTIFY
-      if (m_msb[i] >= 0x80) {
-        m_lun = m_msb[i] & 0x07;
-      }
-      // Extended message
-      if (m_msb[i] == 0x01) {
-        // Check only when synchronous transfer is possible
-        if (!syncenable || m_msb[i + 2] != 0x01) {
-          MsgIn2(0x07);
-          break;
-        }
-        // Transfer period factor(50 x 4 = Limited to 200ns)
-        syncperiod = m_msb[i + 3];
-        if (syncperiod > 50) {
-          syncperiod = 50;
-        }
-        // REQ/ACK offset(Limited to 16)
-        syncoffset = m_msb[i + 4];
-        if (syncoffset > 16) {
-          syncoffset = 16;
-        }
-        // STDR response message generation
-        MsgIn2(0x01);
-        MsgIn2(0x03);
-        MsgIn2(0x01);
-        MsgIn2(syncperiod);
-        MsgIn2(syncoffset);
-        break;
-      }
-    }
-  }
-
-  LOG("Command:");
-  SCSI_OUT(MSG, false); // gpio_write(MSG, low);
-  SCSI_OUT(CD , true); // gpio_write(CD, high);
-  SCSI_OUT(IO , false); // gpio_write(IO, low);
-  
-  int len;
-  byte cmd[12];
-  cmd[0] = readHandshake(); if(m_isBusReset) goto BusFree;
-  LOGHEX(cmd[0]);
-  // Command length selection, reception
-  static const int cmd_class_len[8]={6,10,10,6,6,12,6,6};
-  len = cmd_class_len[cmd[0] >> 5];
-  cmd[1] = readHandshake(); LOG(":");LOGHEX(cmd[1]); if(m_isBusReset) goto BusFree;
-  cmd[2] = readHandshake(); LOG(":");LOGHEX(cmd[2]); if(m_isBusReset) goto BusFree;
-  cmd[3] = readHandshake(); LOG(":");LOGHEX(cmd[3]); if(m_isBusReset) goto BusFree;
-  cmd[4] = readHandshake(); LOG(":");LOGHEX(cmd[4]); if(m_isBusReset) goto BusFree;
-  cmd[5] = readHandshake(); LOG(":");LOGHEX(cmd[5]); if(m_isBusReset) goto BusFree;
-  // Receive the remaining commands
-  for(int i = 6; i < len; i++ ) {
-    cmd[i] = readHandshake();
-    LOG(":");
-    LOGHEX(cmd[i]);
-    if(m_isBusReset) goto BusFree;
-  }
-  // LUN confirmation
-  m_sts = cmd[1]&0xe0;      // Preset LUN in status byte
-  if(m_lun == 0xff) m_lun = m_sts>>5;
-  // HDD Image selection
-  m_img = (HDDIMG *)0; // None
-  if( (m_lun <= NUM_SCSILUN) )
-  {
-    m_img = &(img[m_id][m_lun]); // There is an image
-    if(!(m_img->m_file.isOpen()))
-      m_img = (HDDIMG *)0;       // Image absent
-  }
-  //LOGHEX(((uint32_t)m_img));
-  
-  LOG(":ID ");
-  LOG(m_id);
-  LOG(":LUN ");
-  LOG(m_lun);
-
-  LOGN("");
-
-  if(!m_img) {
-    switch(cmd[0]) {
-    case 0x03:
-      LOGN("[RequestSense]");
-      onRequestSenseCommand(cmd[4]);
-      break;
-    case 0x12:
-      LOGN("[Inquiry]");
-      m_sts |= onInquiryCommand(cmd[4]);
-      break;
-    default:
-      LOGN("[Bad LUN]");
-      m_sts |= 0x02;
-      break;
-    }
-  } else {
-    switch(cmd[0]) {
-    case 0x00:
-      LOGN("[Test Unit]");
-      break;
-    case 0x01:
-      LOGN("[Rezero Unit]");
-      break;
-    case 0x03:
-      LOGN("[RequestSense]");
-      onRequestSenseCommand(cmd[4]);
-      break;
-    case 0x04:
-      LOGN("[FormatUnit]");
-      break;
-    case 0x06:
-      LOGN("[FormatUnit]");
-      break;
-    case 0x07:
-      LOGN("[ReassignBlocks]");
-      break;
-    case 0x08:
-      LOGN("[Read6]");
-      m_sts |= onReadCommand((((uint32_t)cmd[1] & 0x1F) << 16) | ((uint32_t)cmd[2] << 8) | cmd[3], (cmd[4] == 0) ? 0x100 : cmd[4]);
-      break;
-    case 0x0A:
-      LOGN("[Write6]");
-      m_sts |= onWriteCommand((((uint32_t)cmd[1] & 0x1F) << 16) | ((uint32_t)cmd[2] << 8) | cmd[3], (cmd[4] == 0) ? 0x100 : cmd[4]);
-      break;
-    case 0x0B:
-      LOGN("[Seek6]");
-      break;
-    case 0x12:
-      LOGN("[Inquiry]");
-      m_sts |= onInquiryCommand(cmd[4]);
-      break;
-    case 0x1A:
-      LOGN("[ModeSense6]");
-      m_sts |= onModeSenseCommand(cmd[1]&0x80, cmd[2], cmd[4]);
-      break;
-    case 0x1B:
-      LOGN("[StartStopUnit]");
-      break;
-    case 0x1E:
-      LOGN("[PreAllowMed.Removal]");
-      break;
-    case 0x25:
-      LOGN("[ReadCapacity]");
-      m_sts |= onReadCapacityCommand(cmd[8]);
-      break;
-    case 0x28:
-      LOGN("[Read10]");
-      m_sts |= onReadCommand(((uint32_t)cmd[2] << 24) | ((uint32_t)cmd[3] << 16) | ((uint32_t)cmd[4] << 8) | cmd[5], ((uint32_t)cmd[7] << 8) | cmd[8]);
-      break;
-    case 0x2A:
-      LOGN("[Write10]");
-      m_sts |= onWriteCommand(((uint32_t)cmd[2] << 24) | ((uint32_t)cmd[3] << 16) | ((uint32_t)cmd[4] << 8) | cmd[5], ((uint32_t)cmd[7] << 8) | cmd[8]);
-      break;
-    case 0x2B:
-      LOGN("[Seek10]");
-      break;
-    case 0x5A:
-      LOGN("[ModeSense10]");
-      onModeSenseCommand(cmd[1] & 0x80, cmd[2], ((uint32_t)cmd[7] << 8) | cmd[8]);
-      break;
-#if SCSI_SELECT == 1
-    case 0xc2:
-      LOGN("[DTC510B setDriveParameter]");
-      m_sts |= dtc510b_setDriveparameter();
-      break;
-#endif    
-    default:
-      LOGN("[*Unknown]");
-      m_sts |= 0x02;
-      m_senseKey = 5;
-      break;
-    }
-  }
-  if(m_isBusReset) {
-     goto BusFree;
-  }
-
-  LOGN("Sts");
-  SCSI_OUT(MSG, false); // gpio_write(MSG, low);
-  SCSI_OUT(CD , true); // gpio_write(CD, high);
-  SCSI_OUT(IO , true); // gpio_write(IO, high);
-  writeHandshake(m_sts);
-  if(m_isBusReset) {
-     goto BusFree;
-  }
-
-  LOGN("MsgIn");
-  SCSI_OUT(MSG, true); // gpio_write(MSG, high);
-  SCSI_OUT(CD , true); // gpio_write(CD, high);
-  SCSI_OUT(IO , true); // gpio_write(IO, high);
-  writeHandshake(m_msg);
-
-BusFree:
-  LOGN("BusFree");
-  m_isBusReset = false;
-  //SCSI_OUT(REQ,false) // gpio_write(REQ, low);
-  //SCSI_OUT(MSG,false) // gpio_write(MSG, low);
-  //SCSI_OUT(CD ,false) // gpio_write(CD, low);
-  //SCSI_OUT(IO ,false) // gpio_write(IO, low);
-  //SCSI_OUT(BSY,false)
-  SCSI_TARGET_INACTIVE(); // Turn off BSY, REQ, MSG, CD, IO output
-}
diff --git a/src/cmd.h b/src/cmd.h
new file mode 100755
index 0000000..d248a20
--- /dev/null
+++ b/src/cmd.h
@@ -0,0 +1,37 @@
+#ifndef __CMD_H
+#define __CMD_H
+
+#define MAX_MSG_SIZE    256
+#define MAXARG           32
+
+#include <stdint.h>
+
+// command line structure
+typedef struct Commands_s {
+  const char *Name;
+  const char *ValidPath;
+  int MinParams;
+  const char *ShortHelp;
+  const char *FullHelp;
+  void (*Func)(int argc, char **argv);
+  struct Commands_s *Dispatch;
+} Commands_t;
+
+void cmdDisplay();
+void cmdParse(char *cmd);
+void cmdPoll();
+
+void cmdCommandHelp(boolean singleCommand, Commands_t *table, int cmd);
+void cmdDispatchHelp(Commands_t *table, int argc, char **argv);
+void cmdDispatch(Commands_t *table, int argc, char **argv);
+
+int execHandler(char *filename);
+int execLoop();
+
+void execcmd(int argc, char **argv);
+void showcmd(int argc, char **argv);
+void setcmd(int argc, char **argv);
+
+extern Commands_t GlobalCommands[];
+
+#endif /* __CMD_H */
diff --git a/src/cmd.ino b/src/cmd.ino
new file mode 100755
index 0000000..e9043ae
--- /dev/null
+++ b/src/cmd.ino
@@ -0,0 +1,1379 @@
+#include "cmd.h"
+
+// command line message buffer and pointer
+static uint8_t msg[MAX_MSG_SIZE] = "";
+static uint8_t *msg_ptr = msg;
+
+char exec_string[MAX_FILE_PATH+1] = "";
+char exec_file[MAX_FILE_PATH+1] = "";
+int exec_line = 0;
+int echo_on = 1;
+
+char cmd_prefix[256] = "/";
+
+void cmdDisplay() {
+  if(cmd_prefix[0] != 0) {
+    Serial.printf("[%s] $ ", cmd_prefix);
+  } else {
+    strcpy(cmd_prefix, "/");
+    Serial.print("[/] $ ");
+  }
+}
+
+/*
+ * Like strtok, but with quotes
+ */
+char *strmbtok ( char *input, const char *delimit, const char *openblock, const char *closeblock) {
+    static char *token = NULL;
+    char *lead = NULL;
+    char *block = NULL;
+    int iBlock = 0;
+    int iBlockIndex = 0;
+
+    if ( input != NULL) {
+        token = input;
+        lead = input;
+    }
+    else {
+        lead = token;
+        if ( *token == '\0') {
+            lead = NULL;
+        }
+    }
+
+    while ( *token != '\0') {
+        // We are in a quote, is this a matching close quote character?
+        if ( iBlock) {
+            if ( closeblock[iBlockIndex] == *token) {
+                iBlock = 0;
+            }
+            token++;
+            continue;
+        }
+
+        // Is this a valid opening quote?
+        if ( ( block = strchr ( openblock, *token)) != NULL) {
+            iBlock = 1;
+            iBlockIndex = block - openblock;
+            token++;
+            continue;
+        }
+        if ( strchr ( delimit, *token) != NULL) {
+            *token = '\0';
+            token++;
+            break;
+        }
+        token++;
+    }
+    return lead;
+}
+
+/*
+ * Remove quotes from quoted strings.
+ */
+void unquote(char *out, char *in) {
+  char tmp_str[256];
+
+  if(!in) return;
+  if(!out) return;
+
+  char *pout = tmp_str;
+  char inQuote = 0;
+  while(*in != 0) {
+    if(inQuote && (*in == inQuote)) {
+      inQuote = 0;
+    } else if(!inQuote && (*in == '"')) {
+      inQuote = '"';
+    } else if(!inQuote && (*in == '\'')) {
+      inQuote = '\'';
+    } else {
+      *pout = *in;
+      pout++;
+    }
+    in++;
+  }
+  *pout = 0;
+
+  // This way you can have out == in
+  strcpy(out, tmp_str);
+}
+
+void cmdParse(char *cmd) {
+  uint8_t argc, i = 0;
+  char *argv[MAXARG];
+
+  // leading whitespace and blank lines are ignored
+  while((*cmd == ' ') || (*cmd == '\t')) cmd++;
+	if(!cmd[0])	return;
+
+  // parse the statement and tokenize it (with quote handling)
+  argv[i] = strmbtok(cmd, " ", "\"'", "\"'");
+  while ((argv[i] != NULL) && (i < MAXARG)) {
+    // unquote the previous argument (last argument is always NULL)
+    unquote(argv[i], argv[i]);
+    argv[++i] = strmbtok(NULL, " ", "\"'", "\"'");
+  };
+  
+  // save the number of arguments
+  argc = i;
+
+  // find the handler responsible for this command
+  for(int ii=0; GlobalCommands[ii].Name; ii++) {
+    if (!strcmp(argv[0], GlobalCommands[ii].Name)) {
+      GlobalCommands[ii].Func(argc, argv);
+      return;
+    }
+  }
+
+  // command not recognized. print message and re-generate prompt.
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.printf(": Unknown command '%s'.\r\n", argv[0]);
+}
+
+void cmd_handler() {
+  char c = Serial.read();
+
+  switch (c) {
+    case '\n':
+    case '\r':
+      // terminate the msg and reset the msg ptr. then send
+      // it to the handler for processing.
+      *msg_ptr = '\0';
+      if(msg[0] != 0) {
+        Serial.print("\r\n");
+        cmdParse((char *)msg);
+        execLoop();
+        cmdDisplay();
+      }
+      msg_ptr = msg;
+      break;
+  
+    case '\b':
+      // backspace 
+      Serial.print(c);
+      if (msg_ptr > msg)
+      {
+          msg_ptr--;
+      }
+      break;
+  
+    default:
+      // normal character entered. add it to the buffer
+      Serial.print(c);
+      *msg_ptr++ = c;
+      break;
+  }
+}
+
+void cmdPoll() {
+  if (Serial.available()) {
+    cmd_handler();
+  }
+}
+
+int execLoop() {
+  if(!exec_string[0]) return -1;
+  
+  char exec_command[sizeof(exec_string)];
+  memset(exec_command, 0, sizeof(exec_string));
+
+  strncpy(exec_command, exec_string, sizeof(exec_string)-1);
+  memset(exec_string, 0, sizeof(exec_string));
+  execHandler(exec_command);
+
+  return 0;
+}
+
+void execcmd(int argc, char **argv) {
+  if(exec_file[0]) {
+    Serial.printf("ERROR on line %d of '%s': exec called from inside a script.\r\n", exec_line, exec_file);
+    return;
+  }
+
+  if(argc > 1) {
+    memset(exec_string, 0, sizeof(exec_string));
+    strncpy(exec_string, argv[1], sizeof(exec_string)-1);
+  }
+}
+
+void echocmd(int argc, char **argv) {
+  if(argc > 1) {
+    if(!strcmp("off", argv[1])) {
+      echo_on = 0;
+    } else if(!strcmp("on", argv[1])) {
+      echo_on = 1;
+    } else {
+      for(int i = 1; i<argc; i++) {
+        Serial.printf("%s ", argv[i]);
+      }
+      Serial.printf("\r\n");
+    }
+  }
+}
+
+int execHandler(char *filepath) {
+  FsFile file;            /* File object */
+  char exec_cmd[256];
+  char tmp_path[MAX_FILE_PATH+1];
+  char *filename = tmp_path;
+
+  strcpy(exec_file, filepath);
+
+  fixupPath(tmp_path, filepath);
+  if(!strncmp(filename, "/sd/", 4)) {
+    filename += 3;
+  } else {
+    Serial.printf("ERROR: file '%s' not found.\r\n", filepath);
+    return -1;
+  }
+
+  if(!sd.exists(filename)) {
+    Serial.printf("ERROR: file '%s' not found.\r\n", filepath);
+    return -1;
+  }
+
+  if(!file.open(filename, O_RDONLY)) {
+    Serial.printf("ERROR: Unable to open script '%s'.\r\n", filepath);
+  } else {
+    exec_line = 1;
+    while(file.available()) {
+      int n = file.fgets(exec_cmd, sizeof(exec_cmd)-1, NULL);
+      if (n <= 0) {
+        Serial.printf("Reading script '%s' failed at line %d.\r\n", filepath, exec_line);
+        
+        break;
+      }
+      exec_cmd[sizeof(exec_cmd)-1] = 0;
+      for(int ii = 0; ii < n; ii++) {
+        if(exec_cmd[ii] == '\r') exec_cmd[ii] = 0;
+        if(exec_cmd[ii] == '\n') exec_cmd[ii] = 0;
+      }
+
+      if(echo_on && exec_cmd[0]!='@')
+        Serial.printf("%s\r\n", exec_cmd);
+      if(exec_cmd[0] != '#')
+        cmdParse((exec_cmd[0]!='@') ? exec_cmd : exec_cmd+1);
+
+      exec_line++;
+    }
+    file.close();
+    exec_line = 0;
+    exec_file[0] = 0;
+  }
+
+  strcpy(cmd_prefix, "/");
+
+  return 0;
+}
+
+void cmdCommandHelp(boolean singleCommand, Commands_t *table, int cmd) {
+  if(!table) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": Command table invalid.\r\n");
+    return;
+  }
+  if(singleCommand && table[cmd].FullHelp) {
+    Serial.print(table[cmd].FullHelp);
+  } else if(table[cmd].ShortHelp) {
+    Serial.print("\r\n");
+    Serial.print(table[cmd].Name);
+    for(uint8_t i = 0; i < (16 - strlen(table[cmd].Name)); i++) {
+      Serial.print(" ");
+    }
+    Serial.printf("%s\r\n", table[cmd].ShortHelp);
+    if(table[cmd].MinParams) {
+      Serial.printf("                (Requires %d %s)\r\n", table[cmd].MinParams, (table[cmd].MinParams>1) ? "Parameters" : "Parameter");
+    }
+    if(singleCommand)
+      Serial.print("\r\n");
+  }
+}
+
+void cmdDispatchHelp(Commands_t *table, int argc, char **argv) {
+  if(!table) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": invalid command table.\r\n");
+    return;
+  }
+  if(argc<2) {
+    for(int ii=0; table[ii].Name; ii++) {
+      cmdCommandHelp(false, table, ii);
+    }
+    Serial.print("\r\n");
+    return;
+  }
+  for(int ii=0; table[ii].Name; ii++) {
+    if(!strcmp(argv[1], table[ii].Name)) {
+      if(table[ii].Dispatch) {
+        cmdDispatchHelp(table[ii].Dispatch, argc-1, &argv[1]);
+      } else {
+        cmdCommandHelp(true, table, ii);
+      }
+      return;
+    }
+  }
+}
+
+void help(int argc, char **argv) {
+  cmdDispatchHelp(GlobalCommands, argc, argv);
+}
+
+void cmdDispatch(Commands_t *table, int argc, char **argv) {
+  int ii;
+
+  if(!table) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": Command table invalid.\r\n");
+    return;
+  }
+
+  argc--;
+  if(argc<1) {
+    for(ii=0; table[ii].Name; ii++) {
+      cmdCommandHelp(false, table, ii);
+    }
+    return;
+  }
+  for(ii=0; table[ii].Name; ii++) {
+    if(!strcmp(argv[1], table[ii].Name)) {
+      if(argc < table[ii].MinParams) {
+        Serial.print("ERROR");
+        if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+        Serial.printf(": Not enough parameters given\r\n");
+        return;
+      }
+      table[ii].Func(argc, &argv[1]);
+    }
+  }
+}
+
+void fixupPath(char *out, char *in) {
+  char *parent;
+
+  char tmp_path[MAX_FILE_PATH+1];
+
+  if(!in) return;
+  if(!out) return;
+
+  //unquote(tmp_path, in);
+  strcpy(tmp_path, in);
+
+  // Skip leading "./" if present
+  if(!strncmp(tmp_path, "./", 2)) {
+    sprintf(out, "%s/%s", cmd_prefix, tmp_path+2);
+  } else if(!strcmp(tmp_path, "..")) {
+    // Ascend if ".."
+    strcpy(tmp_path, cmd_prefix);
+    parent = strrchr(tmp_path, '/');
+    if(parent && (tmp_path != parent)) {
+      parent[0] = 0;
+    }
+    strcpy(out, tmp_path);
+  } else if(*tmp_path == '/') {
+    // Fully qualified path, replace everything
+    strcpy(out, tmp_path);
+  } else {
+    // Relative path, append
+    parent = strrchr(cmd_prefix, '/');
+    if(parent[1] != 0) {
+      sprintf(out, "%s/%s", cmd_prefix, tmp_path);
+    } else {
+      sprintf(out, "%s%s", cmd_prefix, tmp_path);
+    }
+  }
+
+  // Make sure there is not a trailing /
+  parent = strrchr(out, '/');
+  if((parent != out) && (parent[1] == 0)) {
+    parent[0] = 0;
+  }
+}
+
+// Remove any command prefix levels
+void changeDirectory(int argc, char **argv) {
+  char new_prefix[256];
+  char *suffix = NULL;
+
+  if(argc>=2) {
+    // Parse the path string
+    fixupPath(new_prefix, argv[1]);
+
+    if(!strncmp("/vdevs/vdev", new_prefix, 11)) {
+      char *vdev_name = new_prefix + 11;
+      uint8_t v = strtol(vdev_name, &suffix, 0);
+      if((v < NUM_VDEV) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+        sprintf(cmd_prefix, "/vdevs/vdev%d", v);
+        return;
+      }
+      goto e_invalidpath;
+    }
+
+    if(!strncmp("/tgts/tgt", new_prefix, 9)) {
+      char *tgt_name = new_prefix + 9;
+      char *lun_name = NULL;
+      uint8_t id = strtol(tgt_name, &lun_name, 0);
+      if(id < NUM_SCSIID) {
+        if(!lun_name || (lun_name[0] == 0) || !strcmp(lun_name, "/")) {
+          sprintf(cmd_prefix, "/tgts/tgt%d", id);
+          return;
+        } else if(!strncmp("/lun", lun_name, 4)) {
+          lun_name += 4;
+          uint8_t lun = strtol(lun_name, &suffix, 0);
+          if((lun < NUM_SCSILUN) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+            sprintf(cmd_prefix, "/tgts/tgt%d/lun%d", id, lun);
+            return;
+          }
+        }
+      }
+      goto e_invalidpath;
+    }
+
+    if(!strcmp(new_prefix, "/") || !strcmp(new_prefix, "/tgts") || !strcmp(new_prefix, "/vdevs")
+       || !strcmp(new_prefix, "/sd") || !strcmp(new_prefix, "/diag")|| !strcmp(new_prefix, "/diag/sd")
+    ) {
+      strcpy(cmd_prefix, new_prefix);
+      return;
+    } else if(!strncmp(new_prefix, "/sd/", 4)) {
+      if((new_prefix[4] == 0) || sd.exists(new_prefix+3)) {
+        strcpy(cmd_prefix, new_prefix);
+        return;
+      }
+    }
+
+e_invalidpath:
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": Path '%s' not found.\r\n", new_prefix);
+  } else {
+    Serial.printf("%s\r\n", cmd_prefix);
+  }
+}
+
+char tmp_left[256];
+char tmp_right[256];
+
+void printDirectory(int level, uint8_t lines, const char *l, const char *r) {
+  for(int pad = 0; pad < level; pad++) {
+    if(lines & (1 << (level - pad))) {
+      Serial.print("| ");
+    } else {
+      Serial.print("  ");
+    }
+  }
+  Serial.print("o- ");
+  int dots = 78 - ((level*2) + 3 + strlen(l) + strlen(r));
+  Serial.print(l);
+  while(dots--) {
+    Serial.print(".");
+  }
+  Serial.print(r);
+  Serial.print("\r\n");
+}
+
+void showVDEV(int level, uint8_t lines, uint8_t v) {
+  VirtualDevice_t *h = &m_vdev[v];
+  if(h->m_enabled) {
+    sprintf(tmp_left, "vdev%d/ ", v);
+    printDirectory(level, lines, tmp_left, " [...]");
+
+    level++;
+    lines<<=1;
+    lines|=1;
+
+    switch(h->m_type) {
+      case DEV_DISK:
+        strcpy(tmp_right, " [disk]");
+        break;
+      case DEV_OPTICAL:
+        strcpy(tmp_right, " [optical]");
+        break;
+      case DEV_TAPE:
+        strcpy(tmp_right, " [tape]");
+        break;
+      default:
+        strcpy(tmp_right, " [unknown]");
+        break;
+    }
+    printDirectory(level, lines, "$type ", tmp_right);
+
+    if((h->m_id < NUM_SCSIID) && (h->m_lun < NUM_SCSILUN)) {
+      sprintf(tmp_right, " [/tgts/tgt%d/lun%d]", h->m_id, h->m_lun);
+      printDirectory(level, lines, "$mapping", tmp_right);
+    }
+    if(h->m_file) {
+      sprintf(tmp_right, " [%s]", h->m_filename);
+      printDirectory(level, lines, "$filename ", tmp_right);
+    }
+    if((h->m_fileSize) && (h->m_blocksize)) {
+      sprintf(tmp_right, " [%llu]", h->m_fileSize / h->m_blocksize);
+      printDirectory(level, lines, "$blocks ", tmp_right);
+    }
+    if(h->m_blocksize) {
+      sprintf(tmp_right, " [%d]", h->m_blocksize);
+      printDirectory(level, lines, "$blocksize ", tmp_right);
+    }
+  }
+}
+
+void showLUN(int level, uint8_t lines, uint8_t id, uint8_t lun) {
+  if(!level) {
+    sprintf(tmp_left, "/tgts/tgt%d/lun%d/ ", id, lun);
+  } else {
+    sprintf(tmp_left, "lun%d/ ", lun);
+  }
+  uint8_t v = m_vdevmap[id][lun];
+  if(v < NUM_VDEV) {
+    printDirectory(level, lines, tmp_left, " [...]");
+
+    level++;
+    lines <<= 1;
+    lines |= 1;
+    sprintf(tmp_right, " [/vdevs/vdev%d]", v);
+    printDirectory(level, lines, "$mapping", tmp_right);
+  }
+}
+
+void showTarget(int level, uint8_t lines, uint8_t id) {
+  if(!level) {
+    sprintf(tmp_left, "/tgts/tgt%d/ ", id);
+  } else {
+    sprintf(tmp_left, "tgt%d/ ", id);
+  }
+  if(scsi_id_mask & (1<<id)) {
+    printDirectory(level, lines, tmp_left, " [...]");
+
+    level++;
+    lines <<= 1;
+    lines |= 1;
+    for(uint8_t lun = 0; lun < NUM_SCSILUN; lun++) {
+      showLUN(level, lines, id, lun);
+    }
+  } else {
+    printDirectory(level, lines, tmp_left, " [DISABLED]");
+  }
+}
+
+void showDirectory(int argc, char **argv) {
+  char *local_prefix = cmd_prefix;
+  char tmp_prefix[256];
+  char *suffix = NULL;
+  uint8_t v;
+
+  if(argc == 2) {
+    fixupPath(tmp_prefix, argv[1]);
+    local_prefix = tmp_prefix;
+  }
+  if(!strcmp(local_prefix, "/")) {
+    printDirectory(0, 0, "/ ", " [...]");
+    printDirectory(1, 0, "diag/ ", " [...]");
+    printDirectory(1, 0, "sd/ ", " [...]");
+    sprintf(tmp_right, " [%d Target%s]", NUM_SCSIID, (NUM_SCSIID != 1) ? "s" : "");
+    printDirectory(1, 0, "tgts/ ", tmp_right);
+    sprintf(tmp_right, " [%d Storage Object%s]", m_vdevcnt, (m_vdevcnt != 1) ? "s" : "");
+    printDirectory(1, 0, "vdevs/ ", tmp_right);
+    return;
+  }
+  if(!strcmp(local_prefix, "/diag")) {
+    printDirectory(0, 0, "/diag/ ", " [...]");
+    printDirectory(1, 0, "sd/ ", " [...]");
+    return;
+  }
+  if(!strcmp(local_prefix, "/diag/sd")) {
+    printDirectory(0, 0, "/diag/sd/ ", " [...]");
+
+    sprintf(tmp_right, " [SdFat %s]", SD_FAT_VERSION_STR);
+    printDirectory(1, 0, "$version ", tmp_right);
+
+    switch(SDFAT_FILE_TYPE) {
+      case 0:
+        strcpy(tmp_right, " [FAT16]");
+        break;
+      case 1:
+        strcpy(tmp_right, " [FAT16/FAT32]");
+        break;
+      case 2:
+        strcpy(tmp_right, " [ExFAT]");
+        break;
+      case 3:
+        strcpy(tmp_right, " [FAT16/FAT32/ExFAT]");
+        break;
+    }
+    printDirectory(1, 0, "$filesystems ", tmp_right);
+
+    sprintf(tmp_right, " [%d]", MAX_FILE_PATH);
+    printDirectory(1, 0, "$max_filename_length ", tmp_right);
+
+    csd_t sd_csd;
+    if(sd.card()->readCSD(&sd_csd))
+    {
+      switch (sd.card()->type()) {
+        case SD_CARD_TYPE_SD1:
+          strcpy(tmp_right, " [SD1]");
+          break;
+        case SD_CARD_TYPE_SD2:
+          strcpy(tmp_right, " [SD2]");
+          break;
+        case SD_CARD_TYPE_SDHC:
+          if (sdCardCapacity(&sd_csd) < 70000000) {
+            strcpy(tmp_right, " [SDHC]");
+          } else {
+            strcpy(tmp_right, " [SDXC]");
+          }
+          break;
+        default:
+          strcpy(tmp_right, " [unknown]");
+          break;
+      }
+      printDirectory(1, 0, "$card_type ", tmp_right);
+
+      uint32_t cardsize = 0.000512 * sdCardCapacity(&sd_csd);
+      if(cardsize >= 10000) {
+        sprintf(tmp_right, " [%lu GB]", cardsize);
+      } else {
+        sprintf(tmp_right, " [%lu MB]", cardsize);
+      }
+      printDirectory(1, 0, "$card_size ", tmp_right);
+    }
+    cid_t sd_cid;
+    if(sd.card()->readCID(&sd_cid))
+    {
+      sprintf(tmp_right, " [0x%02X]", sd_cid.mid);
+      printDirectory(1, 0, "$card_manufacturer ", tmp_right);
+
+      sprintf(tmp_right, " [%c%c]", sd_cid.oid[0], sd_cid.oid[1]);
+      printDirectory(1, 0, "$card_oem ", tmp_right);
+
+      sprintf(tmp_right, " [%c%c%c%c%c]", sd_cid.pnm[0], sd_cid.pnm[1], sd_cid.pnm[2], sd_cid.pnm[3], sd_cid.pnm[4]);
+      printDirectory(1, 0, "$card_name ", tmp_right);
+
+      sprintf(tmp_right, " [%d.%d]", sd_cid.prv_n, sd_cid.prv_m);
+      printDirectory(1, 0, "$card_version ", tmp_right);
+
+      sprintf(tmp_right, " [%02d/20%02d]", sd_cid.mdt_month, (sd_cid.mdt_year_high << 4) | sd_cid.mdt_year_low);
+      printDirectory(1, 0, "$card_date ", tmp_right);
+
+      sprintf(tmp_right, " [%08lX]", sd_cid.psn);
+      printDirectory(1, 0, "$card_serial ", tmp_right);
+    }
+    return;
+  }  
+  if(!strcmp(local_prefix, "/tgts")) {
+    sprintf(tmp_right, " [%d Target%s]", NUM_SCSIID, (NUM_SCSIID != 1) ? "s" : "");
+    printDirectory(0, 0, "/tgts/ ", tmp_right);
+    for(uint8_t id = 0; id < NUM_SCSIID; id++) {
+      showTarget(1, 1, id);
+    }
+    return;
+  }
+  if(!strncmp(local_prefix, "/tgts/tgt", 9)) {
+    char *tgt_name = local_prefix + 9;
+    char *lun_name = NULL;
+    uint8_t id = strtol(tgt_name, &lun_name, 0);
+    if(id < NUM_SCSIID) {
+      if(!lun_name || (lun_name[0] == 0) || !strcmp(lun_name, "/")) {
+        showTarget(0, 0, id);
+        return;
+      } else if(!strncmp("/lun", lun_name, 4)) {
+        lun_name += 4;
+        uint8_t lun = strtol(lun_name, &suffix, 0);
+        if((lun < NUM_SCSILUN) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+          showLUN(0, 0, id, lun);
+          return;
+        }
+      }
+    }
+  }
+  
+  if(!strcmp(local_prefix, "/vdevs")) {
+    sprintf(tmp_right, " [%d Storage Objects]", m_vdevcnt);
+    printDirectory(0, 0, "/vdevs/ ", tmp_right);
+    for(v = 0; v < NUM_VDEV; v++) {
+      showVDEV(1, 1, v);
+    }
+    return;
+  }
+  if(!strncmp(local_prefix, "/vdevs/vdev", 11)) {
+    char *vdev_name = local_prefix + 11;
+    v = strtol(vdev_name, &suffix, 0);
+    if((v < NUM_VDEV) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+      showVDEV(0, 0, v);
+      return;
+    }
+  }
+  if(!strcmp(local_prefix, "/sd") || !strncmp(local_prefix, "/sd/", 4)) {
+    char name[MAX_FILE_PATH+1];
+    SdFile root;
+    SdFile file;
+    if(!strcmp(local_prefix, "/sd"))
+      strcat(local_prefix, "/");
+    root.open(local_prefix+3);
+    if(!root.isDir()) {
+      sprintf(tmp_left, "%s ", local_prefix);
+      if(root.fileSize() >= 1000000) {
+        sprintf(tmp_right, " [%llu GB]", root.fileSize() / 1000000);
+      } else if(file.fileSize() >= 10000) {
+        sprintf(tmp_right, " [%llu MB]", root.fileSize() / 1000);
+      } else {
+        sprintf(tmp_right, " [%llu Bytes]", root.fileSize());
+      }
+      printDirectory(0, 0, tmp_left, tmp_right);
+      root.close();
+      return;
+    }
+    sprintf(tmp_left, "%s ", local_prefix);
+    printDirectory(0, 0, tmp_left, " [...]");
+
+    while(file.openNext(&root, O_READ)) {
+      file.getName(name, MAX_FILE_PATH+1);
+      if(file.isDir()) {
+        sprintf(tmp_left, "%s/ ", name);
+        printDirectory(1, 0, tmp_left, " [...]");
+      } else {
+        sprintf(tmp_left, "%s ", name);
+        if(file.fileSize() >= 1000000) {
+          sprintf(tmp_right, " [%llu GB]", file.fileSize() / 1000000);
+        } else if(file.fileSize() >= 10000) {
+          sprintf(tmp_right, " [%llu MB]", file.fileSize() / 1000);
+        } else {
+          sprintf(tmp_right, " [%llu Bytes]", file.fileSize());
+        }
+        printDirectory(1, 0, tmp_left, tmp_right);
+      }
+      file.close();
+    }
+    root.close();
+    return;
+  }
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.printf(": Path '%s' not found.\r\n", local_prefix);
+}
+
+void unlinkcmd(int argc, char **argv) {
+  FsFile file;            /* File object */
+  char tmp_path[MAX_FILE_PATH+1];
+  char *vdev_name = NULL;
+  char *tgt_name = NULL;
+  char *lun_name = NULL;
+  char *suffix = NULL;
+  uint8_t v = 0xff;
+  uint8_t id = 0xff;
+  uint8_t lun = 0xff;
+  VirtualDevice_t *h = NULL;
+  
+  if(argc < 2) {
+    return;
+  }
+
+  fixupPath(tmp_path, argv[1]);
+
+  if(!strncmp("/tgts/tgt", tmp_path, 9)) {
+    tgt_name = tmp_path + 9;
+    id = strtol(tgt_name, &lun_name, 0);
+    if(id < NUM_SCSIID) {
+      if((lun_name) && !strncmp("/lun", lun_name, 4)) {
+        // Disable a specific LUN
+        lun_name += 4;
+        lun = strtol(lun_name, &suffix, 0);
+        if((lun < NUM_SCSILUN) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+          v = m_vdevmap[id][lun];
+          if(v < NUM_VDEV) {
+            h = &m_vdev[v];
+            h->m_id = 0xff;
+            h->m_lun = 0xff;
+            m_vdevmap[id][lun] = 0xff;
+            return;
+          }
+        }
+      } else if(!lun_name || (lun_name[0] == 0) || !strcmp(lun_name, "/")) {
+        // Disable this target and all LUNs under it
+        scsi_id_mask &= ~(1 << id);
+        for(lun = 0; lun < NUM_SCSILUN; lun++) {
+          v = m_vdevmap[id][lun];
+          if(v < NUM_VDEV) {
+            h = &m_vdev[v];
+            h->m_id = 0xff;
+            h->m_lun = 0xff;
+            m_vdevmap[id][lun] = 0xff;
+          }
+        }
+        return;
+      }
+    }
+  }
+
+  if(!strncmp(tmp_path, "/sd/", 4)) {
+    sd.remove(tmp_path+3);
+    return;
+  }
+
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.printf(": Path '%s' not found.\r\n", tmp_path);
+}
+
+void catcmd(int argc, char **argv) {
+  FsFile file;            /* File object */
+  char tmp_path[MAX_FILE_PATH+1];
+  char tmp_str[256];
+  char *filename = tmp_path;
+  
+  if(argc < 2) {
+    return;
+  }
+
+  fixupPath(tmp_path, argv[1]);
+  if(!strncmp(filename, "/sd/", 4)) {
+    filename += 3;
+  } else {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": file '%s' not found.\r\n", tmp_path);
+    return;
+  }
+
+  if(!sd.exists(filename)) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": file '%s' not found.\r\n", tmp_path);
+    return;
+  }
+
+  if(!file.open(filename, O_RDONLY)) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": Unable to open '%s'.\r\n", tmp_path);
+  } else {
+    while(file.available()) {
+      int n = file.fgets(tmp_str, sizeof(tmp_str)-1, NULL);
+      if (n <= 0) {
+        Serial.print("ERROR");
+        if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+        Serial.printf(": Reading '%s' failed.\r\n", tmp_path);
+        
+        break;
+      }
+
+      Serial.print(tmp_str);
+    }
+    file.close();
+  }
+}
+
+void enablecmd(int argc, char **argv) {
+  uint8_t id = 0xff;
+  char *tgt_name = NULL;
+  char *suffix;
+  char tmp_path[MAX_FILE_PATH+1];
+
+  // The logic here is slightly complicated to allow specifying enable /tgts/tgtN from anywhere, but otherwise require we are in /tgts
+  if((argc<2) && strncmp("/tgts", cmd_prefix, 5)) {
+    goto e_invalidpath;
+  }
+
+  if((argc<2) && !strcmp("/tgts", cmd_prefix)) {
+    goto e_notarget;
+  }
+
+  if(argc<2) {
+    strcpy(tmp_path, cmd_prefix);
+  } else {
+    fixupPath(tmp_path, argv[1]);
+  }
+
+  if(strncmp("/tgts/", tmp_path, 6)) {
+    goto e_invalidpath;
+  }
+  if(!strncmp("/tgts/tgt", tmp_path, 9)) {
+    tgt_name = tmp_path + 9;
+    id = strtol(tgt_name, &suffix, 0);
+  }
+
+  if(id < NUM_SCSIID) {
+    scsi_id_mask |= (1<<id);
+    return;
+  }
+
+e_invalidtarget:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": Invalid target specified.\r\n");
+  return;
+
+e_invalidpath:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": The enable command is not valid in this context.\r\n");
+  return;
+
+e_notarget:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": No target specified.\r\n");
+  return;
+}
+
+void setcmd(int argc, char **argv) {
+  uint8_t v = 0xff;
+  char *vdev_name = NULL;
+  char *param_name = NULL;
+
+  char tmp_path[MAX_FILE_PATH+1];
+
+  if(argc<2) {
+    goto e_syntax;
+  }
+
+  fixupPath(tmp_path, argv[1]);
+
+  // Allow specifying set /vdevs/vdevN/parameter from anywhere, but otherwise require we are in /vdevs/vdevN
+  if(!strncmp("/vdevs/vdev", tmp_path, 11)) {
+    vdev_name = tmp_path + 11;
+    v = strtol(vdev_name, &param_name, 0);
+
+    if(v < NUM_VDEV) {
+      VirtualDevice_t *h = &m_vdev[v];
+
+      if((param_name) && !strcasecmp(param_name, "/blocksize")) {
+        if(argc<3) {
+          Serial.printf("%d\r\n", h->m_blocksize);
+        } else {
+          h->m_blocksize = strtol(argv[2], NULL, 0);
+        }
+        return;
+      }
+
+      goto e_invalidparam;
+    }
+    goto e_invalidpath;
+  }
+
+e_invalidpath:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": The set command is not valid in this context.\r\n");
+  return;
+
+e_invalidparam:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.printf(": Invalid parameter '%s' specified.\r\n", param_name);
+  return;
+
+e_syntax:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": Command syntax violation.\r\n");
+  return;
+}
+
+void createcmd(int argc, char **argv) {
+  if(strncmp("/vdev", cmd_prefix, 5)) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.print(": The create command is not valid in this context.\r\n");
+    return;
+  }
+
+  if(m_vdevcnt >= NUM_VDEV) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.print(": Virtual Device pool exhausted.\r\n");
+    return;
+  }
+
+  if(argc<2) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.print(": No device type specified.\r\n");
+    return;
+  }
+
+  uint8_t v = m_vdevcnt;
+  VirtualDevice_t *h = &m_vdev[v];
+
+#if SUPPORT_DISK
+  if(!strcasecmp(argv[1], "disk")) {
+    ConfigureDisk(h, NULL);
+    goto success;
+  }
+#endif /* SUPPORT_DISK */
+#if SUPPORT_OPTICAL
+  if(!strcasecmp(argv[1], "optical") || !strcasecmp(argv[1], "cdrom")) {
+    ConfigureOptical(h, NULL);
+    goto success;
+  }
+#endif /* SUPPORT_OPTICAL */
+#if SUPPORT_TAPE
+  if(!strcasecmp(argv[1], "tape")) {
+    ConfigureTape(h, NULL);
+    goto success;
+  }
+#endif /* SUPPORT_TAPE */
+
+failure:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": Unknown device type specified.\r\n");
+  return;
+
+success:
+  sprintf(cmd_prefix, "/vdevs/vdev%d", v);
+  h->m_enabled = true;
+  h->m_id = 0xff;
+  h->m_lun = 0xff;
+  m_vdevcnt++;
+}
+
+void mountcmd(int argc, char **argv) {
+  char tmp_path[MAX_FILE_PATH+1];
+  char *vdev_name = NULL;
+  VirtualDevice_t *h;
+  uint8_t v = 0xff;
+  
+  if(!strncmp("/vdevs/vdev", cmd_prefix, 11)) {
+    vdev_name = cmd_prefix + 11;
+    v = strtol(vdev_name, NULL, 0);
+  }
+
+  if(v < NUM_VDEV) {
+    h = &m_vdev[v];
+    if(h->m_blocksize == 0) {
+      if(h->m_type == DEV_TAPE) {
+        h->m_blocksize = 1024;
+      } else {
+        h->m_blocksize = 512;
+      }
+    }
+  } else if(argc>=2) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.print(": The mount command is not valid in this context.\r\n");
+    return;
+  } else {
+    // TODO: Print out all mounted images
+    return;
+  }
+
+  if(argc<2) {
+    // TODO: Print out the mounted image
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.print(": No backingstore specified.\r\n");
+    return;
+  }
+
+  fixupPath(tmp_path, argv[1]);
+
+  if(!strncmp(tmp_path, "/sd/", 4)) {
+    if(OpenDiskImage(h, tmp_path, 512)) {
+      strcpy(h->m_filename, tmp_path);
+      h->m_enabled = true;
+      
+      return;
+    }
+  }
+
+  h->m_fileSize = 0;
+  h->m_blocksize = 0;
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": Unsupported backingstore path.\r\n");
+  return;
+}
+
+
+void mapcmd(int argc, char **argv) {
+  char tmp_path[MAX_FILE_PATH+1];
+  char *vdev_name = NULL;
+  char *tgt_name = NULL;
+  char *lun_name = NULL;
+  char *suffix = NULL;
+  VirtualDevice_t *h;
+  uint8_t v = 0xff;
+  uint8_t id = 0xff;
+  uint8_t lun = 0xff;
+  
+  if(!strncmp("/vdevs/vdev", cmd_prefix, 11)) {
+    vdev_name = cmd_prefix + 11;
+    v = strtol(vdev_name, &suffix, 0);
+
+    if((v < NUM_VDEV) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+      h = &m_vdev[v];
+      if(argc>=2) {
+        fixupPath(tmp_path, argv[1]);
+
+        if(!strncmp("/tgts/tgt", tmp_path, 9)) {
+          tgt_name = tmp_path + 9;
+          id = strtol(tgt_name, &lun_name, 0);
+          if((id < NUM_SCSIID) && (lun_name)) {
+            if(!strncmp("/lun", lun_name, 4)) {
+              lun_name += 4;
+              lun = strtol(lun_name, &suffix, 0);
+              if((lun < NUM_SCSILUN) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+                h->m_id = id;
+                h->m_lun = lun;
+                m_vdevmap[id][lun] = v;
+                return;
+              }
+            }
+          }
+        }
+        goto e_invalidtarget;
+      } else {
+        sprintf(tmp_left, "/vdevs/vdev%d/$mapping ", v);
+        sprintf(tmp_right, " [/tgts/tgt%d/lun%d]\r\n", h->m_id, h->m_lun);
+        printDirectory(0, 0, tmp_left, tmp_right);
+        return;
+      }
+    }
+  }
+  if(!strncmp("/tgts/tgt", cmd_prefix, 9)) {
+    tgt_name = cmd_prefix + 9;
+    id = strtol(tgt_name, &lun_name, 0);
+    if((id < NUM_SCSIID) && (lun_name)) {
+      if(!strncmp("/lun", lun_name, 4)) {
+        lun_name += 4;
+        lun = strtol(lun_name, &suffix, 0);
+        if((lun < NUM_SCSILUN) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+          if(argc>=2) {
+            fixupPath(tmp_path, argv[1]);
+            
+            if(!strncmp("/vdevs/vdev", tmp_path, 11)) {
+              vdev_name = cmd_prefix + 11;
+              v = strtol(vdev_name, &suffix, 0);
+          
+              if((v < NUM_VDEV) && (!suffix || (suffix[0] == 0) || !strcmp(suffix, "/"))) {
+                h = &m_vdev[v];
+                h->m_id = id;
+                h->m_lun = lun;
+                m_vdevmap[id][lun] = v;
+                return;
+              }
+            }
+            goto e_invalidtarget;
+          } else {
+            v = m_vdevmap[id][lun];
+            if(v < NUM_VDEV) {
+              sprintf(tmp_left, "/tgts/tgt%d/lun%d/$mapping ", id, lun);
+              sprintf(tmp_right, " [/vdevs/vdev%d]\r\n", v);
+              printDirectory(0, 0, tmp_left, tmp_right);
+            }
+            return;
+          }
+        }
+      }
+    }
+  }
+
+e_invalidcontext:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.print(": The map command is not valid in this context.\r\n");
+  return;
+
+e_invalidtarget:
+  Serial.print("ERROR");
+  if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+  Serial.printf(": Invalid target '%s' specified.\r\n", tmp_path);
+  return;
+}
+
+void saveconfig(int argc, char **argv) {
+  FsFile config_file;
+
+  config_file = sd.open("/greenscsi.cfg", O_WRONLY | O_CREAT | O_TRUNC);
+  if(!config_file.isOpen()) {
+    Serial.print("ERROR");
+    if(exec_file[0]) Serial.printf(" on line %d of '%s'", exec_line, exec_file);
+    Serial.printf(": Unable to open 'greenscsi.cfg' for writing.\r\n");
+    return;
+  }
+  config_file.print( "@echo off\r\n\r\n");
+
+  if(scsi_id_mask != 0) {
+    config_file.print( "cd /tgts\r\n");
+    for(uint8_t id = 0; id < NUM_SCSIID; id++) {
+      if(scsi_id_mask & (1<<id)) {
+        config_file.printf("enable tgt%d\r\n", id);
+      }
+    }
+  }
+  config_file.print( "\r\n");
+
+  config_file.print( "cd /vdevs\r\n");
+  for(uint8_t id = 0; id < NUM_SCSIID; id++) {
+    for(uint8_t lun = 0; lun < NUM_SCSILUN; lun++) {
+      uint8_t v = m_vdevmap[id][lun];
+      if(v < NUM_VDEV) {
+        VirtualDevice_t *h = &m_vdev[v];
+        if(h->m_enabled) {
+          // The create command automatically changes directory to the created object
+          switch(h->m_type) {
+            case DEV_DISK:
+              config_file.print("create disk\r\n");
+              break;
+            case DEV_OPTICAL:
+              config_file.print("create optical\r\n");
+              break;
+            case DEV_TAPE:
+              config_file.print("create tape\r\n");
+              break;
+            default:
+              config_file.printf("create 0x%02x\r\n", h->m_type);
+              break;
+          }
+          if(h->m_file) {
+            config_file.printf("mount %s\r\n", h->m_filename);
+          }
+          if(h->m_blocksize) {
+            config_file.printf("set blocksize %d\r\n", h->m_blocksize);
+          }
+    
+          // The map command should be last, as it will escape back to /vdevs
+          if((h->m_id < NUM_SCSIID) && (h->m_lun < NUM_SCSILUN)) {
+            config_file.printf("map /tgts/tgt%d/lun%d\r\n", h->m_id, h->m_lun);
+          } else {
+            config_file.print("cd ..\r\n");
+          }
+          config_file.print( "\r\n");
+        }
+      }
+    }
+  }
+  config_file.close();
+}
+
+char helponSave[] =
+  "\r\n"
+  "saveconfig\r\n"
+  "\r\n"
+  "  Generate a new greenscsi.cfg file containing currently configured parameters.\r\n"
+  "Any existing greenscsi.cfg file will be overwritten and its previous contents lost.\r\n"
+  "\r\n"
+;
+
+char helponSet[] =
+  "\r\n"
+  "set <key> <value>\r\n"
+  "\r\n"
+  "  Use the set command to configure vdev parameters.\r\n"
+  "\r\n"
+;
+
+char helponCreate[] =
+  "\r\n"
+  "create <type>\r\n\r\n"
+  "  The create command allocates and changes the path to a newly created vdev.\r\n"
+  "Recognized values for type:"
+#if SUPPORT_DISK
+  " disk"
+#endif
+#if SUPPORT_OPTICAL
+  " optical"
+#endif
+#if SUPPORT_TAPE
+  " tape"
+#endif
+  ".\r\n"
+  "You can also specify the SCSI device code value directly.\r\n"
+  "\r\n"
+;
+
+char helponEcho[] =
+  "\r\n"
+  "echo [on|off|<message>]\r\n"
+  "\r\n"
+  "  The echo command allows you to turn on or off the echoing of script commands to the console.\r\n"
+  "Additionally, you can use the echo command to relay a message to the console.\r\n"
+  "\r\n"
+;
+
+char helponCat[] =
+  "\r\n"
+  "cat <file>\r\n"
+  "\r\n"
+  "  The cat command dumps files to the console for reading.\r\n"
+  "\r\n"
+;
+
+char helponUnlink[] =
+  "\r\n"
+  "unlink <path>\r\n"
+  "\r\n"
+  "  The unlink command removes objects such as files or vdevs.\r\n"
+  "\r\n"
+;
+
+char helponExec[] =
+  "\r\n"
+  "exec <script>\r\n"
+  "\r\n"
+  "  The exec command executes command script files.\r\n"
+  "At startup, greenscsi.cfg will be executed from the sd card if found.\r\n"
+  "\r\n"
+;
+
+char helponEnable[] =
+  "\r\n"
+  "enable tgt<n>\r\n"
+  "\r\n"
+  "  The enable command sets a SCSI device ID to be handled by GreenSCSI.\r\n"
+  "\r\n"
+;
+
+char helponMapT[] =
+  "\r\n"
+  "map <vdev>\r\n"
+  "\r\n"
+  "  The map command associates a given vdev with this lun.\r\n"
+  "\r\n"
+;
+
+char helponMapV[] =
+  "\r\n"
+  "map <lun>\r\n"
+  "\r\n"
+  "  The map command associates a given lun with this vdev.\r\n"
+  "\r\n"
+;
+
+char helponMount[] =
+  "\r\n"
+  "mount <path>\r\n"
+  "\r\n"
+  "  The mount command associates a given image or block device with this vdev.\r\n"
+  "\r\n"
+;
+
+char helponHelp[] =
+  "\r\n"
+  "help\r\n"
+  "\r\n"
+  "  This help facility will show you the commands available within the current path.\r\n"
+  "Some commands may have longer help articles such as this one to describe their parameters fully.\r\n"
+  "\r\n"
+;
+
+Commands_t GlobalCommands[] = {
+  // Command      Valid From Path   Req. Params    Short Help                    Long Help       Handler           Dispatch
+  { "cd",         "/",              1,             "change current directory",   NULL,           changeDirectory,  NULL },
+  { "ls",         "/",              0,             "display directory contents", NULL,           showDirectory,    NULL },
+  { "saveconfig", "/",              0,             "write greenscsi.cfg file",   helponSave,     saveconfig,       NULL },
+  { "enable",     "/tgts",          1,             "<target>",                   helponEnable,   enablecmd,        NULL },
+  { "map",        "/tgts/tgt",      1,             "<vdev>",                     helponMapT,     mapcmd,           NULL },
+  { "create",     "/vdevs",         1,             "<type>",                     helponCreate,   createcmd,        NULL },
+  { "set",        "/vdevs/vdev",    2,             "<key> <value>",              helponSet,      setcmd,           NULL },
+  { "mount",      "/vdevs/vdev",    1,             "<path>",                     helponMount,    mountcmd,         NULL },
+  { "map",        "/vdevs/vdev",    1,             "<lun>",                      helponMapV,     mapcmd,           NULL },
+  { "cat",        "/sd",            1,             "<file>",                     helponCat,      catcmd,           NULL },
+  { "unlink",     "/",              1,             "<path>",                     helponUnlink,   unlinkcmd,        NULL },
+  { "exec",       "/",              1,             "<script>",                   helponExec,     execcmd,          NULL },
+  { "echo",       "/",              0,             "[on|off|<message>]",         helponEcho,     echocmd,          NULL },
+  { "help",       "/",              0,             "This help facility.",        helponHelp,     help,             NULL },
+  { NULL,         NULL,             0,             NULL,                         NULL,           NULL,             NULL }
+};
diff --git a/src/config.h b/src/config.h
new file mode 100755
index 0000000..5aec68b
--- /dev/null
+++ b/src/config.h
@@ -0,0 +1,35 @@
+#ifndef __CONFIG_H
+#define __CONFIG_H
+
+#define DEBUG                  0          // 0:No debug information output (Faster by 2-3x+)
+                                          // 1: Debug information output to USB Serial
+                                          // 2: Debug information output to LOG.txt (slow)
+
+#define SYNCHRO                false      // Support Synchronous mode.
+#define READ_SPEED_OPTIMIZE    true       // 
+#define WRITE_SPEED_OPTIMIZE   true       // 
+#define USE_DB2ID_TABLE        true       // Use table to get ID from SEL-DB
+
+// SCSI config
+#define NUM_SCSIID             8          // Maximum number of supported SCSI-IDs (The minimum is 1)
+#define NUM_SCSILUN            8          // Maximum number of LUNs supported     (The minimum is 1)
+#define NUM_VDEV               8          // Maximum number of VDEVs supported    (The minimum is 1)
+
+#define READ_PARITY_CHECK      0          // Perform read parity check (unverified)
+
+// HDD format
+#define MAX_BLOCKSIZE          (1 << 15)  // Maximum BLOCK size (2048 to 8192 tested, 16384 had issues)
+
+
+// Supported Device Types
+#define SUPPORT_DISK           true
+#define SUPPORT_OPTICAL        true
+#define SUPPORT_TAPE           false
+
+
+// Compatibility Settings
+#define SUPPORT_SASI           false      // Enable SASI compatiblity for Sharp X68000
+#define SUPPORT_SASI_DEFAULT   false      // Turn it on by default
+
+
+#endif /* __CONFIG_H */
diff --git a/src/defaults.ino b/src/defaults.ino
new file mode 100755
index 0000000..e286a3f
--- /dev/null
+++ b/src/defaults.ino
@@ -0,0 +1,75 @@
+#include "config.h"
+
+// SCSI Drive Vendor information
+#if SUPPORT_DISK
+static uint8_t SCSI_DISK_INQUIRY_RESPONSE[96] = {
+  0x00, //device type
+  0x00, //RMB = 0
+  0x05, //ISO, ECMA, ANSI version
+  0x02, //Response data format
+  35 - 4, //Additional data length
+  0, 0, //Reserve
+  0x00, //Support function
+  'Q', 'U', 'A', 'N', 'T', 'U', 'M', ' ', // vendor 8
+  'F', 'I', 'R', 'E', 'B', 'A', 'L', 'L', '1', ' ', ' ',' ', ' ', ' ', ' ', ' ', // product 16
+  '1', '.', '0', ' ', // version 4
+  // Release Number (1 Byte)
+  0x20,
+  // Revision Date (10 Bytes)
+  '2','0','2','1','/','1','1','/','2','2',
+  0
+};
+#endif /* SUPPORT_DISK */
+
+#if SUPPORT_SASI
+static uint8_t SCSI_NEC_INQUIRY_RESPONSE[96] = {
+  0x00, //Device type
+  0x00, //RMB = 0
+  0x01, //ISO,ECMA,ANSI version
+  0x01, //Response data format
+  35 - 4, //Additional data length
+  0, 0, //Reserve
+  0x00, //Support function
+  'N', 'E', 'C', 'I', 'T', 'S', 'U', ' ',
+  'A', 'r', 'd', 'S', 'C', 'S', 'i', 'n', 'o', ' ', ' ',' ', ' ', ' ', ' ', ' ',
+  '0', '0', '1', '0',
+  0
+};
+#endif /* SUPPORT_SASI */
+
+#if SUPPORT_OPTICAL
+static uint8_t SCSI_CDROM_INQUIRY_RESPONSE[96] = {
+  0x05, //device type
+  0x80, //RMB = 0
+  0x05, //ISO, ECMA, ANSI version
+  0x02, //Response data format
+  46 - 4, //Additional data length
+  0, 0, //Reserve
+  0x00, //Support function
+  'S', 'O', 'N', 'Y', ' ', ' ', ' ', ' ', // vendor 8
+  'C', 'D', '-', 'R', 'O', 'M', ' ', 'C', 'D', 'U', '-','8', '0', '0', '3', ' ', // product 16
+  '1', '.', '7', 'w', // version 4
+  // Release Number (1 Byte)
+  0x20,
+  // Revision Date (10 Bytes)
+  '2','0','2','1','/','1','1','/','2','2',
+  0
+};
+#endif /* SUPPORT_OPTICAL */
+
+#if SUPPORT_TAPE
+static uint8_t SCSI_TAPE_INQUIRY_RESPONSE[96] = {
+  0x01, //device type
+  0x80, //RMB = 0
+  0x03, //ISO, ECMA, ANSI version
+  0x02, //Response data format
+  37 - 4, //Additional data length
+  0, //Reserve
+  0x00, // Support function
+  0x00, // Support function
+  'I', 'B', 'M', ' ', ' ', ' ', ' ', ' ', // vendor 8
+  'U', 'L', 'T', '3', '5', '8', '0', '-', 'T', 'D', '1',' ', ' ', ' ', ' ', ' ', // product 16
+  'M', 'B', 'N', '0', // version 4
+  0
+};
+#endif /* SUPPORT_TAPE */
diff --git a/src/disk.ino b/src/disk.ino
index 5e3c38e..317e515 100755
--- a/src/disk.ino
+++ b/src/disk.ino
@@ -1,3 +1,4 @@
+#include "config.h"
 #include "scsi_defs.h"
 
 void Read6CommandHandler() {
@@ -22,14 +23,14 @@ void Seek6CommandHandler() {
  */
 void ReadCapacityCommandHandler() {
   LOGN("[ReadCapacity]");
-  if(!m_img) {
+  if(!m_sel) {
     m_sts |= 0x02; // Image file absent
     m_phase = PHASE_STATUSIN;
     return;
   }
 
-  uint32_t bl = m_img->m_blocksize;
-  uint32_t bc = m_img->m_fileSize / bl;
+  uint32_t bl = m_sel->m_blocksize;
+  uint32_t bc = m_sel->m_fileSize / bl;
   uint8_t buf[8] = {
     (uint8_t)(((uint32_t)(bc >> 24))&0xff), (uint8_t)(((uint32_t)(bc >> 16))&0xff), (uint8_t)(((uint32_t)(bc >> 8))&0xff), (uint8_t)(((uint32_t)(bc))&0xff),
     (uint8_t)(((uint32_t)(bl >> 24))&0xff), (uint8_t)(((uint32_t)(bl >> 16))&0xff), (uint8_t)(((uint32_t)(bl >> 8))&0xff), (uint8_t)(((uint32_t)(bl))&0xff)
@@ -65,7 +66,7 @@ uint8_t onReadCommand(uint32_t adds, uint32_t len)
   LOG(" ");
   LOGHEX4N(len);
 
-  if(!m_img) return 0x02; // Image file absent
+  if(!m_sel) return 0x02; // Image file absent
   
   LED_ON();
   writeDataPhaseSD(adds, len);
@@ -83,7 +84,7 @@ uint8_t onWriteCommand(uint32_t adds, uint32_t len)
   LOG(" ");
   LOGHEX4N(len);
   
-  if(!m_img) return 0x02; // Image file absent
+  if(!m_sel) return 0x02; // Image file absent
   
   LED_ON();
   readDataPhaseSD(adds, len);
@@ -91,30 +92,76 @@ uint8_t onWriteCommand(uint32_t adds, uint32_t len)
   return 0; //sts
 }
 
-void ConfigureDiskHandlers(int id) {
+void ConfigureDiskHandlers(VirtualDevice_t *vdev) {
   for(int c = 0; c < 256; c++)
-    m_handler[id][c] = &UnknownCommandHandler;
+    vdev->m_handler[c] = &UnknownCommandHandler;
 
-  m_handler[id][CMD_TEST_UNIT_READY] = &TestUnitCommandHandler;
-  m_handler[id][CMD_REZERO_UNIT]     = &RezeroUnitCommandHandler;
-  m_handler[id][CMD_REQUEST_SENSE]   = &RequestSenseCommandHandler;
-  m_handler[id][CMD_FORMAT_UNIT]     = &FormatUnitCommandHandler;
-  m_handler[id][0x06] = &FormatUnitCommandHandler;
-  m_handler[id][0x07] = &ReassignBlocksCommandHandler;
-  m_handler[id][CMD_READ6]           = &Read6CommandHandler;
-  m_handler[id][CMD_WRITE6]          = &Write6CommandHandler;
-  m_handler[id][CMD_SEEK6]           = &Seek6CommandHandler;
-  m_handler[id][CMD_INQUIRY]         = &InquiryCommandHandler;
-  m_handler[id][CMD_MODE_SELECT6]                  = &ModeSelect6CommandHandler;
-  m_handler[id][CMD_MODE_SENSE6]     = &ModeSense6CommandHandler;
-  m_handler[id][CMD_START_STOP_UNIT] = &StartStopUnitCommandHandler;
-  m_handler[id][CMD_PREVENT_REMOVAL] = &PreAllowMediumRemovalCommandHandler;
-  m_handler[id][CMD_READ_CAPACITY10] = &ReadCapacityCommandHandler;
-  m_handler[id][CMD_READ10]          = &Read10CommandHandler;
-  m_handler[id][CMD_WRITE10]         = &Write10CommandHandler;
-  m_handler[id][CMD_SEEK10]          = &Seek10CommandHandler;
-  m_handler[id][CMD_MODE_SENSE10]    = &ModeSense10CommandHandler;
-#if SCSI_SELECT == 1
-  m_handler[id][0xC2]                = &DTCsetDriveParameterCommandHandler;
+  vdev->m_handler[CMD_TEST_UNIT_READY] = &TestUnitCommandHandler;
+  vdev->m_handler[CMD_REZERO_UNIT]     = &RezeroUnitCommandHandler;
+  vdev->m_handler[CMD_REQUEST_SENSE]   = &RequestSenseCommandHandler;
+  vdev->m_handler[CMD_FORMAT_UNIT]     = &FormatUnitCommandHandler;
+  vdev->m_handler[CMD_FORMAT_UNIT_ALT] = &FormatUnitCommandHandler;
+  vdev->m_handler[CMD_REASSIGN_BLOCKS] = &ReassignBlocksCommandHandler;
+  vdev->m_handler[CMD_READ6]           = &Read6CommandHandler;
+  vdev->m_handler[CMD_WRITE6]          = &Write6CommandHandler;
+  vdev->m_handler[CMD_SEEK6]           = &Seek6CommandHandler;
+  vdev->m_handler[CMD_INQUIRY]         = &InquiryCommandHandler;
+  vdev->m_handler[CMD_MODE_SELECT6]    = &ModeSelect6CommandHandler;
+  vdev->m_handler[CMD_MODE_SENSE6]     = &ModeSenseCommandHandler;
+  vdev->m_handler[CMD_START_STOP_UNIT] = &StartStopUnitCommandHandler;
+  vdev->m_handler[CMD_PREVENT_REMOVAL] = &PreAllowMediumRemovalCommandHandler;
+  vdev->m_handler[CMD_READ_CAPACITY10] = &ReadCapacityCommandHandler;
+  vdev->m_handler[CMD_READ10]          = &Read10CommandHandler;
+  vdev->m_handler[CMD_WRITE10]         = &Write10CommandHandler;
+  vdev->m_handler[CMD_SEEK10]          = &Seek10CommandHandler;
+  vdev->m_handler[CMD_MODE_SENSE10]    = &ModeSenseCommandHandler;
+#if SUPPORT_SASI
+  if(m_sasi_mode)
+    vdev->m_handler[CMD_SET_DRIVE_PARAMETER] = &DTCsetDriveParameterCommandHandler;
 #endif    
 }
+
+// If config file exists, read the first three lines and copy the contents.
+// File must be well formed or you will get junk in the SCSI Vendor fields.
+void ConfigureDisk(VirtualDevice_t *vdev, const char *image_name) {
+  memcpy(vdev->m_inquiryresponse, SCSI_DISK_INQUIRY_RESPONSE, sizeof(SCSI_DISK_INQUIRY_RESPONSE));
+
+  if(image_name) {
+    char configname[MAX_FILE_PATH+1];
+    memcpy(configname, image_name, MAX_FILE_PATH+1);
+    char *psuffix = strstr(configname, ".img");
+    if(psuffix) {
+      strcpy(psuffix, ".cfg");
+    } else {
+      sprintf(configname, "hd%d%d.cfg", vdev->m_id, vdev->m_lun);
+    }
+  
+    FsFile config_file = sd.open(configname, O_RDONLY);
+    if (config_file.isOpen()) {
+      char vendor[9];
+      memset(vendor, 0, sizeof(vendor));
+      config_file.readBytes(vendor, sizeof(vendor));
+      LOGN("SCSI VENDOR: ");
+      LOGN(vendor);
+      memcpy(&(vdev->m_inquiryresponse[8]), vendor, 8);
+    
+      char product[17];
+      memset(product, 0, sizeof(product));
+      config_file.readBytes(product, sizeof(product));
+      LOGN("SCSI PRODUCT: ");
+      LOGN(product);
+      memcpy(&(vdev->m_inquiryresponse[16]), product, 16);
+    
+      char version[5];
+      memset(version, 0, sizeof(version));
+      config_file.readBytes(version, sizeof(version));
+      LOGN("SCSI VERSION: ");
+      LOGN(version);
+      memcpy(&(vdev->m_inquiryresponse[32]), version, 4);
+      config_file.close();
+    }
+  }
+
+  vdev->m_type = DEV_DISK;
+  ConfigureDiskHandlers(vdev);
+}
diff --git a/src/general.ino b/src/general.ino
index e6133c1..da916f2 100755
--- a/src/general.ino
+++ b/src/general.ino
@@ -1,3 +1,4 @@
+#include "config.h"
 #include "scsi_defs.h"
 
 
@@ -6,13 +7,20 @@
  */
 void InquiryCommandHandler() {
   LOGN("[Inquiry]");
-  uint8_t len = m_cmd[4];
-  uint8_t buf[36];
-  memcpy(buf, m_inquiryresponse[m_id], 36);
+  uint8_t len;
 
-  if(!m_img) buf[0] = 0x7f;
+  if(!m_sel) {
+    memset(m_responsebuffer, 0, 96);
+    m_responsebuffer[0] = 0x7f;
+    m_responsebuffer[4] = 35 - 4;
+    len = 36;
+  } else {
+    memcpy(m_responsebuffer, m_sel->m_inquiryresponse, 96);
+    len = m_responsebuffer[4] + 5;
+  }
 
-  writeDataPhase(len < 36 ? len : 36, buf);
+  len = min(len, m_cmd[4]);
+  writeDataPhase(len, m_responsebuffer);
   m_phase = PHASE_STATUSIN;
 }
 
@@ -31,13 +39,13 @@ void RequestSenseCommandHandler() {
     0,
   };
 
-  if(!m_img) {
+  if(!m_sel) {
     // Image file absent
     buf[2] = 0x02; // NOT_READY
     buf[12] = 0x25; // Logical Unit Not Supported
   } else {
-    buf[2] = m_sense[m_id][m_lun].m_key;
-    m_sense[m_id][m_lun].m_key = 0;
+    buf[2] = m_sel->m_sense.m_key;
+    m_sel->m_sense.m_key = 0;
   }
   writeDataPhase(len < 18 ? len : 18, buf);  
   m_phase = PHASE_STATUSIN;
@@ -45,10 +53,10 @@ void RequestSenseCommandHandler() {
 
 void TestUnitCommandHandler() {
   LOGN("[TestUnit]");
-  if(!m_img) {
+  if(!m_sel) {
     // Image file absent
-    m_sense[m_id][m_lun].m_key = NOT_READY; // NOT_READY
-    m_sense[m_id][m_lun].m_code = NO_MEDIA; // Logical Unit Not Supported
+    m_sel->m_sense.m_key = NOT_READY; // NOT_READY
+    m_sel->m_sense.m_code = NO_MEDIA; // Logical Unit Not Supported
   }
   m_phase = PHASE_STATUSIN;
 }
@@ -68,28 +76,295 @@ void ReassignBlocksCommandHandler() {
   m_phase = PHASE_STATUSIN;
 }
 
-void ModeSense6CommandHandler() {
-  LOGN("[ModeSense6]");
-  m_sts |= onModeSenseCommand(m_cmd[1]&0x80, m_cmd[2], m_cmd[4]);
+/*
+ * MODE SENSE command processing.
+ */
+void ModeSenseCommandHandler()
+{
+  uint16_t len, maxlen;
+  int page, pagemax, pagemin;
+
+  switch(m_cmd[0]) {
+    case CMD_MODE_SENSE6:
+      LOGN("[ModeSense6]");
+      maxlen = m_cmd[4];
+      len = 1;
+      break;
+    case CMD_MODE_SENSE10:
+      LOGN("[ModeSense10]");
+      maxlen = ((uint16_t)m_cmd[7] << 8) | m_cmd[8];
+      len = 2;
+      break;
+    default:
+      m_sts |= 0x02;
+      m_phase = PHASE_STATUSIN;
+      return;
+  }
+  
+  /* Check whether medium is present */
+  if(!m_sel) {
+    m_sts |= 0x02;
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+
+  memset(m_responsebuffer, 0, sizeof(m_responsebuffer));
+#if 0
+  if(m_sasi_mode) {
+    int pageCode = cmd2 & 0x3F;
+  
+    // Assuming sector size 512, number of sectors 25, number of heads 8 as default settings
+    int size = m_sel->m_fileSize;
+    int cylinders = (int)(size >> 9);
+    cylinders >>= 3;
+    cylinders /= 25;
+    int sectorsize = 512;
+    int sectors = 25;
+    int heads = 8;
+    // Sector size
+   int disksize = 0;
+    for(disksize = 16; disksize > 0; --(disksize)) {
+      if ((1 << disksize) == sectorsize)
+        break;
+    }
+    // Number of blocks
+    uint32_t diskblocks = (uint32_t)(size >> disksize);
+    int a = 4;
+    if(dbd == 0) {
+      uint32_t bl = m_sel->m_blocksize;
+      uint32_t bc = m_sel->m_fileSize / bl;
+      uint8_t c[8] = {
+        0,// Density code
+        bc >> 16, bc >> 8, bc,
+        0, //Reserve
+        bl >> 16, bl >> 8, bl
+      };
+      memcpy(&m_responsebuffer[4], c, 8);
+      a += 8;
+      m_responsebuffer[3] = 0x08;
+    }
+    switch(pageCode) {
+    case 0x3F:
+    {
+      m_responsebuffer[len + 0] = 0x01;
+      m_responsebuffer[len + 1] = 0x06;
+      a += 8;
+    }
+    case 0x03:  // drive parameters
+    {
+      m_responsebuffer[len + 0] = 0x80 | 0x03; // Page code
+      m_responsebuffer[len + 1] = 0x16; // Page length
+      m_responsebuffer[len + 2] = (uint8_t)(heads >> 8);// number of sectors / track
+      m_responsebuffer[len + 3] = (uint8_t)(heads);// number of sectors / track
+      m_responsebuffer[len + 10] = (uint8_t)(sectors >> 8);// number of sectors / track
+      m_responsebuffer[len + 11] = (uint8_t)(sectors);// number of sectors / track
+      int size = 1 << disksize;
+      m_responsebuffer[len + 12] = (uint8_t)(size >> 8);// number of sectors / track
+      m_responsebuffer[len + 13] = (uint8_t)(size);// number of sectors / track
+      a += 24;
+      if(pageCode != 0x3F) {
+        break;
+      }
+    }
+    case 0x04:  // drive parameters
+    {
+        LOGN("AddDrive");
+        m_responsebuffer[len + 0] = 0x04; // Page code
+        m_responsebuffer[len + 1] = 0x12; // Page length
+        m_responsebuffer[len + 2] = (cylinders >> 16);// Cylinder length
+        m_responsebuffer[len + 3] = (cylinders >> 8);
+        m_responsebuffer[len + 4] = cylinders;
+        m_responsebuffer[len + 5] = heads;   // Number of heads
+        a += 20;
+      if(pageCode != 0x3F) {
+        break;
+      }
+    }
+    default:
+      break;
+    }
+    m_responsebuffer[0] = a - 1;
+    writeDataPhase(len < a ? len : a, m_responsebuffer);
+  } else
+#endif
+  {
+    /* Default medium type */
+    m_responsebuffer[len++] = (m_sel->m_type == DEV_OPTICAL) ? 0xf0 : 0x00;
+    /* Write protected */
+    m_responsebuffer[len++] = (m_sel->m_type == DEV_OPTICAL) ? 0x80 : 0x00;
+
+    // ModeSense10 has two extra bytes, and Block Descriptor Length has an extra MSB
+    if(m_cmd[0] == CMD_MODE_SENSE10) {
+      len += 2;
+      m_responsebuffer[len++] = 0;
+    }
+  
+    /* Add block descriptor if DBD is not set */
+    if (m_cmd[1] & 0x08) {
+      m_responsebuffer[len++] = 0;             /* No block descriptor */
+    } else {
+      if(m_sel->m_type == DEV_TAPE) {
+        m_responsebuffer[len++] = 8;             /* Block descriptor length */
+        m_responsebuffer[len++] = 0x40;          /* Medium Density Code */
+        m_responsebuffer[len++] = 0x00;          /* Number of Blocks (0) */
+        m_responsebuffer[len++] = 0x00;
+        m_responsebuffer[len++] = 0x00;
+        m_responsebuffer[len++] = 0x00;          /* Block Length 1024 */
+        m_responsebuffer[len++] = 0x00;
+        m_responsebuffer[len++] = 0x04;
+        m_responsebuffer[len++] = 0x00;
+      } else {
+        uint32_t capacity = (m_sel->m_fileSize / m_sel->m_blocksize);
+        m_responsebuffer[len++] = 8;             /* Block descriptor length */
+        m_responsebuffer[len++] = (capacity >> 24) & 0xff;
+        m_responsebuffer[len++] = (capacity >> 16) & 0xff;
+        m_responsebuffer[len++] = (capacity >> 8) & 0xff;
+        m_responsebuffer[len++] = capacity & 0xff;
+        m_responsebuffer[len++] = (m_sel->m_blocksize >> 24) & 0xff;
+        m_responsebuffer[len++] = (m_sel->m_blocksize >> 16) & 0xff;
+        m_responsebuffer[len++] = (m_sel->m_blocksize >> 8) & 0xff;
+        m_responsebuffer[len++] = (m_sel->m_blocksize) & 0xff;
+      }
+    }
+  
+    /* Check for requested mode page */
+    page = m_cmd[2] & 0x3F;
+    pagemax = (page != 0x3f) ? page : 0x3e;
+    pagemin = (page != 0x3f) ? page : 0x00;
+    for(page = pagemax; page >= pagemin; page--) {
+      switch (page) {
+        case MODEPAGE_VENDOR_SPECIFIC:
+          /* Accept request only for current values */
+          if (m_cmd[2] & 0xC0) {
+            //DEBUGPRINT(DBG_TRACE, " [2]==%d", m_cmd[2]);
+            /* Prepare sense data */
+            m_sts |= STATUS_CHECK;
+            m_sel->m_sense.m_key = ILLEGAL_REQUEST;
+            m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
+            m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
+            m_sel->m_sense.m_key_specific[1] = 0x00;
+            m_sel->m_sense.m_key_specific[2] = 0x02;
+            m_phase = PHASE_STATUSIN;
+            return;
+          }
+  
+          /* Unit attention */
+          m_responsebuffer[len++] = 0x80; // PS, page id
+          m_responsebuffer[len++] = 0x02; // Page length
+          m_responsebuffer[len++] = 0x00;
+          m_responsebuffer[len++] = 0x00;
+          break;
+        case MODEPAGE_RW_ERROR_RECOVERY:
+          if(m_cmd[2] & 0x40) {
+            m_sts |= STATUS_CHECK;
+            m_sel->m_sense.m_key = ILLEGAL_REQUEST;
+            m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
+            m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
+            m_sel->m_sense.m_key_specific[1] = 0x00;
+            m_sel->m_sense.m_key_specific[2] = 0x02;
+            m_phase = PHASE_STATUSIN;
+            return;
+          }
+          m_responsebuffer[len++] = MODEPAGE_RW_ERROR_RECOVERY; // PS, page id
+          m_responsebuffer[len++] = 0x0a; // Page length
+          m_responsebuffer[len++] = 0x05; // 
+          m_responsebuffer[len++] = 0x00; // Read Retry Count
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Write Retry Count
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Recovery Time Limit
+          m_responsebuffer[len++] = 0x00;
+          break;
+#if 0
+        case MODEPAGE_DCRC_PARAMETERS:
+          m_responsebuffer[len++] = 0x82; // PS, page id
+          m_responsebuffer[len++] = 0x0e; // Page length
+          m_responsebuffer[len++] = 0xe6; // Buffer full ratio, 90%
+          m_responsebuffer[len++] = 0x1a; // Buffer empty ratio, 10%
+          m_responsebuffer[len++] = 0x00; // Bus inactivity limit
+          m_responsebuffer[len++] = 0x00;
+          m_responsebuffer[len++] = 0x00; // Disconnect time limit
+          m_responsebuffer[len++] = 0x00;
+          m_responsebuffer[len++] = 0x00; // Connect time limit
+          m_responsebuffer[len++] = 0x00;
+          m_responsebuffer[len++] = 0x00; // Maximum burst size
+          m_responsebuffer[len++] = 0x00;
+          m_responsebuffer[len++] = 0x00; // EMDP, Dimm, DTDC
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Reserved
+          m_responsebuffer[len++] = 0x00; // Reserved
+          break;
+#endif
+        case MODEPAGE_FORMAT_PARAMETERS:
+          m_responsebuffer[len + 0] = MODEPAGE_FORMAT_PARAMETERS; //Page code
+          m_responsebuffer[len + 1] = 0x16; // Page length
+          m_responsebuffer[len + 11] = 0x3F;//Number of sectors / track
+          len += 24;
+          break;
+        case MODEPAGE_RIGID_GEOMETRY:
+          {
+            uint32_t bc = m_sel->m_fileSize / m_sel->m_file;
+            m_responsebuffer[len + 0] = MODEPAGE_RIGID_GEOMETRY; //Page code
+            m_responsebuffer[len + 1] = 0x16; // Page length
+            m_responsebuffer[len + 2] = bc >> 16;// Cylinder length
+            m_responsebuffer[len + 3] = bc >> 8;
+            m_responsebuffer[len + 4] = bc;
+            m_responsebuffer[len + 5] = 1;   //Number of heads
+            len += 24;
+          }
+          break;
+        default:
+          if(pagemin == pagemax) {
+            m_sts |= STATUS_CHECK;
+            m_sel->m_sense.m_key = ILLEGAL_REQUEST;
+            m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB;       /* "Invalid field in CDB" */
+            m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
+            m_sel->m_sense.m_key_specific[1] = 0x00;
+            m_sel->m_sense.m_key_specific[2] = 0x02;
+            m_phase = PHASE_STATUSIN;
+            return;
+          }
+      }
+    }
+  
+    /* Report size of requested data */
+    if(m_cmd[0] == CMD_MODE_SENSE6) {
+      m_responsebuffer[0] = len;
+    } else {  
+      m_responsebuffer[0] = (len >> 8) & 0xff;
+      m_responsebuffer[1] = len & 0xff;
+    }  
+
+    /* Truncate data if necessary */
+    if (maxlen < len) {
+      len = maxlen;
+    }
+  
+    // Send it
+    writeDataPhase(len, m_responsebuffer);
+  }
   m_phase = PHASE_STATUSIN;
 }
 
 uint8_t onModeSelectCommand() {
-  m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
-  m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
-  m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-  m_sense[m_id][m_lun].m_key_specific[2] = 0x04;
+  m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
+  m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
+  m_sel->m_sense.m_key_specific[1] = 0x00;
+  m_sel->m_sense.m_key_specific[2] = 0x04;
   return 0x02;
 }
 
 void ModeSelect6CommandHandler() {
   LOG("[ModeSelect6] ");
   uint16_t len = m_cmd[4];
-  readDataPhase(len, m_buf);
+  readDataPhase(len, m_responsebuffer);
 
   for(int i = 1; i < len; i++ ) {
     LOG(":");
-    LOGHEX2(m_buf[i]);
+    LOGHEX2(m_responsebuffer[i]);
   }
   LOGN("");
 //  m_sts |= onModeSelectCommand();
@@ -99,11 +374,11 @@ void ModeSelect6CommandHandler() {
 void ModeSelect10CommandHandler() {
   LOGN("[ModeSelect10]");
   uint16_t len = ((uint16_t)m_cmd[7] << 8) | m_cmd[8];
-  readDataPhase(len, m_buf);
+  readDataPhase(len, m_responsebuffer);
 
   for(int i = 1; i < len; i++ ) {
     LOG(":");
-    LOGHEX2(m_buf[i]);
+    LOGHEX2(m_responsebuffer[i]);
   }
   LOGN("");
 //  m_sts |= onModeSelectCommand();
@@ -120,16 +395,10 @@ void PreAllowMediumRemovalCommandHandler() {
   m_phase = PHASE_STATUSIN;
 }
 
-void ModeSense10CommandHandler() {
-  LOGN("[ModeSense10]");
-  onModeSenseCommand(m_cmd[1] & 0x80, m_cmd[2], ((uint32_t)m_cmd[7] << 8) | m_cmd[8]);
-  m_phase = PHASE_STATUSIN;
-}
-
 void UnknownCommandHandler() {
   LOGN("[*Unknown]");
   m_sts |= 0x02;
-  m_sense[m_id][m_lun].m_key = 5;
+  m_sel->m_sense.m_key = 5;
   m_phase = PHASE_STATUSIN;
 }
 
diff --git a/src/greenscsi-next.ino b/src/greenscsi-next.ino
deleted file mode 100755
index 376f79e..0000000
--- a/src/greenscsi-next.ino
+++ /dev/null
@@ -1,1614 +0,0 @@
-/*  
- *  GreenSCSI
- *  Copyright (c) 2021  David Kuder
- *  
- *  Based on BlueSCSI
- *  Copyright (c) 2021  Eric Helgeson, Androda
- *  
- *  This file is free software: you may copy, redistribute and/or modify it  
- *  under the terms of the GNU General Public License as published by the  
- *  Free Software Foundation, either version 2 of the License, or (at your  
- *  option) any later version.  
- *  
- *  This file is distributed in the hope that it will be useful, but  
- *  WITHOUT ANY WARRANTY; without even the implied warranty of  
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU  
- *  General Public License for more details.  
- *  
- *  You should have received a copy of the GNU General Public License  
- *  along with this program.  If not, see https://github.com/erichelgeson/bluescsi.  
- *  
- * This file incorporates work covered by the following copyright and  
- * permission notice:  
- *  
- *     Copyright (c) 2019 komatsu   
- *  
- *     Permission to use, copy, modify, and/or distribute this software  
- *     for any purpose with or without fee is hereby granted, provided  
- *     that the above copyright notice and this permission notice appear  
- *     in all copies.  
- *  
- *     THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL  
- *     WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED  
- *     WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE  
- *     AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR  
- *     CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS  
- *     OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,  
- *     NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN  
- *     CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.  
- */
-
-#include <Arduino.h> // For Platform.IO
-#include <SPI.h>
-#include <SdFat.h>
-#include "sdios.h"
-#include "scsi_defs.h"
-
-#define DEBUG            0      // 0:No debug information output (Faster by 2-3x+)
-                                // 1: Debug information output to USB Serial
-                                // 2: Debug information output to LOG.txt (slow)
-
-#define SCSI_SELECT      0      // 0 for STANDARD
-                                // 1 for SHARP X1turbo
-                                // 2 for NEC PC98
-#define READ_INTERRUPT       1  // Should be Fastest (until we get Synch working)
-#define READ_SPEED_OPTIMIZE  0  // Faster reads
-#define WRITE_INTERRUPT      1  // Should be Fastest (until we get Synch working)
-#define WRITE_SPEED_OPTIMIZE 0  // Faster writes
-#define USE_DB2ID_TABLE      1  // Use table to get ID from SEL-DB
-
-// SCSI config
-#define NUM_SCSIID  8          // Maximum number of supported SCSI-IDs (The minimum is 0)
-#define NUM_SCSILUN 8          // Maximum number of LUNs supported     (The minimum is 0)
-#define READ_PARITY_CHECK 0    // Perform read parity check (unverified)
-
-// HDD format
-#define MAX_BLOCKSIZE 4096     // Maximum BLOCK size
-
-// SDFAT
-SdFs sd;
-
-#if DEBUG == 1
-#define LOG(XX)     Serial.print(XX)
-#define LOGHEX2(XX)  Serial.printf("%02x", XX)
-#define LOGHEX4(XX)  Serial.printf("%04x", XX)
-#define LOGHEX6(XX)  Serial.printf("%06x", XX)
-#define LOGN(XX)    Serial.println(XX)
-#define LOGHEX2N(XX) Serial.printf("%02x\r\n", XX)
-#define LOGHEX4N(XX) Serial.printf("%04x\r\n", XX)
-#define LOGHEX6N(XX) Serial.printf("%06x\r\n", XX)
-#elif DEBUG == 2
-#define LOG(XX)      LOG_FILE.print(XX); LOG_FILE.sync();
-#define LOGHEX2(XX)  LOG_FILE.printf("%02x", XX); LOG_FILE.sync();
-#define LOGHEX4(XX)  LOG_FILE.printf("%04x", XX); LOG_FILE.sync();
-#define LOGHEX6(XX)  LOG_FILE.printf("%06x", XX); LOG_FILE.sync();
-#define LOGN(XX)     LOG_FILE.println(XX); LOG_FILE.sync();
-#define LOGHEX2N(XX) LOG_FILE.printf("%02x\r\n", XX); LOG_FILE.sync();
-#define LOGHEX4N(XX) LOG_FILE.printf("%04x\r\n", XX); LOG_FILE.sync();
-#define LOGHEX6N(XX) LOG_FILE.printf("%06x\r\n", XX); LOG_FILE.sync();
-#else
-#define LOG(XX)      //Serial.print(XX)
-#define LOGHEX2(XX)  //Serial.printf("%02x", XX)
-#define LOGHEX4(XX)  //Serial.printf("%04x", XX)
-#define LOGHEX6(XX)  //Serial.printf("%06x", XX)
-#define LOGN(XX)     //Serial.println(XX)
-#define LOGHEX2N(XX) //Serial.printf("%02x\r\n", XX)
-#define LOGHEX4N(XX) //Serial.printf("%04x\r\n", XX)
-#define LOGHEX6N(XX) //Serial.printf("%06x\r\n", XX)
-#endif
-
-#define DB0       0     // SCSI:DB0
-#define DB1       1     // SCSI:DB1
-#define DB2       29    // SCSI:DB2
-#define DB3       30    // SCSI:DB3
-#define DB4       43    // SCSI:DB4
-#define DB5       46    // SCSI:DB5
-#define DB6       44    // SCSI:DB6
-#define DB7       45    // SCSI:DB7
-#define DB8       32    // SCSI:DBP
-#define ATN       12    // SCSI:ATN
-#define BSY       10    // SCSI:BSY
-#define ACK       9     // SCSI:ACK
-#define RST       8     // SCSI:RST
-#define MSG       7     // SCSI:MSG
-#define SEL       6     // SCSI:SEL
-#define CD        5     // SCSI:C/D
-#define REQ       4     // SCSI:REQ
-#define IO        3     // SCSI:I/O
-
-#define SD_CS     BUILTIN_SDCARD      // SDCARD:CS
-#define LED       13     // LED
-
-// LED control
-#define LED_ON()       digitalWrite(LED, HIGH);
-#define LED_OFF()      digitalWrite(LED, LOW);
-
-// SCSI output pin control: opendrain active LOW (direct pin drive)
-inline void SCSI_OUT(int VPIN, boolean ACTIVE) {
-  if(ACTIVE) {
-    pinMode(VPIN, OUTPUT_OPENDRAIN);
-    digitalWrite(VPIN, LOW);
-  } else {
-    pinMode(VPIN, INPUT);
-  }
-};
-
-//#define SET_REQ_ACTIVE() SCSI_OUT(REQ, true)
-//#define SET_REQ_INACTIVE() SCSI_OUT(REQ, false)
-#define SET_REQ_ACTIVE() { GPIOA_PCOR = (1 << 13); }
-#define SET_REQ_INACTIVE() { GPIOA_PSOR = (1 << 13); }
-#define SET_MSG_ACTIVE() { GPIOD_PCOR = (1 << 2); }
-#define SET_MSG_INACTIVE() { GPIOD_PSOR = (1 << 2); }
-#define SET_CD_ACTIVE() { GPIOD_PCOR = (1 << 7); }
-#define SET_CD_INACTIVE() { GPIOD_PSOR = (1 << 7); }
-#define SET_IO_ACTIVE() { GPIOA_PCOR = (1 << 12); }
-#define SET_IO_INACTIVE() { GPIOA_PSOR = (1 << 12); }
-
-//#define GET_ACK() SCSI_IN(ACK)
-#define GET_ACK() (!(GPIOC_PDIR & (1 << 3)))
-#define GET_ATN() (!(GPIOC_PDIR & (1 << 7)))
-#define GET_BSY() (!(GPIOC_PDIR & (1 << 4)))
-#define GET_RST() (!(GPIOD_PDIR & (1 << 3)))
-#define GET_SEL() (!(GPIOD_PDIR & (1 << 4)))
-
-// SCSI input pin check (invert)
-inline boolean SCSI_IN(int VPIN) {
-  pinMode(VPIN, INPUT_PULLUP);
-  return !digitalRead(VPIN);
-};
-
-#define SCSI_DB_MASK 0x00ff0800
-
-// Put DB and DP in output mode
-inline void SCSI_DB_OUTPUT() {
-  GPIOB_PDOR = SCSI_DB_MASK;
-  GPIOB_PDDR = 0x00000000;
-}
-
-// Put DB and DP in input mode
-inline void SCSI_DB_INPUT()  {
-  GPIOB_PDDR = 0x00000000;
-}
-
-
-// Turn on the output only for BSY
-#define SCSI_BSY_ACTIVE()      { SCSI_OUT(BSY, true); }
-// BSY,REQ,MSG,CD,IO Turn on the output (no change required for OD)
-#define SCSI_TARGET_ACTIVE()   { }
-// BSY,REQ,MSG,CD,IO Turn off output, BSY is the last input
-#define SCSI_TARGET_INACTIVE() { SCSI_DB_INPUT(); SET_REQ_INACTIVE(); SET_MSG_INACTIVE(); SET_CD_INACTIVE(); SET_IO_INACTIVE(); SCSI_OUT(BSY, false); pinMode(BSY, INPUT_PULLUP); }
-
-// HDDiamge file
-#define HDIMG_ID_POS  2                 // Position to embed ID number
-#define HDIMG_LUN_POS 3                 // Position to embed LUN numbers
-#define HDIMG_BLK_POS 5                 // Position to embed block size numbers
-#define MAX_FILE_PATH 32                // Maximum file name length
-
-// HDD image
-typedef struct hddimg_struct
-{
-  FsFile      m_file;                 // File object
-  uint64_t    m_fileSize;             // File size
-  size_t      m_blocksize;            // SCSI BLOCK size
-} HDDIMG;
-HDDIMG  img[NUM_SCSIID][NUM_SCSILUN]; // Maximum number
-
-volatile bool m_isBusReset = false;   // Bus reset
-
-uint8_t       scsi_id_mask;           // Mask list of responding SCSI IDs
-uint8_t       m_id;                   // Currently responding SCSI-ID
-uint8_t       m_lun;                  // Logical unit number currently responding
-uint8_t       m_sts;                  // Status uint8_t
-uint8_t       m_msg;                  // Message uint8_ts
-HDDIMG       *m_img;                  // HDD image for current SCSI-ID, LUN
-uint8_t       m_buf[MAX_BLOCKSIZE+1]; // General purpose buffer + overrun fetch
-int           m_msc;
-uint8_t       m_msb[256];             // Command storage uint8_ts
-uint8_t       m_phase;
-uint8_t       m_cmdlen;
-uint8_t       m_cmd[12];
-uint8_t       m_inquiryresponse[NUM_SCSIID][96];
-uint8_t       m_responsebuffer[256];
-
-typedef struct m_sense_s {
-  uint8_t     m_key;
-  uint8_t     m_code;
-  uint8_t     m_key_specific[4];
-} msense_t;
-
-msense_t      m_sense[NUM_SCSIID][NUM_SCSILUN];
-
-typedef void (*CommandHandler_t)();
-CommandHandler_t m_handler[NUM_SCSIID][256];
-CommandHandler_t m_badlunhandler[256];
-
-typedef enum {
-  PHASE_BUSFREE = 0,
-  PHASE_SELECTION,
-  PHASE_MESSAGEOUT,
-  PHASE_COMMAND,
-  PHASE_STATUSIN,
-  PHASE_MESSAGEIN,
-} phase_t;
-
-/*
- *  Data uint8_t to GPIOB register setting value and parity table
-*/
-
-// Parity bit generation
-#define PTY(V)   ((1^((V)^((V)>>1)^((V)>>2)^((V)>>3)^((V)>>4)^((V)>>5)^((V)>>6)^((V)>>7)))&1)
-
-// Set DBP
-#define DBP(D)    (((uint32_t)(D)<<16)|(PTY(D)<<11))
-
-#define DBP8(D)   DBP(D),DBP(D+1),DBP(D+2),DBP(D+3),DBP(D+4),DBP(D+5),DBP(D+6),DBP(D+7)
-#define DBP32(D)  DBP8(D),DBP8(D+8),DBP8(D+16),DBP8(D+24)
-
-// BSRR register control value that simultaneously performs DB set, DP set, and REQ = H (inactrive)
-static const uint32_t db_bsrr[256]={
-  DBP32(0x00),DBP32(0x20),DBP32(0x40),DBP32(0x60),
-  DBP32(0x80),DBP32(0xA0),DBP32(0xC0),DBP32(0xE0)
-};
-// Parity bit acquisition
-#define PARITY(DB) ((db_bsrr[DB & 0xff] & 0x0800) >> 11)
-
-// Macro cleaning
-#undef DBP32
-#undef DBP8
-//#undef DBP
-//#undef PTY
-
-#if USE_DB2ID_TABLE
-/* DB to SCSI-ID translation table */
-static const uint8_t db2scsiid[256]={
-  0xff,
-  0,
-  1,1,
-  2,2,2,2,
-  3,3,3,3,3,3,3,3,
-  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
-  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
-  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
-  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
-  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7
-};
-#endif
-
-// Log File
-#define VERSION "1.1-SNAPSHOT"
-#if DEBUG == 2
-#define LOG_FILENAME "LOG.txt"
-FsFile LOG_FILE;
-#endif
-
-// SCSI Drive Vendor information
-static uint8_t SCSI_DISK_INQUIRY_RESPONSE[96] = {
-  0x00, //device type
-  0x00, //RMB = 0
-  0x01, //ISO, ECMA, ANSI version
-  0x01, //Response data format
-  35 - 4, //Additional data length
-  0, 0, //Reserve
-  0x00, //Support function
-  'Q', 'U', 'A', 'N', 'T', 'U', 'M', ' ', // vendor 8
-  'F', 'I', 'R', 'E', 'B', 'A', 'L', 'L', '1', ' ', ' ',' ', ' ', ' ', ' ', ' ', // product 16
-  '1', '.', '0', ' ', // version 4
-  0
-};
-
-#if 0
-static uint8_t SCSI_NEC_INQUIRY_RESPONSE[96] = {
-  0x00, //Device type
-  0x00, //RMB = 0
-  0x01, //ISO,ECMA,ANSI version
-  0x01, //Response data format
-  35 - 4, //Additional data length
-  0, 0, //Reserve
-  0x00, //Support function
-  'N', 'E', 'C', 'I', 'T', 'S', 'U', ' ',
-  'A', 'r', 'd', 'S', 'C', 'S', 'i', 'n', 'o', ' ', ' ',' ', ' ', ' ', ' ', ' ',
-  '0', '0', '1', '0',
-  0
-};
-#endif
-
-static uint8_t SCSI_CDROM_INQUIRY_RESPONSE[96] = {
-  0x05, //device type
-  0x80, //RMB = 0
-  0x05, //ISO, ECMA, ANSI version
-  0x02, //Response data format
-  46 - 4, //Additional data length
-  0, 0, //Reserve
-  0x00, //Support function
-  'S', 'O', 'N', 'Y', ' ', ' ', ' ', ' ', // vendor 8
-  'C', 'D', '-', 'R', 'O', 'M', ' ', 'C', 'D', 'U', '-','8', '0', '0', '3', ' ', // product 16
-  '1', '.', '7', 'w', // version 4
-  // Release Number (1 Byte)
-  0x20,
-  // Revision Date (10 Bytes)
-  '2','0','1','6','/','1','2','/','2','6',
-  0
-};
-
-void onFalseInit(void);
-void noSDCardFound(void);
-void onBusReset(void);
-void initFileLog(void);
-void finalizeFileLog(void);
-
-/*
- * IO read.
- */
-inline uint8_t readIO(void)
-{
-  // Port input data register
-  uint32_t ret = GPIOB_PDIR;
-  uint8_t bret = (uint8_t)((~ret)>>16);
-#if READ_PARITY_CHECK
-  if((db_bsrr[bret] ^ ret) & 0x0800)
-    m_sts |= 0x01; // parity error
-#endif
-
-  return bret;
-}
-
-// If config file exists, read the first three lines and copy the contents.
-// File must be well formed or you will get junk in the SCSI Vendor fields.
-void ConfigureDiskIdentity(int id, const char *image_name) {
-  char configname[MAX_FILE_PATH+1];
-  memcpy(configname, image_name, MAX_FILE_PATH+1);
-  char *psuffix = strstr(configname, ".img");
-  if(psuffix) {
-    strcpy(psuffix, ".cfg");
-  } else {
-    sprintf(configname, "hd%d0_512.cfg", id);
-  }
-  memcpy(m_inquiryresponse[id], SCSI_DISK_INQUIRY_RESPONSE, sizeof(SCSI_DISK_INQUIRY_RESPONSE));
-
-  FsFile config_file = sd.open(configname, O_RDONLY);
-  if (!config_file.isOpen()) {
-    return;
-  }
-  char vendor[9];
-  memset(vendor, 0, sizeof(vendor));
-  config_file.readBytes(vendor, sizeof(vendor));
-  LOGN("SCSI VENDOR: ");
-  LOGN(vendor);
-  memcpy(&(m_inquiryresponse[id][8]), vendor, 8);
-
-  char product[17];
-  memset(product, 0, sizeof(product));
-  config_file.readBytes(product, sizeof(product));
-  LOGN("SCSI PRODUCT: ");
-  LOGN(product);
-  memcpy(&(m_inquiryresponse[id][16]), product, 16);
-
-  char version[5];
-  memset(version, 0, sizeof(version));
-  config_file.readBytes(version, sizeof(version));
-  LOGN("SCSI VERSION: ");
-  LOGN(version);
-  memcpy(&(m_inquiryresponse[id][32]), version, 4);
-  config_file.close();
-}
-
-// If config file exists, read the first three lines and copy the contents.
-// File must be well formed or you will get junk in the SCSI Vendor fields.
-void ConfigureOpticalIdentity(int id, const char *image_name) {
-  char configname[MAX_FILE_PATH+1];
-  memcpy(configname, image_name, MAX_FILE_PATH+1);
-  char *psuffix = strstr(configname, ".img");
-  if(psuffix) {
-    strcpy(psuffix, ".cfg");
-  } else {
-    sprintf(configname, "cd%d0_512.cfg", id);
-  }
-  memcpy(m_inquiryresponse[id], SCSI_CDROM_INQUIRY_RESPONSE, sizeof(SCSI_CDROM_INQUIRY_RESPONSE));
-
-  FsFile config_file = sd.open(configname, O_RDONLY);
-  if (!config_file.isOpen()) {
-    return;
-  }
-  char vendor[9];
-  memset(vendor, 0, sizeof(vendor));
-  config_file.readBytes(vendor, sizeof(vendor));
-  LOGN("SCSI VENDOR: ");
-  LOGN(vendor);
-  memcpy(&(m_inquiryresponse[id][8]), vendor, 8);
-
-  char product[17];
-  memset(product, 0, sizeof(product));
-  config_file.readBytes(product, sizeof(product));
-  LOGN("SCSI PRODUCT: ");
-  LOGN(product);
-  memcpy(&(m_inquiryresponse[id][16]), product, 16);
-
-  char version[5];
-  memset(version, 0, sizeof(version));
-  config_file.readBytes(version, sizeof(version));
-  LOGN("SCSI VERSION: ");
-  LOGN(version);
-  memcpy(&(m_inquiryresponse[id][32]), version, 4);
-  config_file.close();
-}
-
-// read SD information and print to logfile
-void readSDCardInfo()
-{
-  cid_t sd_cid;
-
-  if(sd.card()->readCID(&sd_cid))
-  {
-    LOG("Sd MID:");
-    LOGHEX2(sd_cid.mid);
-    LOG(" OID:");
-    LOG(sd_cid.oid[0]);
-    LOGN(sd_cid.oid[1]);
-
-    LOG("Sd Name:");
-    LOG(sd_cid.pnm[0]);
-    LOG(sd_cid.pnm[1]);
-    LOG(sd_cid.pnm[2]);
-    LOG(sd_cid.pnm[3]);
-    LOGN(sd_cid.pnm[4]);
-
-    LOG("Sd Date:");
-    LOG(sd_cid.mdt_month);
-    LOG("/20"); // CID year is 2000 + high/low
-    LOG(sd_cid.mdt_year_high);
-    LOGN(sd_cid.mdt_year_low);
-
-    LOG("Sd Serial:");
-    LOGN(sd_cid.psn);
-  }
-}
-
-/*
- * Open HDD image file
- */
-
-boolean OpenDiskImage(void *hp,const char *image_name,int id,int lun,int blocksize)
-{
-  HDDIMG *h = (HDDIMG *)hp;
-
-  h->m_fileSize = 0;
-  h->m_blocksize = blocksize;
-  h->m_file = sd.open(image_name, O_RDWR);
-  if(h->m_file.isOpen())
-  {
-    h->m_fileSize = h->m_file.size();
-    LOG(" Imagefile: ");
-    LOG(image_name);
-    if(h->m_fileSize>0)
-    {
-      // check blocksize dummy file
-      LOG(" / ");
-      LOG(h->m_fileSize / h->m_blocksize);
-      LOG(" sectors / ");
-      LOG(h->m_fileSize / 1024);
-      LOG(" KiB / ");
-      LOG(h->m_fileSize / 1024 / 1024);
-      LOGN(" MiB");
-      return true; // File opened
-    }
-    else
-    {
-      h->m_file.close();
-      h->m_fileSize = h->m_blocksize = 0; // no file
-      LOGN(" FileSizeError");
-    }
-  }
-  return false;
-}
-
-/*
- * Initialization.
- *  Initialize the bus and set the PIN orientation
- */
-void setup()
-{
-  Serial.begin(115200);
-
-  delay(1500);
-  
-  // PIN initialization
-  pinMode(LED, OUTPUT);
-  LED_OFF();
-
-  //GPIO(SCSI BUS)Initialization
-
-  // Input port
-  pinMode(ATN, INPUT_PULLUP);
-  pinMode(BSY, INPUT_PULLUP);
-  pinMode(ACK, INPUT_PULLUP);
-  pinMode(RST, INPUT_PULLUP);
-  pinMode(SEL, INPUT_PULLUP);
-  // Output port
-  pinMode(MSG, OUTPUT_OPENDRAIN);
-  pinMode(CD,  OUTPUT_OPENDRAIN);
-  pinMode(REQ, OUTPUT_OPENDRAIN);
-  pinMode(IO,  OUTPUT_OPENDRAIN);
-
-  pinMode(DB0, OUTPUT_OPENDRAIN);
-  pinMode(DB1, OUTPUT_OPENDRAIN);
-  pinMode(DB2, OUTPUT_OPENDRAIN);
-  pinMode(DB3, OUTPUT_OPENDRAIN);
-  pinMode(DB4, OUTPUT_OPENDRAIN);
-  pinMode(DB5, OUTPUT_OPENDRAIN);
-  pinMode(DB6, OUTPUT_OPENDRAIN);
-  pinMode(DB7, OUTPUT_OPENDRAIN);
-  pinMode(DB8, OUTPUT_OPENDRAIN);
-
-  // DB and DP are input modes
-  SCSI_DB_INPUT();
-
-  // Turn off the output port
-  SCSI_TARGET_INACTIVE();
-
-  //Occurs when the RST pin state changes from HIGH to LOW
-  attachInterrupt(RST, onBusReset, FALLING);
-  attachInterrupt(ACK, ackTransferISR, CHANGE);
-
-  LED_ON();
-
-  if(!sd.begin(SdioConfig(FIFO_SDIO))) {
-#if DEBUG > 0
-    Serial.println("SD initialization failed!");
-#endif
-    noSDCardFound();
-  }
-  initFileLog();
-  readSDCardInfo();
-  ConfigureBadLunHandlers();
-
-  //Sector data overrun uint8_t setting
-  m_buf[MAX_BLOCKSIZE] = 0xff; // DB0 all off,DBP off
-  //HD image file open
-  scsi_id_mask = 0x00;
-
-  // Iterate over the root path in the SD card looking for candidate image files.
-  SdFile root;
-  root.open("/");
-  SdFile file;
-  bool imageReady;
-  int usedDefaultId = 0;
-  while (1) {
-    if (!file.openNext(&root, O_READ)) break;
-    char name[MAX_FILE_PATH+1];
-    if(!file.isDir()) {
-      file.getName(name, MAX_FILE_PATH+1);
-      file.close();
-      String file_name = String(name);
-      file_name.toLowerCase();
-      if((file_name.startsWith("hd") || file_name.startsWith("cd")) && file_name.endsWith(".img")) {
-        // Defaults for Hard Disks
-        int id  = 1; // 0 and 3 are common in Macs for physical HD and CD, so avoid them.
-        int lun = 0;
-        int blk = 512;
-
-        // Positionally read in and coerase the chars to integers.
-        // We only require the minimum and read in the next if provided.
-        int file_name_length = file_name.length();
-        if(file_name_length > 2) { // HD[N]
-          int tmp_id = file_name[HDIMG_ID_POS] - '0';
-
-          if(tmp_id > -1 && tmp_id < NUM_SCSIID) {
-            id = tmp_id; // If valid id, set it, else use default
-          } else {
-            usedDefaultId++;
-          }
-        }
-        if(file_name_length > 3) { // HD0[N]
-          int tmp_lun = file_name[HDIMG_LUN_POS] - '0';
-
-          if(tmp_lun > -1 && tmp_lun < NUM_SCSILUN) {
-            lun = tmp_lun; // If valid id, set it, else use default
-          }
-        }
-        int blk1 = 0, blk2 = 0, blk3 = 0, blk4 = 0;
-        if(file_name_length > 8) { // HD00_[111]
-          blk1 = name[HDIMG_BLK_POS] - '0';
-          blk2 = name[HDIMG_BLK_POS+1] - '0';
-          blk3 = name[HDIMG_BLK_POS+2] - '0';
-          if(file_name_length > 9) // HD00_NNN[1]
-            blk4 = name[HDIMG_BLK_POS+3] - '0';
-        }
-        if(blk1 == 2 && blk2 == 5 && blk3 == 6) {
-          blk = 256;
-        } else if(blk1 == 1 && blk2 == 0 && blk3 == 2 && blk4 == 4) {
-          blk = 1024;
-        } else if(blk1 == 2 && blk2 == 0 && blk3 == 4 && blk4 == 8) {
-          blk  = 2048;
-        }
-
-        if(id < NUM_SCSIID && lun < NUM_SCSILUN) {
-          HDDIMG *h = &img[id][lun];
-
-          // Configure Handlers and Inquiry Response for all luns
-          if(file_name.startsWith("hd")) {
-            LOG("HD at ID ");
-            LOG(id);
-            LOG(":LUN ");
-            LOG(lun);
-            if(lun == 0) {
-              ConfigureDiskIdentity(id, name);
-              ConfigureDiskHandlers(id);
-            }
-          }
-          if(file_name.startsWith("cd")) {
-            LOG("CD at ID ");
-            LOG(id);
-            LOG(":LUN ");
-            LOG(lun);
-            if(lun == 0) {
-              ConfigureOpticalIdentity(id, name);
-              ConfigureOpticalHandlers(id);
-            }
-          }
-          imageReady = OpenDiskImage(h, name, id, lun, blk);
-          if(imageReady) { // Marked as a responsive ID
-            scsi_id_mask |= 1<<id;
-          }
-        } else {
-          LOG("Bad LUN or SCSI id for image: ");
-          LOGN(name);
-        }
-      } else {
-          LOG("Not an image: ");
-          LOGN(name);
-      }
-    }
-  }
-  if(usedDefaultId > 0) {
-    LOGN("!! More than one image did not specify a SCSI ID. Last file will be used at ID 1. !!");
-  }
-  root.close();
-
-  // Error if there are 0 image files
-  if(scsi_id_mask==0) {
-    LOGN("ERROR: No valid images found!");
-    onFalseInit();
-  }
-
-  finalizeFileLog();
-  LED_OFF();
-}
-
-/*
- * Setup initialization logfile
- */
-void initFileLog() {
-#if DEBUG == 2
-  LOG_FILE = sd.open(LOG_FILENAME, O_WRONLY | O_CREAT | O_TRUNC);
-  LOGN("GreenSCSI <-> SD - https://github.com/dkgrizzly/GreenSCSI");
-  LOG("VERSION: ");
-  LOGN(VERSION);
-  LOG("DEBUG:");
-  LOG(DEBUG);
-  LOG(" SCSI_SELECT:");
-  LOG(SCSI_SELECT);
-  LOG(" SDFAT_FILE_TYPE:");
-  LOGN(SDFAT_FILE_TYPE);
-  LOG("SdFat version: ");
-  LOGN(SD_FAT_VERSION_STR);
-  LOG("SdFat Max FileName Length: ");
-  LOGN(MAX_FILE_PATH);
-  LOGN("Initialized SD Card - lets go!");
-#endif
-}
-
-/*
- * Finalize initialization logfile
- */
-void finalizeFileLog() {
-  // View support drive map
-  LOG("ID");
-  for(int lun=0;lun<NUM_SCSILUN;lun++)
-  {
-    LOG(":LUN");
-    LOG(lun);
-  }
-  LOGN(":");
-  //
-  for(int id=0;id<NUM_SCSIID;id++)
-  {
-    LOG(" ");
-    LOG(id);
-    for(int lun=0;lun<NUM_SCSILUN;lun++)
-    {
-      HDDIMG *h = &img[id][lun];
-      if( (lun<NUM_SCSILUN) && (h->m_file))
-      {
-        LOG((h->m_blocksize<1000) ? ": " : ":");
-        LOG(h->m_blocksize);
-      }
-      else      
-        LOG(":----");
-    }
-    LOGN(":");
-  }
-  LOGN("Finished initialization of SCSI Devices - Entering main loop.");
-#if DEBUG == 2
-  LOG_FILE.close();
-#endif
-}
-
-/*
- * Initialization failed, blink 3x fast
- */
-void onFalseInit(void)
-{
-#if DEBUG == 2
-  LOG_FILE.sync();
-  LOG_FILE.close();
-#endif
-  while(true) {
-    for(int i = 0; i < 3; i++) {
-      LED_ON();
-      delay(250);
-      LED_OFF();
-      delay(250);
-    }
-    delay(3000);
-  }
-}
-
-/*
- * No SC Card found, blink 5x fast
- */
-void noSDCardFound(void)
-{
-#if DEBUG == 2
-  LOG_FILE.sync();
-  LOG_FILE.close();
-#endif
-  while(true) {
-    for(int i = 0; i < 5; i++) {
-      LED_ON();
-      delay(250);
-      LED_OFF();
-      delay(250);
-    }
-    delay(3000);
-  }
-}
-
-/*
- * ISR Driven Transfers
- */
-#define TRANSFER_IDLE 0
-#define TRANSFER_OUT0 1
-#define TRANSFER_OUT1 2
-#define TRANSFER_IN0  3
-#define TRANSFER_IN1  4
-uint8_t TransferPhase = TRANSFER_IDLE;
-uint16_t TransferBytesLeft = 0;
-uint8_t *TransferBuffer;
-
-void ackTransferISR() {
-  switch(TransferPhase) {
-    case TRANSFER_IDLE:
-      return;
-    case TRANSFER_OUT0:
-      TransferPhase = TRANSFER_OUT1;
-      GPIOB_PDDR = db_bsrr[*TransferBuffer];
-      GPIOB_PDOR = db_bsrr[*TransferBuffer] ^ SCSI_DB_MASK; // setup DB,DBP (160ns)
-      SET_REQ_ACTIVE();   // (30ns)
-      return;
-    case TRANSFER_OUT1:
-      SET_REQ_INACTIVE();
-      TransferBuffer++;
-      TransferBytesLeft--;
-      TransferPhase = (TransferBytesLeft > 0) ? TRANSFER_OUT0 : TRANSFER_IDLE;
-      return;
-    case TRANSFER_IN0:
-      TransferPhase = TRANSFER_IN1;
-      SET_REQ_ACTIVE();   // (30ns)
-      return;
-    case TRANSFER_IN1:
-      *TransferBuffer = readIO();
-      SET_REQ_INACTIVE();
-      TransferBuffer++;
-      TransferBytesLeft--;
-      TransferPhase = (TransferBytesLeft > 0) ? TRANSFER_IN0 : TRANSFER_IDLE;
-      return;
-  }
-}
-
-inline void readHandshakeBlockInterrupt(uint8_t *d, uint16_t len)
-{
-  //SCSI_DB_INPUT();
-  if(len) {
-    TransferBuffer = d;
-    TransferBytesLeft = len;
-    TransferPhase = TRANSFER_IN1;
-    SET_REQ_ACTIVE();
-  }
-}
-
-inline void writeHandshakeBlockInterrupt(const uint8_t *d, uint16_t len)
-{
-  //SCSI_DB_INPUT();
-  if(len) {
-    TransferBuffer = (uint8_t*)d;
-    TransferBytesLeft = len;
-    TransferPhase = TRANSFER_OUT1;
-    GPIOB_PDDR = db_bsrr[*TransferBuffer];
-    GPIOB_PDOR = db_bsrr[*TransferBuffer] ^ SCSI_DB_MASK; // setup DB,DBP (160ns)
-    SET_REQ_ACTIVE();
-  }
-}
-
-inline boolean waitInterruptTransferDone() {
-  for(;;) {
-    if(TransferPhase == TRANSFER_IDLE) {
-      return 1;
-    }
-    if(m_isBusReset) {
-      return 0;
-    }
-    yield();
-  }
-}
-
-/*
- * Bus reset interrupt.
- */
-void onBusReset(void)
-{
-  TransferPhase = TRANSFER_IDLE;
-
-#if SCSI_SELECT == 1
-  // SASI I / F for X1 turbo has RST pulse write cycle +2 clock ==
-  // I can't filter because it only activates about 1.25us
-  {{
-#else
-  if(!digitalRead(RST)) {
-    delayMicroseconds(20);
-    if(!digitalRead(RST)) {
-#endif  
-      SCSI_DB_INPUT();
-
-      LOGN("BusReset!");
-      m_isBusReset = true;
-    }
-  }
-}
-
-/*
- * Read by handshake.
- */
-inline uint8_t readHandshake(void)
-{
-  SET_REQ_ACTIVE();
-  //SCSI_DB_INPUT();
-  while(!m_isBusReset && !GET_ACK());
-  uint8_t r = readIO();
-  SET_REQ_INACTIVE();
-  while(GET_ACK()) { if(m_isBusReset) return 0; }
-  return r;  
-}
-
-inline void readHandshakeBlock(uint8_t *d, uint16_t len)
-{
-  //SCSI_DB_INPUT();
-  while(len) {
-    SET_REQ_ACTIVE();
-    while(!m_isBusReset && !GET_ACK());
-    *d++ = readIO();
-    len--;
-    SET_REQ_INACTIVE();
-    while(GET_ACK()) { if(m_isBusReset) return; }
-  }
-}
-
-
-/*
- * Write with a handshake.
- */
-inline void writeHandshake(uint8_t d)
-{
-  SCSI_DB_OUTPUT(); // (180ns)
-  GPIOB_PDDR = db_bsrr[d];
-  GPIOB_PDOR = db_bsrr[d] ^ SCSI_DB_MASK; // setup DB,DBP (160ns)
-
-  // ACK.Fall to DB output delay 100ns(MAX)  (DTC-510B)
-  SET_REQ_ACTIVE();   // (30ns)
-  while(!m_isBusReset && !GET_ACK());
-  // ACK.Fall to REQ.Raise delay 500ns(typ.) (DTC-510B)
-  GPIOB_PDDR = DBP(0xff);
-  GPIOB_PDOR = DBP(0xff) ^ SCSI_DB_MASK;  // DB=0xFF
-  SET_REQ_INACTIVE();
-
-  // REQ.Raise to DB hold time 0ns
-  SCSI_DB_INPUT(); // (150ns)
-  while( GET_ACK()) { if(m_isBusReset) return; }
-}
-
-inline void writeHandshakeBlock(const uint8_t *d, uint16_t len)
-{
-  SCSI_DB_OUTPUT(); // (180ns)
-  while(len) {
-    GPIOB_PDDR = db_bsrr[*d];
-    GPIOB_PDOR = db_bsrr[*d] ^ SCSI_DB_MASK; // setup DB,DBP (160ns)
-    SET_REQ_ACTIVE();   // (30ns)
-    while(!m_isBusReset && !GET_ACK());
-    d++;
-    len--;
-    SET_REQ_INACTIVE();
-    while( GET_ACK()) { if(m_isBusReset) return; }
-  }
-  SCSI_DB_INPUT(); // (150ns)
-}
-
-/*
- * Data in phase.
- *  Send len uint8_ts of data array p.
- */
-void writeDataPhase(int len, const uint8_t* p)
-{
-  //LOGN("DATAIN PHASE");
-  SET_MSG_INACTIVE(); //  gpio_write(MSG, low);
-  SCSI_OUT(CD , false); //  gpio_write(CD, low);
-  SCSI_OUT(IO , true);  //  gpio_write(IO, high);
-
-#if READ_INTERRUPT
-  writeHandshakeBlockInterrupt(p, len);
-  waitInterruptTransferDone();
-#elif READ_SPEED_OPTIMIZE
-  writeHandshakeBlock(p, len);
-#else
-  for (int i = 0; i < len; i++) {
-    if(m_isBusReset) {
-      m_phase = PHASE_BUSFREE;
-      return;
-    }
-    writeHandshake(p[i]);
-  }
-#endif
-  if(m_isBusReset) {
-    m_phase = PHASE_BUSFREE;
-  }
-}
-
-/* 
- * Data in phase.
- *  Send len block while reading from SD card.
- */
-void writeDataPhaseSD(uint32_t adds, uint32_t len)
-{
-#if (READ_SPEED_OPTIMIZE or READ_INTERRUPT)
-  uint32_t bigread = (MAX_BLOCKSIZE / m_img->m_blocksize);
-#endif
-  uint32_t i = 0;
-
-  //LOGN("DATAIN PHASE(SD)");
-  uint32_t pos = adds * m_img->m_blocksize;
-  m_img->m_file.seek(pos);
-
-  SET_MSG_INACTIVE(); //  gpio_write(MSG, low);
-  SCSI_OUT(CD , false); //  gpio_write(CD, low);
-  SCSI_OUT(IO , true); //  gpio_write(IO, high);
-
-  while(i < len) {
-      // Asynchronous reads will make it faster ...
-#if READ_INTERRUPT
-    if((len-i) >= bigread) {
-      m_img->m_file.read(m_buf, MAX_BLOCKSIZE/2);
-      waitInterruptTransferDone();
-      if(m_isBusReset) {
-        m_phase = PHASE_BUSFREE;
-        return;
-      }
-      writeHandshakeBlockInterrupt(m_buf, MAX_BLOCKSIZE/2);
-      m_img->m_file.read(m_buf+(MAX_BLOCKSIZE/2), MAX_BLOCKSIZE/2);
-
-      waitInterruptTransferDone();
-      if(m_isBusReset) {
-        m_phase = PHASE_BUSFREE;
-        return;
-      }
-      writeHandshakeBlockInterrupt(m_buf+(MAX_BLOCKSIZE/2), MAX_BLOCKSIZE/2);
-      i += bigread;
-    } else {
-      waitInterruptTransferDone();
-      if(m_isBusReset) {
-        m_phase = PHASE_BUSFREE;
-        return;
-      }
-      m_img->m_file.read(m_buf, m_img->m_blocksize * (len-i));
-      writeHandshakeBlock(m_buf, m_img->m_blocksize * (len-i));
-      waitInterruptTransferDone();
-      i = len;
-    }
-#elif READ_SPEED_OPTIMIZE
-    if((len-i) >= bigread) {
-      m_img->m_file.read(m_buf, MAX_BLOCKSIZE);
-      writeHandshakeBlock(m_buf, MAX_BLOCKSIZE);
-      i += bigread;
-    } else {
-      m_img->m_file.read(m_buf, m_img->m_blocksize * (len-i));
-      writeHandshakeBlock(m_buf, m_img->m_blocksize * (len-i));
-      i = len;
-    }
-#else
-    m_img->m_file.read(m_buf, m_img->m_blocksize);
-    for(unsigned int j = 0; j < m_img->m_blocksize; j++) {
-      if(m_isBusReset) {
-        m_phase = PHASE_BUSFREE;
-        return;
-      }
-      writeHandshake(m_buf[j]);
-    }
-#endif
-    if(m_isBusReset) {
-      m_phase = PHASE_BUSFREE;
-      return;
-    }
-  }
-}
-
-/*
- * Data out phase.
- *  len block read
- */
-void readDataPhase(int len, uint8_t* p)
-{
-  //LOGN("DATAOUT PHASE");
-  SET_MSG_INACTIVE();
-  SCSI_OUT(CD , false);
-  SCSI_OUT(IO , false);
-
-#if WRITE_INTERRUPT
-  readHandshakeBlockInterrupt(p, len);
-  waitInterruptTransferDone();
-#elif WRITE_SPEED_OPTIMIZE
-  readHandshakeBlock(p, len);
-#else
-  for(int i = 0; i < len; i++)
-    p[i] = readHandshake();
-#endif
-  if(m_isBusReset) {
-    m_phase = PHASE_BUSFREE;
-  }
-}
-
-/*
- * Data out phase.
- *  Write to SD card while reading len block.
- */
-void readDataPhaseSD(uint32_t adds, uint32_t len)
-{
-#if (WRITE_INTERRUPT or WRITE_SPEED_OPTIMIZE)
-  uint32_t bigread = (MAX_BLOCKSIZE / m_img->m_blocksize);
-#endif
-  uint32_t i = 0;
-
-  //LOGN("DATAOUT PHASE(SD)");
-  uint32_t pos = adds * m_img->m_blocksize;
-  m_img->m_file.seek(pos);
-  SET_MSG_INACTIVE();
-  SCSI_OUT(CD , false);
-  SCSI_OUT(IO , false);
-#if WRITE_INTERRUPT
-  if((len-i) >= bigread) {
-    readHandshakeBlockInterrupt(m_buf, (MAX_BLOCKSIZE/2));
-  } else {
-    readHandshakeBlockInterrupt(m_buf, (len * m_img->m_blocksize));
-  }
-#endif
-  while(i < len) {
-#if WRITE_INTERRUPT
-    if((len-i) >= bigread) {
-      waitInterruptTransferDone();
-      readHandshakeBlockInterrupt(m_buf+(MAX_BLOCKSIZE/2), MAX_BLOCKSIZE/2);
-      m_img->m_file.write(m_buf, MAX_BLOCKSIZE/2);
-      waitInterruptTransferDone();
-      i += bigread;
-      if((len-i) >= bigread) {
-        readHandshakeBlockInterrupt(m_buf, (MAX_BLOCKSIZE/2));
-        m_img->m_file.write(m_buf+(MAX_BLOCKSIZE/2), MAX_BLOCKSIZE/2);
-      } else if(i < len) {
-        m_img->m_file.write(m_buf+(MAX_BLOCKSIZE/2), MAX_BLOCKSIZE/2);
-        readHandshakeBlockInterrupt(m_buf, (len * m_img->m_blocksize));
-      }
-    } else {
-      waitInterruptTransferDone();
-      m_img->m_file.write(m_buf, m_img->m_blocksize * (len-i));
-      i = len;
-    }
-#elif WRITE_SPEED_OPTIMIZE
-    if((len-i) >= bigread) {
-      readHandshakeBlock(m_buf, MAX_BLOCKSIZE);
-      m_img->m_file.write(m_buf, MAX_BLOCKSIZE);
-      i += bigread;
-    } else {
-      readHandshakeBlock(m_buf, m_img->m_blocksize * (len-i));
-      m_img->m_file.write(m_buf, m_img->m_blocksize * (len-i));
-      i = len;
-    }
-#else
-    for(unsigned int j = 0; j <  m_img->m_blocksize; j++) {
-      if(m_isBusReset) {
-        return;
-      }
-      m_buf[j] = readHandshake();
-    }
-    m_img->m_file.write(m_buf, m_img->m_blocksize);
-#endif
-  }
-  m_img->m_file.flush();
-}
-
-/*
- * MODE SENSE command processing.
- */
-#if SCSI_SELECT == 2
-uint8_t onModeSenseCommand(uint8_t dbd, int cmd2, uint32_t len)
-{
-  if(!m_img) return 0x02; // Image file absent
-
-  int pageCode = cmd2 & 0x3F;
-
-  // Assuming sector size 512, number of sectors 25, number of heads 8 as default settings
-  int size = m_img->m_fileSize;
-  int cylinders = (int)(size >> 9);
-  cylinders >>= 3;
-  cylinders /= 25;
-  int sectorsize = 512;
-  int sectors = 25;
-  int heads = 8;
-  // Sector size
- int disksize = 0;
-  for(disksize = 16; disksize > 0; --(disksize)) {
-    if ((1 << disksize) == sectorsize)
-      break;
-  }
-  // Number of blocks
-  uint32_t diskblocks = (uint32_t)(size >> disksize);
-  memset(m_buf, 0, sizeof(m_buf)); 
-  int a = 4;
-  if(dbd == 0) {
-    uint32_t bl = m_img->m_blocksize;
-    uint32_t bc = m_img->m_fileSize / bl;
-    uint8_t c[8] = {
-      0,// Density code
-      bc >> 16, bc >> 8, bc,
-      0, //Reserve
-      bl >> 16, bl >> 8, bl
-    };
-    memcpy(&m_buf[4], c, 8);
-    a += 8;
-    m_buf[3] = 0x08;
-  }
-  switch(pageCode) {
-  case 0x3F:
-  {
-    m_buf[a + 0] = 0x01;
-    m_buf[a + 1] = 0x06;
-    a += 8;
-  }
-  case 0x03:  // drive parameters
-  {
-    m_buf[a + 0] = 0x80 | 0x03; // Page code
-    m_buf[a + 1] = 0x16; // Page length
-    m_buf[a + 2] = (uint8_t)(heads >> 8);// number of sectors / track
-    m_buf[a + 3] = (uint8_t)(heads);// number of sectors / track
-    m_buf[a + 10] = (uint8_t)(sectors >> 8);// number of sectors / track
-    m_buf[a + 11] = (uint8_t)(sectors);// number of sectors / track
-    int size = 1 << disksize;
-    m_buf[a + 12] = (uint8_t)(size >> 8);// number of sectors / track
-    m_buf[a + 13] = (uint8_t)(size);// number of sectors / track
-    a += 24;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  }
-  case 0x04:  // drive parameters
-  {
-      LOGN("AddDrive");
-      m_buf[a + 0] = 0x04; // Page code
-      m_buf[a + 1] = 0x12; // Page length
-      m_buf[a + 2] = (cylinders >> 16);// Cylinder length
-      m_buf[a + 3] = (cylinders >> 8);
-      m_buf[a + 4] = cylinders;
-      m_buf[a + 5] = heads;   // Number of heads
-      a += 20;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  }
-  default:
-    break;
-  }
-  m_buf[0] = a - 1;
-  writeDataPhase(len < a ? len : a, m_buf);
-  return 0x00;
-}
-#else
-uint8_t onModeSenseCommand(uint8_t dbd, int cmd2, uint32_t len)
-{
-  if(!m_img) return 0x02; // No image file
-
-  memset(m_buf, 0, sizeof(m_buf));
-  int pageCode = cmd2 & 0x3F;
-  unsigned int a = 4;
-  if(dbd == 0) {
-    uint32_t bl =  m_img->m_blocksize;
-    uint32_t bc = m_img->m_fileSize / bl;
-
-    uint8_t c[8] = {
-      0,//Density code
-      (uint8_t)(((uint32_t)(bc >> 16))&0xff), (uint8_t)(((uint32_t)(bc >> 8))&0xff), (uint8_t)((bc)&0xff),
-      0, //Reserve
-      (uint8_t)(((uint32_t)(bl >> 16))&0xff), (uint8_t)(((uint32_t)(bl >> 8))&0xff), (uint8_t)((bl)&0xff)
-    };
-    memcpy(&m_buf[4], c, 8);
-    a += 8;
-    m_buf[3] = 0x08;
-  }
-  switch(pageCode) {
-  case 0x3F:
-  case MODEPAGE_RW_ERROR_RECOVERY:
-    if(cmd2 & 0x40) {
-      m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST;
-      m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
-      m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
-      m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-      m_sense[m_id][m_lun].m_key_specific[2] = 0x02;
-      return 0x02;
-    }
-    m_buf[a++] = 0x01; // PS, page id
-    m_buf[a++] = 0x0a; // Page length
-    m_buf[a++] = 0x05; // 
-    m_buf[a++] = 0x00; // Read Retry Count
-    m_buf[a++] = 0x00; // Reserved
-    m_buf[a++] = 0x00; // Reserved
-    m_buf[a++] = 0x00; // Reserved
-    m_buf[a++] = 0x00; // Reserved
-    m_buf[a++] = 0x00; // Write Retry Count
-    m_buf[a++] = 0x00; // Reserved
-    m_buf[a++] = 0x00; // Recovery Time Limit
-    m_buf[a++] = 0x00;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  case 0x03:  //Drive parameters
-    m_buf[a + 0] = 0x03; //Page code
-    m_buf[a + 1] = 0x16; // Page length
-    m_buf[a + 11] = 0x3F;//Number of sectors / track
-    a += 24;
-    if(pageCode != 0x3F) {
-      break;
-    }
-  case 0x04:  //Drive parameters
-    {
-      uint32_t bc = m_img->m_fileSize / m_img->m_file;
-      m_buf[a + 0] = 0x04; //Page code
-      m_buf[a + 1] = 0x16; // Page length
-      m_buf[a + 2] = bc >> 16;// Cylinder length
-      m_buf[a + 3] = bc >> 8;
-      m_buf[a + 4] = bc;
-      m_buf[a + 5] = 1;   //Number of heads
-      a += 24;
-    }
-    if(pageCode != 0x3F) {
-      break;
-    }
-  default:
-    m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST;
-    m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
-    m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
-    m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-    m_sense[m_id][m_lun].m_key_specific[2] = 0x02;
-    return 0x02;
-  }
-  m_buf[0] = a - 1;
-  writeDataPhase(len < a ? len : a, m_buf);
-  return 0x00;
-}
-#endif
-    
-#if SCSI_SELECT == 1
-/*
- * dtc510b_setDriveparameter
- */
-#define PACKED  __attribute__((packed))
-typedef struct PACKED dtc500_cmd_c2_param_struct
-{
-  uint8_t StepPlusWidth;        // Default is 13.6usec (11)
-  uint8_t StepPeriod;         // Default is  3  msec.(60)
-  uint8_t StepMode;         // Default is  Bufferd (0)
-  uint8_t MaximumHeadAdress;      // Default is 4 heads (3)
-  uint8_t HighCylinderAddressuint8_t;  // Default set to 0   (0)
-  uint8_t LowCylinderAddressuint8_t;   // Default is 153 cylinders (152)
-  uint8_t ReduceWrietCurrent;     // Default is above Cylinder 128 (127)
-  uint8_t DriveType_SeekCompleteOption;// (0)
-  uint8_t Reserved8;          // (0)
-  uint8_t Reserved9;          // (0)
-} DTC510_CMD_C2_PARAM;
-
-static void logStrHex(const char *msg,uint32_t num)
-{
-    LOG(msg);
-    LOGHEXN(num);
-}
-
-void DTCsetDriveParameterCommandHandler() {
-  LOGN("[DTC510B setDriveParameter]");
-
-  DTC510_CMD_C2_PARAM DriveParameter;
-  uint16_t maxCylinder;
-  uint16_t numLAD;
-  //uint32_t stepPulseUsec;
-  int StepPeriodMsec;
-
-  // receive paramter
-  writeDataPhase(sizeof(DriveParameter),(uint8_t *)(&DriveParameter));
- 
-  maxCylinder =
-    (((uint16_t)DriveParameter.HighCylinderAddressuint8_t)<<8) |
-    (DriveParameter.LowCylinderAddressuint8_t);
-  numLAD = maxCylinder * (DriveParameter.MaximumHeadAdress+1);
-  //stepPulseUsec  = calcStepPulseUsec(DriveParameter.StepPlusWidth);
-  StepPeriodMsec = DriveParameter.StepPeriod*50;
-  logStrHex (" StepPlusWidth      : ",DriveParameter.StepPlusWidth);
-  logStrHex (" StepPeriod         : ",DriveParameter.StepPeriod   );
-  logStrHex (" StepMode           : ",DriveParameter.StepMode     );
-  logStrHex (" MaximumHeadAdress  : ",DriveParameter.MaximumHeadAdress);
-  logStrHex (" CylinderAddress    : ",maxCylinder);
-  logStrHex (" ReduceWrietCurrent : ",DriveParameter.ReduceWrietCurrent);
-  logStrHex (" DriveType/SeekCompleteOption : ",DriveParameter.DriveType_SeekCompleteOption);
-  logStrHex (" Maximum LAD        : ",numLAD-1);
-  m_sts = 0;
-  m_phase = PHASE_STATUSIN;
-}
-#endif
-
-/*
- * MsgIn2.
- */
-void MsgIn2(int msg)
-{
-  //LOGN("MsgIn2");
-  SET_MSG_ACTIVE();
-  SCSI_OUT(CD , true);
-  SCSI_OUT(IO , true);
-  writeHandshake(msg);
-}
-
-/*
- * MsgOut2.
- */
-void MsgOut2()
-{
-  //LOGN("MsgOut2");
-  SET_MSG_ACTIVE();
-  SCSI_OUT(CD , true);
-  SCSI_OUT(IO , false);
-  m_msb[m_msc] = readHandshake();
-  m_msc++;
-  m_msc %= 256;
-}
-
-/*
- * Main loop.
- */
-void loop() {
-  switch(m_phase) {
-    default:
-    case PHASE_BUSFREE:
-      BusFreePhaseHandler();
-      break;
-    case PHASE_SELECTION:
-      SelectionPhaseHandler();
-      break;
-    case PHASE_MESSAGEOUT:
-      MessageOutPhaseHandler();
-      break;
-    case PHASE_COMMAND:
-      CommandPhaseHandler();
-      break;
-    case PHASE_STATUSIN:
-      StatusInPhaseHandler();
-      break;
-    case PHASE_MESSAGEIN:
-      MessageInPhaseHandler();
-      break;
-  }
-}
-
-void SelectionPhaseHandler() {
-  m_msg = 0;
-
-  // Wait until RST = H, BSY = H, SEL = L
-  do {} while( SCSI_IN(BSY) || !SCSI_IN(SEL) || SCSI_IN(RST));
-
-  // BSY+ SEL-
-  // If the ID to respond is not driven, wait for the next
-  //uint8_t db = readIO();
-  //uint8_t scsiid = db & scsi_id_mask;
-  uint8_t scsiid = readIO() & scsi_id_mask;
-  if((scsiid) == 0) {
-    return;
-  }
-  LOG("Selection ");
-  m_isBusReset = false;
-  // Set BSY to-when selected
-  SCSI_BSY_ACTIVE();     // Turn only BSY output ON, ACTIVE
-
-  // Ask for a TARGET-ID to respond
-#if USE_DB2ID_TABLE
-  m_id = db2scsiid[scsiid];
-  //if(m_id==0xff) return;
-#else
-  for(m_id=7;m_id>=0;m_id--)
-    if(scsiid & (1<<m_id)) break;
-  //if(m_id<0) return;
-#endif
-  m_lun = 0xff;
-
-  //LOGN("Wait !SEL");
-  // Wait until SEL becomes inactive
-  while(!digitalRead(SEL) && digitalRead(BSY)) {
-    if(m_isBusReset) {
-      LOGN("!SEL");
-      m_phase = PHASE_BUSFREE;
-      return;
-    }
-  }
-  m_phase = PHASE_MESSAGEOUT;
-}
-
-void MessageOutPhaseHandler() {
-  //LOGN("ACTIVE");
-  SCSI_TARGET_ACTIVE();  // (BSY), REQ, MSG, CD, IO output turned on
-  //  
-  if(!digitalRead(ATN)) {
-    //LOGN("ATN");
-    bool syncenable = false;
-    int syncperiod = 50;
-    int syncoffset = 0;
-    int loopWait = 0;
-    m_msc = 0;
-    memset(m_msb, 0x00, sizeof(m_msb));
-    while(!digitalRead(ATN) && loopWait < 255) {
-      MsgOut2();
-      loopWait++;
-    }
-    for(int i = 0; i < m_msc; i++) {
-      // ABORT
-      if (m_msb[i] == 0x06) {
-        m_phase = PHASE_BUSFREE;
-        return;
-      }
-      // BUS DEVICE RESET
-      if (m_msb[i] == 0x0C) {
-        syncoffset = 0;
-        m_phase = PHASE_BUSFREE;
-        return;
-      }
-      // IDENTIFY message overrides LUN IDs specified in commands.
-      if (m_msb[i] & 0x80) {
-        m_lun = m_msb[i] & 0x07;
-      }
-      // Extended message
-      if (m_msb[i] == 0x01) {
-        // Check only when synchronous transfer is possible
-        if (!syncenable || m_msb[i + 2] != 0x01) {
-          MsgIn2(0x07);
-          break;
-        }
-        // Transfer period factor(50 x 4 = Limited to 200ns)
-        syncperiod = m_msb[i + 3];
-        if (syncperiod > 50) {
-          syncperiod = 50;
-        }
-        // REQ/ACK offset(Limited to 16)
-        syncoffset = m_msb[i + 4];
-        if (syncoffset > 16) {
-          syncoffset = 16;
-        }
-        // STDR response message generation
-        MsgIn2(0x01);
-        MsgIn2(0x03);
-        MsgIn2(0x01);
-        MsgIn2(syncperiod);
-        MsgIn2(syncoffset);
-        break;
-      }
-    }
-  }
-  m_phase = PHASE_COMMAND;
-}
-
-void CommandPhaseHandler() {
-  LOG("Command:");
-  SET_MSG_INACTIVE();
-  SCSI_OUT(CD , true);
-  SCSI_OUT(IO , false);
-  
-  m_cmd[0] = readHandshake();
-  if(m_isBusReset) {
-    m_phase = PHASE_BUSFREE;
-    return;
-  }
-  LOGHEX2(m_cmd[0]);
-
-  // Command length selection, reception
-  static const int cmd_class_len[8]={6,10,10,6,6,12,6,6};
-  m_cmdlen = cmd_class_len[m_cmd[0] >> 5];
-
-  // Receive the remaining command bytes
-  for(int i = 1; i < m_cmdlen; i++ ) {
-    m_cmd[i] = readHandshake();
-    LOG(":");
-    LOGHEX2(m_cmd[i]);
-    if(m_isBusReset) {
-      m_phase = PHASE_BUSFREE;
-      return;
-    }
-  }
-
-  // LUN confirmation
-  m_sts = m_cmd[1]&0xe0;      // Preset LUN in status byte if not specified by Identify Message
-  if(m_lun == 0xff) m_lun = (m_sts>>5);
-
-  // HDD Image selection
-  m_img = (HDDIMG *)0; // None
-  if( (m_lun < NUM_SCSILUN) )
-  {
-    m_img = &(img[m_id][m_lun]); // There is an image
-    if(!(m_img->m_file.isOpen()))
-      m_img = (HDDIMG *)0;       // Image absent
-  }
-  
-  LOG(":ID ");
-  LOG(m_id);
-  LOG(":LUN ");
-  LOG(m_lun);
-
-  LOG(" ");
-
-  if(!m_img) {
-    m_badlunhandler[m_cmd[0]]();
-  } else {
-    m_handler[m_id][m_cmd[0]]();
-  }
-}
-
-void ConfigureBadLunHandlers() {
-  for(int c = 0; c < 256; c++)
-    m_badlunhandler[c] = &BadLunCommandHandler;
-
-  m_badlunhandler[0x03] = &RequestSenseCommandHandler;
-  m_badlunhandler[0x12] = &InquiryCommandHandler;
-}
-
-void StatusInPhaseHandler() {
-  //LOGN("Sts");
-  SET_MSG_INACTIVE();
-  SCSI_OUT(CD , true);
-  SCSI_OUT(IO , true);
-  writeHandshake(m_sts);
-  m_phase = PHASE_MESSAGEIN;
-}
-
-void MessageInPhaseHandler() {
-  //LOGN("MsgIn");
-  SET_MSG_ACTIVE();
-  SCSI_OUT(CD , true);
-  SCSI_OUT(IO , true);
-  writeHandshake(m_msg);
-  m_phase = PHASE_BUSFREE;
-}
-
-void BusFreePhaseHandler() {
-  //LOGN("BusFree");
-  m_isBusReset = false;
-  SCSI_TARGET_INACTIVE();
-  m_phase = PHASE_SELECTION;
-}
diff --git a/src/greenscsi.ino b/src/greenscsi.ino
new file mode 100755
index 0000000..cf65516
--- /dev/null
+++ b/src/greenscsi.ino
@@ -0,0 +1,893 @@
+/*  
+ *  GreenSCSI
+ *  Copyright (c) 2021  David Kuder
+ *  
+ *  Based on BlueSCSI
+ *  Copyright (c) 2021  Eric Helgeson, Androda
+ *  
+ *  This file is free software: you may copy, redistribute and/or modify it  
+ *  under the terms of the GNU General Public License as published by the  
+ *  Free Software Foundation, either version 2 of the License, or (at your  
+ *  option) any later version.  
+ *  
+ *  This file is distributed in the hope that it will be useful, but  
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of  
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU  
+ *  General Public License for more details.  
+ *  
+ *  You should have received a copy of the GNU General Public License  
+ *  along with this program.  If not, see https://github.com/erichelgeson/bluescsi.  
+ *  
+ * This file incorporates work covered by the following copyright and  
+ * permission notice:  
+ *  
+ *     Copyright (c) 2019 komatsu   
+ *  
+ *     Permission to use, copy, modify, and/or distribute this software  
+ *     for any purpose with or without fee is hereby granted, provided  
+ *     that the above copyright notice and this permission notice appear  
+ *     in all copies.  
+ *  
+ *     THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL  
+ *     WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED  
+ *     WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE  
+ *     AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR  
+ *     CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS  
+ *     OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,  
+ *     NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN  
+ *     CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.  
+ */
+
+#include <Arduino.h> // For Platform.IO
+#include <SPI.h>
+#include <SdFat.h>
+#include "sdios.h"
+#include "config.h"
+#include "scsi_defs.h"
+#include "cmd.h"
+
+// SDFAT
+SdFs sd;
+
+#if DEBUG == 1
+#define LOG(XX)      Serial.print(XX)
+#define LOGHEX2(XX)  Serial.printf("%02x", XX)
+#define LOGHEX4(XX)  Serial.printf("%04x", XX)
+#define LOGHEX6(XX)  Serial.printf("%06x", XX)
+#define LOGHEX8(XX)  Serial.printf("%08x", XX)
+#define LOGN(XX)     Serial.println(XX)
+#define LOGHEX2N(XX) Serial.printf("%02x\r\n", XX)
+#define LOGHEX4N(XX) Serial.printf("%04x\r\n", XX)
+#define LOGHEX6N(XX) Serial.printf("%06x\r\n", XX)
+#define LOGHEX8N(XX) Serial.printf("%08x\r\n", XX)
+#elif DEBUG == 2
+#define LOG(XX)      LOG_FILE.print(XX); LOG_FILE.sync();
+#define LOGHEX2(XX)  LOG_FILE.printf("%02x", XX); LOG_FILE.sync();
+#define LOGHEX4(XX)  LOG_FILE.printf("%04x", XX); LOG_FILE.sync();
+#define LOGHEX6(XX)  LOG_FILE.printf("%06x", XX); LOG_FILE.sync();
+#define LOGHEX8(XX)  LOG_FILE.printf("%08x", XX); LOG_FILE.sync();
+#define LOGN(XX)     LOG_FILE.println(XX); LOG_FILE.sync();
+#define LOGHEX2N(XX) LOG_FILE.printf("%02x\r\n", XX); LOG_FILE.sync();
+#define LOGHEX4N(XX) LOG_FILE.printf("%04x\r\n", XX); LOG_FILE.sync();
+#define LOGHEX6N(XX) LOG_FILE.printf("%06x\r\n", XX); LOG_FILE.sync();
+#define LOGHEX8N(XX) LOG_FILE.printf("%08x\r\n", XX); LOG_FILE.sync();
+#else
+#define LOG(XX)      //Serial.print(XX)
+#define LOGHEX2(XX)  //Serial.printf("%02x", XX)
+#define LOGHEX4(XX)  //Serial.printf("%04x", XX)
+#define LOGHEX6(XX)  //Serial.printf("%06x", XX)
+#define LOGHEX8(XX)  //Serial.printf("%08x", XX)
+#define LOGN(XX)     //Serial.println(XX)
+#define LOGHEX2N(XX) //Serial.printf("%02x\r\n", XX)
+#define LOGHEX4N(XX) //Serial.printf("%04x\r\n", XX)
+#define LOGHEX6N(XX) //Serial.printf("%06x\r\n", XX)
+#define LOGHEX8N(XX) //Serial.printf("%08x\r\n", XX)
+#endif
+
+#define DB0       0     // SCSI:DB0  Port B Bit 16
+#define DB1       1     // SCSI:DB1  Port B Bit 17
+#define DB2       29    // SCSI:DB2  Port B Bit 18
+#define DB3       30    // SCSI:DB3  Port B Bit 19
+#define DB4       43    // SCSI:DB4  Port B Bit 20
+#define DB5       46    // SCSI:DB5  Port B Bit 21
+#define DB6       44    // SCSI:DB6  Port B Bit 22
+#define DB7       45    // SCSI:DB7  Port B Bit 23
+#define DB8       32    // SCSI:DBP  Port B Bit 11
+
+#define ATN       12    // SCSI:ATN  Port C Bit 7
+#define BSY       10    // SCSI:BSY  Port C Bit 4
+#define ACK       9     // SCSI:ACK  Port C Bit 3
+#define RST       8     // SCSI:RST  Port D Bit 3
+#define MSG       7     // SCSI:MSG  Port D Bit 2
+#define SEL       6     // SCSI:SEL  Port D Bit 4
+#define CD        5     // SCSI:C/D  Port D Bit 7
+#define REQ       4     // SCSI:REQ  Port A Bit 13
+#define IO        3     // SCSI:I/O  Port A Bit 12
+
+#define SD_CS     BUILTIN_SDCARD      // SDCARD:CS
+#define LED       13     // LED  Port C Bit 5
+
+// LED control
+#define LED_ON()       { GPIOC_PSOR = (1 << 5); }
+#define LED_OFF()      { GPIOC_PCOR = (1 << 5); }
+
+#define SET_REQ_ACTIVE()    { GPIOA_PCOR = (1 << 13); }
+#define SET_REQ_INACTIVE()  { GPIOA_PSOR = (1 << 13); }
+#define SET_MSG_ACTIVE()    { GPIOD_PCOR = (1 << 2); }
+#define SET_MSG_INACTIVE()  { GPIOD_PSOR = (1 << 2); }
+//#define SET_CD_ACTIVE()     { GPIOD_PCOR = (1 << 7); }
+//#define SET_CD_INACTIVE()   { GPIOD_PSOR = (1 << 7); }
+//#define SET_IO_ACTIVE()     { GPIOA_PCOR = (1 << 12); }
+//#define SET_IO_INACTIVE()   { GPIOA_PSOR = (1 << 12); }
+
+//#define SET_REQ_ACTIVE()     { pinMode(REQ, OUTPUT_OPENDRAIN);  digitalWrite(REQ, LOW); }
+//#define SET_REQ_INACTIVE()   { digitalWrite(REQ, HIGH); pinMode(REQ, INPUT); }
+//#define SET_MSG_ACTIVE()     { pinMode(MSG, OUTPUT_OPENDRAIN); digitalWrite(MSG, LOW); }
+//#define SET_MSG_INACTIVE()   { digitalWrite(MSG, HIGH); pinMode(MSG, INPUT); }
+#define SET_CD_ACTIVE()     { pinMode(CD, OUTPUT_OPENDRAIN);  digitalWrite(CD, LOW); }
+#define SET_CD_INACTIVE()   { digitalWrite(CD, HIGH); pinMode(CD, INPUT); }
+#define SET_IO_ACTIVE()     { pinMode(IO, OUTPUT_OPENDRAIN); digitalWrite(IO, LOW); }
+#define SET_IO_INACTIVE()   { digitalWrite(IO, HIGH); pinMode(IO, INPUT); }
+//#define SET_BSY_ACTIVE()     { pinMode(BSY, OUTPUT_OPENDRAIN); digitalWrite(BSY, LOW); }
+//#define SET_BSY_INACTIVE()   { digitalWrite(BSY, HIGH); pinMode(BSY, INPUT); }
+
+#define SET_BSY_ACTIVE()   { GPIOC_PCOR = (1 << 4); }
+#define SET_BSY_INACTIVE() { GPIOC_PSOR = (1 << 4); }
+
+#define GET_ACK() (!(GPIOC_PDIR & (1 << 3)))
+#define GET_ATN() (!(GPIOC_PDIR & (1 << 7)))
+#define GET_BSY() (!(GPIOC_PDIR & (1 << 4)))
+#define GET_RST() (!(GPIOD_PDIR & (1 << 3)))
+#define GET_SEL() (!(GPIOD_PDIR & (1 << 4)))
+
+// Turn on the output only for BSY
+// BSY,REQ,MSG,CD,IO Turn on the output (no change required for OD)
+#define SCSI_TARGET_ACTIVE()   { }
+// BSY,REQ,MSG,CD,IO Turn off output
+#define SCSI_TARGET_INACTIVE() { SCSI_DB_INPUT(); SET_REQ_INACTIVE(); SET_MSG_INACTIVE(); SET_CD_INACTIVE(); SET_IO_INACTIVE(); SET_BSY_INACTIVE(); }
+
+// HDImage file
+#define HDIMG_ID_POS  2                 // Position to embed ID number
+#define HDIMG_LUN_POS 3                 // Position to embed LUN numbers
+#define HDIMG_BLK_POS 5                 // Position to embed block size numbers
+#define MAX_FILE_PATH 32                // Maximum file name length
+
+typedef struct m_sense_s {
+  uint8_t     m_key;
+  uint8_t     m_code;
+  uint8_t     m_key_specific[4];
+} msense_t;
+
+typedef void (*CommandHandler_t)();
+CommandHandler_t m_badlunhandler[256];
+
+// VirtualDevice
+typedef struct VirtualDevice_s
+{
+  boolean     m_enabled;
+  uint8_t     m_id;
+  uint8_t     m_lun;
+  uint8_t     m_type;                 // Device Type
+  char        m_filename[MAX_FILE_PATH+1];
+  FsFile      m_file;                 // File object
+  uint64_t    m_fileSize;             // File size
+  size_t      m_blocksize;            // SCSI BLOCK size
+#if SUPPORT_TAPE
+  size_t      m_filemarks;            // Tape position counter (file marks since BOM)
+#endif
+  uint8_t     m_inquiryresponse[96];
+  CommandHandler_t m_handler[256];
+  msense_t    m_sense;
+} VirtualDevice_t;
+
+VirtualDevice_t  m_vdev[NUM_VDEV];    // Maximum number
+uint8_t       m_vdevcnt = 0;          // Number of allocated vdevs
+uint8_t       m_vdevmap[NUM_SCSIID][NUM_SCSILUN]; // Map ID/LUN to a vdev
+
+volatile bool m_isBusReset = false;   // Bus reset
+
+uint8_t       scsi_id_mask;           // Mask list of responding SCSI IDs
+uint8_t       m_id;                   // Currently responding SCSI-ID
+uint8_t       m_lun;                  // Logical unit number currently responding
+uint8_t       m_sts;                  // Status uint8_t
+uint8_t       m_msg;                  // Message uint8_ts
+VirtualDevice_t *m_sel;               // VirtualDevice for current SCSI-ID, LUN
+uint8_t       m_buf[MAX_BLOCKSIZE+1]; // General purpose buffer + overrun fetch
+int           m_msc;
+uint8_t       m_msb[256];             // Command storage uint8_ts
+uint8_t       m_cmdlen;
+uint8_t       m_cmd[12];
+uint8_t       m_responsebuffer[256];
+#if SUPPORT_SASI
+boolean       m_sasi_mode = SUPPORT_SASI_DEFAULT;
+#endif
+
+uint16_t ledbits = 0;
+uint8_t ledbit = 0;
+
+typedef enum {
+  PHASE_BUSFREE = 0,
+  PHASE_SELECTION,
+  PHASE_MESSAGEOUT,
+  PHASE_COMMAND,
+  PHASE_STATUSIN,
+  PHASE_MESSAGEIN,
+} phase_t;
+phase_t       m_phase = PHASE_BUSFREE;
+
+// Log File
+#define VERSION "1.2-20211126"
+#if DEBUG == 2
+#define LOG_FILENAME "LOG.txt"
+FsFile LOG_FILE;
+#endif
+
+
+void onBusReset(void);
+void initFileLog(void);
+void finalizeFileLog(void);
+
+/*
+ * IO read.
+ */
+inline uint8_t readIO(void)
+{
+  // Port input data register
+  uint32_t ret = ~GPIOB_PDIR;
+  //uint8_t bret = ret >> 16;
+#if READ_PARITY_CHECK
+  if((db_bsrr[bret] ^ ret) & 0x0800)
+    m_sts |= 0x01; // parity error
+#endif
+
+  return ret >> 16; //bret;
+}
+
+boolean OpenImage(VirtualDevice_t *h, const char *image_name)
+{
+  h->m_fileSize = 0;
+  h->m_file = sd.open(image_name, O_RDWR);
+  if(h->m_file.isOpen()) {
+    h->m_fileSize = h->m_file.size();
+    return true; // File opened
+  }
+  return false;
+}
+
+#if SUPPORT_DISK or SUPPORT_OPTICAL
+/*
+ * Open HDD or CDROM image file
+ */
+boolean OpenDiskImage(VirtualDevice_t *h, const char *image_name, int blocksize)
+{
+  if(!strncmp(image_name, "/sd/", 4))
+    image_name += 3;
+
+  h->m_fileSize = 0;
+  h->m_blocksize = blocksize;
+  h->m_file = sd.open(image_name, O_RDWR);
+  if(h->m_file.isOpen())
+  {
+    h->m_fileSize = h->m_file.size();
+    LOG(" Imagefile: ");
+    LOG(image_name);
+    if(h->m_fileSize>0)
+    {
+      // check blocksize dummy file
+      LOG(" / ");
+      LOG(h->m_fileSize / h->m_blocksize);
+      LOG(" sectors / ");
+      LOG(h->m_fileSize / 1024);
+      LOG(" KiB / ");
+      LOG(h->m_fileSize / 1024 / 1024);
+      LOGN(" MiB");
+      return true; // File opened
+    }
+    else
+    {
+      h->m_file.close();
+      h->m_fileSize = h->m_blocksize = 0; // no file
+      LOGN(" FileSizeError");
+    }
+  }
+  return false;
+}
+#endif /* SUPPORT_DISK or SUPPORT_OPTICAL */
+
+#if SUPPORT_TAPE
+/*
+ * Open Tape image file
+ */
+boolean OpenTapeImage(VirtualDevice_t *h, const char *image_name)
+{
+  h->m_fileSize = 0;
+  h->m_blocksize = 0;
+  h->m_filemarks = 0;
+  h->m_file = sd.open(image_name, O_RDWR);
+  if(h->m_file.isOpen()) {
+    h->m_fileSize = h->m_file.size();
+    h->m_blocksize = 1024;
+    LOG(" Imagefile: ");
+    LOG(image_name);
+    if(h->m_fileSize>0) {
+      // check blocksize dummy file
+      LOG(" / ");
+      LOG(h->m_fileSize / 1024);
+      LOG(" KiB / ");
+      LOG(h->m_fileSize / 1024 / 1024);
+      LOGN(" MiB");
+    } else {
+      LOGN(" / Blank Tape");
+    }
+    return true; // File opened
+  }
+  return false;
+}
+#endif /* SUPPORT_TAPE */
+
+void pinModeFastSlew(uint8_t pin, int dummy) {
+  pinMode(pin, OUTPUT_OPENDRAIN);
+  volatile uint32_t *config = portConfigRegister(pin);
+  *config = PORT_PCR_ODE | PORT_PCR_DSE | PORT_PCR_MUX(1);
+}
+
+/*
+ * Initialization.
+ *  Initialize the bus and set the PIN orientation
+ */
+void setup()
+{
+#if DEBUG > 0
+  Serial.begin(115200);
+
+  delay(1500);
+#endif
+
+  for(int id = 0; id < NUM_SCSIID; id++)
+    for(int lun = 0; lun < NUM_SCSILUN; lun++)
+      m_vdevmap[id][lun] = 0xff;
+  m_vdevcnt = 0;
+
+  // PIN initialization
+  pinMode(LED, OUTPUT);
+  LED_OFF();
+
+  //GPIO(SCSI BUS)Initialization
+
+  // Input port
+  pinMode(ATN, INPUT_PULLUP);
+  pinMode(ACK, INPUT_PULLUP);
+  pinMode(RST, INPUT_PULLUP);
+  pinMode(SEL, INPUT_PULLUP);
+
+  // Output port
+  pinModeFastSlew(BSY, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(MSG, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(CD, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(IO, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(REQ, OUTPUT_OPENDRAIN);
+
+  pinModeFastSlew(DB0, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB1, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB2, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB3, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB4, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB5, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB6, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB7, OUTPUT_OPENDRAIN);
+  pinModeFastSlew(DB8, OUTPUT_OPENDRAIN);
+
+  // DB and DP are input modes
+  SCSI_DB_INPUT();
+
+  // Turn off the output port
+  SCSI_TARGET_INACTIVE();
+
+  //Occurs when the RST pin state changes from HIGH to LOW
+  attachInterrupt(RST, onBusReset, FALLING);
+  attachInterrupt(SEL, SelectionPhaseISR, FALLING);
+
+  LED_ON();
+
+  if(!sd.begin(SdioConfig(FIFO_SDIO))) {
+#if DEBUG > 0
+    Serial.println("SD initialization failed!");
+#endif
+    ledbits = 0b0000001010101010;
+  }
+  initFileLog();
+  ConfigureBadLunHandlers();
+
+  //Sector data overrun uint8_t setting
+  //m_buf[MAX_BLOCKSIZE] = 0xff; // DB0 all off,DBP off
+  //HD image file open
+  scsi_id_mask = 0x00;
+
+  // If greenscsi.cfg exists, run it
+  if(sd.exists("/greenscsi.cfg"))
+    execHandler((char*)"/sd/greenscsi.cfg");
+
+  // Scan for images if we haven't defined any targets yet.
+  if(m_vdevcnt == 0) findImages();
+
+  finalizeFileLog();
+  LED_OFF();
+
+  cmdDisplay();
+}
+
+void findImages() {
+  // Iterate over the root path in the SD card looking for candidate image files.
+  SdFile root;
+  root.open("/");
+  SdFile file;
+  bool imageReady;
+  int usedDefaultId = 0;
+  while (1) {
+    if (!file.openNext(&root, O_READ)) break;
+    char name[MAX_FILE_PATH+1];
+    if(!file.isDir()) {
+      file.getName(name, MAX_FILE_PATH+1);
+      file.close();
+      String file_name = String(name);
+      file_name.toLowerCase();
+
+#if SUPPORT_DISK or SUPPORT_OPTICAL
+      if((file_name.startsWith("hd") || file_name.startsWith("cd")) && (file_name.endsWith(".img") || file_name.endsWith(".hda"))) {
+        // Defaults for Hard Disks and CD-ROMs
+        int id  = 1; // 0 and 3 are common in Macs for physical HD and CD, so avoid them.
+        int lun = 0;
+        int blk = 512;
+
+        // Positionally read in and coerase the chars to integers.
+        // We only require the minimum and read in the next if provided.
+        int file_name_length = file_name.length();
+        if(file_name_length > 2) { // HD[N]
+          int tmp_id = file_name[HDIMG_ID_POS] - '0';
+
+          if(tmp_id > -1 && tmp_id < NUM_SCSIID) {
+            id = tmp_id; // If valid id, set it, else use default
+          } else {
+            usedDefaultId++;
+          }
+        }
+        if(file_name_length > 3) { // HD0[N]
+          int tmp_lun = file_name[HDIMG_LUN_POS] - '0';
+
+          if(tmp_lun > -1 && tmp_lun < NUM_SCSILUN) {
+            lun = tmp_lun; // If valid id, set it, else use default
+          }
+        }
+
+        int blk1 = 0, blk2 = 0, blk3 = 0, blk4 = 0;
+        if(file_name_length > 8) { // HD00_[111]
+          blk1 = name[HDIMG_BLK_POS] - '0';
+          blk2 = name[HDIMG_BLK_POS+1] - '0';
+          blk3 = name[HDIMG_BLK_POS+2] - '0';
+          if(file_name_length > 9) // HD00_NNN[1]
+            blk4 = name[HDIMG_BLK_POS+3] - '0';
+        }
+        if(blk1 == 2 && blk2 == 5 && blk3 == 6) {
+          blk = 256;
+        } else if(blk1 == 1 && blk2 == 0 && blk3 == 2 && blk4 == 4) {
+          blk = 1024;
+        } else if(blk1 == 2 && blk2 == 0 && blk3 == 4 && blk4 == 8) {
+          blk  = 2048;
+        }
+
+        if(id < NUM_SCSIID && lun < NUM_SCSILUN) {
+          uint8_t v = m_vdevcnt;
+          VirtualDevice_t *h = &m_vdev[v];
+          m_vdevmap[id][lun] = v;
+          h->m_id = id;
+          h->m_lun = lun;
+          imageReady = false;
+
+          // Configure Handlers and Inquiry Response for all luns
+#if SUPPORT_DISK
+          if(file_name.startsWith("hd")) {
+            LOG(" Disk at ID ");
+            LOG(id);
+            LOG(":LUN ");
+            LOG(lun);
+            ConfigureDisk(h, name);
+            imageReady = OpenDiskImage(h, name, blk);
+          }
+#endif /* SUPPORT_DISK */
+#if SUPPORT_OPTICAL
+          if(file_name.startsWith("cd")) {
+            LOG("CDROM at ID ");
+            LOG(id);
+            LOG(":LUN ");
+            LOG(lun);
+            ConfigureOptical(h, name);
+            imageReady = OpenDiskImage(h, name, blk);
+          }
+#endif /* SUPPORT_OPTICAL */
+          if(imageReady) { // Marked as a responsive ID
+            m_vdevcnt++;
+            sprintf(h->m_filename, "/sd/%s", name);
+            h->m_enabled = true;
+            scsi_id_mask |= 1<<id;
+          }
+        } else {
+          LOG("Bad LUN or SCSI id for image: ");
+          LOGN(name);
+        }
+      } else
+#endif /* SUPPORT_DISK or SUPPORT_OPTICAL */
+#if SUPPORT_TAPE
+      if(file_name.startsWith("dt") && file_name.endsWith(".tap")) {
+        // Defaults for Tapes
+        int id  = 1; // 0 and 3 are common in Macs for physical HD and CD, so avoid them.
+        int lun = 0;
+
+        // Positionally read in and coerase the chars to integers.
+        // We only require the minimum and read in the next if provided.
+        int file_name_length = file_name.length();
+        if(file_name_length > 2) { // HD[N]
+          int tmp_id = file_name[HDIMG_ID_POS] - '0';
+
+          if(tmp_id > -1 && tmp_id < NUM_SCSIID) {
+            id = tmp_id; // If valid id, set it, else use default
+          } else {
+            usedDefaultId++;
+          }
+        }
+        if(file_name_length > 3) { // HD0[N]
+          int tmp_lun = file_name[HDIMG_LUN_POS] - '0';
+
+          if(tmp_lun > -1 && tmp_lun < NUM_SCSILUN) {
+            lun = tmp_lun; // If valid id, set it, else use default
+          }
+        }
+
+        if(id < NUM_SCSIID && lun < NUM_SCSILUN) {
+          uint8_t v = m_vdevcnt;
+          VirtualDevice_t *h = &m_vdev[v];
+          m_vdevmap[id][lun] = v;
+          h->m_id = id;
+          h->m_lun = lun;
+          imageReady = false;
+
+          LOG(" Tape at ID ");
+          LOG(id);
+          LOG(":LUN ");
+          LOG(lun);
+          ConfigureTape(h, name);
+          imageReady = OpenTapeImage(h, name);
+          if(imageReady) { // Marked as a responsive ID
+            m_vdevcnt++;
+            h->m_enabled = true;
+            sprintf(h->m_filename, "/sd/%s", name);
+            scsi_id_mask |= 1<<id;
+          }
+        } else {
+          LOG("Bad LUN or SCSI id for image: ");
+          LOGN(name);
+        }
+      } else 
+#endif /* SUPPORT_TAPE */
+      {
+//          LOG("Not an image: ");
+//          LOGN(name);
+      }
+    } else {
+      file.close();
+    }
+  }
+  if(usedDefaultId > 0) {
+    LOGN("!! More than one image did not specify a SCSI ID. Last file will be used at ID 1. !!");
+  }
+  if(m_vdevcnt == 0) {
+    LOGN("!! No Virtual Devices defined. Please manually configure. !!");
+  }
+  root.close();
+}
+
+/*
+ * Setup initialization logfile
+ */
+void initFileLog() {
+#if DEBUG == 2
+  LOG_FILE = sd.open(LOG_FILENAME, O_WRONLY | O_CREAT | O_TRUNC);
+#endif
+#if DEBUG
+  LOGN("GreenSCSI <-> SD - https://github.com/dkgrizzly/GreenSCSI");
+  LOG("VERSION: ");
+  LOGN(VERSION);
+  LOG("DEBUG:");
+  LOGN(DEBUG);
+#endif
+}
+
+/*
+ * Finalize initialization logfile
+ */
+void finalizeFileLog() {
+  LOGN("Initialization complete.");
+#if DEBUG == 2
+  LOG_FILE.close();
+#endif
+}
+
+
+/*
+ * MsgIn2.
+ */
+void MsgIn2(int msg)
+{
+  //LOGN("MsgIn2");
+  SET_MSG_ACTIVE();
+  SET_CD_ACTIVE();
+  SET_IO_ACTIVE();
+  writeHandshake(msg);
+}
+
+/*
+ * MsgOut2.
+ */
+void MsgOut2()
+{
+  //LOGN("MsgOut2");
+  SET_MSG_ACTIVE();
+  SET_CD_ACTIVE();
+  SET_IO_INACTIVE();
+  m_msb[m_msc] = readHandshake();
+  m_msc++;
+  m_msc %= 256;
+}
+
+/*
+ * Main loop.
+ */
+elapsedMillis ticktock;
+
+void loop() {
+  if(ledbits) {
+    if(ticktock > 250) {
+      if(ledbits & (1<<ledbit)) {
+        LED_ON();
+      } else {
+        LED_OFF();
+      }
+
+      ledbit++;
+      ledbit &= 0xf;
+
+      ticktock = 0;
+    }
+
+    // Cancel ID error indication if we have any IDs enabled
+    if((ledbit == 0) && (ledbits == 0b0000000000101010) && (scsi_id_mask != 0))
+      ledbits = 0;
+  }
+  if(!ledbits) {
+    if(scsi_id_mask == 0) ledbits = 0b0000000000101010;
+  }
+
+  switch(m_phase) {
+    default:
+    case PHASE_BUSFREE:
+      BusFreePhaseHandler();
+      break;
+    case PHASE_SELECTION:
+      //SelectionPhaseHandler();
+
+      // Do other work while we wait for SEL to be asserted
+      cmdPoll();
+      break;
+    case PHASE_MESSAGEOUT:
+      MessageOutPhaseHandler();
+      break;
+    case PHASE_COMMAND:
+      CommandPhaseHandler();
+      break;
+    case PHASE_STATUSIN:
+      StatusInPhaseHandler();
+      break;
+    case PHASE_MESSAGEIN:
+      MessageInPhaseHandler();
+      break;
+  }
+}
+
+void SelectionPhaseISR() {
+  m_msg = 0;
+
+  // Wait until RST = H, BSY = H, SEL = L
+  while(GET_BSY() || GET_RST()) {
+    if(!GET_SEL()) return;
+  }
+
+  // BSY+ SEL-
+  // If the ID to respond is not driven, wait for the next
+  uint8_t scsiid = readIO() & scsi_id_mask;
+  if(scsiid == 0) {
+    // We were not selected, treat this as an abort and cancel any transactions we can.
+    m_isBusReset = true;
+    SCSI_TARGET_INACTIVE();
+    m_phase = PHASE_BUSFREE;
+    return;
+  }
+  LOG("Selection ");
+  m_isBusReset = false;
+  // Set BSY to-when selected
+  SET_BSY_ACTIVE();     // Turn only BSY output ON, ACTIVE
+
+  // Ask for a TARGET-ID to respond
+#if USE_DB2ID_TABLE
+  m_id = db2scsiid[scsiid];
+#else
+  for(m_id=7;m_id>=0;m_id--)
+    if(scsiid & (1<<m_id)) break;
+#endif
+  m_lun = 0xff;
+
+  //LOGN("Wait !SEL");
+  // Wait until SEL becomes inactive
+  while(GET_SEL() && !GET_BSY()) {
+    if(m_isBusReset) {
+      LOGN("!SEL");
+      m_phase = PHASE_BUSFREE;
+      return;
+    }
+  }
+  m_phase = PHASE_MESSAGEOUT;
+}
+
+void MessageOutPhaseHandler() {
+  //LOGN("ACTIVE");
+  SCSI_TARGET_ACTIVE();  // (BSY), REQ, MSG, CD, IO output turned on
+  //  
+  if(GET_ATN()) {
+    //LOGN("ATN");
+    bool syncenable = false;
+    int syncperiod = 50;
+    int syncoffset = 0;
+    int loopWait = 0;
+    m_msc = 0;
+    memset(m_msb, 0x00, sizeof(m_msb));
+    while(GET_ATN() && loopWait < 255) {
+      MsgOut2();
+      loopWait++;
+    }
+    for(int i = 0; i < m_msc; i++) {
+      // ABORT
+      if (m_msb[i] == 0x06) {
+        m_phase = PHASE_BUSFREE;
+        return;
+      }
+      // BUS DEVICE RESET
+      if (m_msb[i] == 0x0C) {
+        syncoffset = 0;
+        m_phase = PHASE_BUSFREE;
+        return;
+      }
+      // IDENTIFY message overrides LUN IDs specified in commands.
+      if (m_msb[i] & 0x80) {
+        m_lun = m_msb[i] & 0x07;
+      }
+      // Extended message
+      if (m_msb[i] == 0x01) {
+        // Check only when synchronous transfer is possible
+        if (!syncenable || m_msb[i + 2] != 0x01) {
+          MsgIn2(0x07);
+          break;
+        }
+        // Transfer period factor(50 x 4 = Limited to 200ns)
+        syncperiod = m_msb[i + 3];
+        if (syncperiod > 50) {
+          syncperiod = 50;
+        }
+        // REQ/ACK offset(Limited to 16)
+        syncoffset = m_msb[i + 4];
+        if (syncoffset > 16) {
+          syncoffset = 16;
+        }
+        // STDR response message generation
+        MsgIn2(0x01);
+        MsgIn2(0x03);
+        MsgIn2(0x01);
+        MsgIn2(syncperiod);
+        MsgIn2(syncoffset);
+        break;
+      }
+    }
+  }
+  m_phase = PHASE_COMMAND;
+}
+
+void CommandPhaseHandler() {
+  LOG("Command:");
+  SET_MSG_INACTIVE();
+  SET_CD_ACTIVE();
+  SET_IO_INACTIVE();
+  
+  m_cmd[0] = readHandshake();
+  if(m_isBusReset) {
+    m_phase = PHASE_BUSFREE;
+    return;
+  }
+  LOGHEX2(m_cmd[0]);
+
+  // Command length selection, reception
+  static const int cmd_class_len[8]={6,10,10,6,6,12,6,6};
+  m_cmdlen = cmd_class_len[m_cmd[0] >> 5];
+
+  // Receive the remaining command bytes
+  for(int i = 1; i < m_cmdlen; i++ ) {
+    m_cmd[i] = readHandshake();
+    LOG(":");
+    LOGHEX2(m_cmd[i]);
+    if(m_isBusReset) {
+      m_phase = PHASE_BUSFREE;
+      return;
+    }
+  }
+
+  // LUN confirmation
+  m_sts = m_cmd[1]&0xe0;      // Preset LUN in status byte if not specified by Identify Message
+  if(m_lun == 0xff) m_lun = (m_sts>>5);
+
+  // HDD Image selection
+  m_sel = (VirtualDevice_t *)0; // None
+  if( (m_lun < NUM_SCSILUN) )
+  {
+    uint8_t v = m_vdevmap[m_id][m_lun];
+    if(v < NUM_VDEV) {
+      m_sel = &(m_vdev[v]); // There is an image
+      if(!m_sel->m_enabled)
+        m_sel = (VirtualDevice_t *)0;       // Image absent
+    }
+  }
+  
+  LOG(":ID ");
+  LOG(m_id);
+  LOG(":LUN ");
+  LOG(m_lun);
+
+  LOG(" ");
+
+  if(!m_sel) {
+    m_badlunhandler[m_cmd[0]]();
+  } else {
+    m_sel->m_handler[m_cmd[0]]();
+  }
+}
+
+void ConfigureBadLunHandlers() {
+  for(int c = 0; c < 256; c++)
+    m_badlunhandler[c] = &BadLunCommandHandler;
+
+  m_badlunhandler[0x03] = &RequestSenseCommandHandler;
+  m_badlunhandler[0x12] = &InquiryCommandHandler;
+}
+
+void StatusInPhaseHandler() {
+  //LOGN("Sts");
+  SET_MSG_INACTIVE();
+  SET_CD_ACTIVE();
+  SET_IO_ACTIVE();
+  writeHandshake(m_sts);
+  m_phase = PHASE_MESSAGEIN;
+}
+
+void MessageInPhaseHandler() {
+  //LOGN("MsgIn");
+  SET_MSG_ACTIVE();
+  SET_CD_ACTIVE();
+  SET_IO_ACTIVE();
+  writeHandshake(m_msg);
+  m_phase = PHASE_BUSFREE;
+}
+
+void BusFreePhaseHandler() {
+  //LOGN("BusFree");
+  // Release control of the bus.
+  SCSI_TARGET_INACTIVE();
+  // Clear the reset pending flag.
+  m_isBusReset = false;
+  // Reset back to waiting for selection phase.
+  m_phase = PHASE_SELECTION;
+}
diff --git a/src/optical.ino b/src/optical.ino
index b0f64e9..ec21769 100755
--- a/src/optical.ino
+++ b/src/optical.ino
@@ -1,5 +1,8 @@
+#include "config.h"
 #include "scsi_defs.h"
 
+#if SUPPORT_OPTICAL
+
 static const uint8_t SessionTOC[] =
 {
   0x00, // toc length, MSB
@@ -142,140 +145,13 @@ static void LBA2MSF(uint32_t LBA, uint8_t* MSF)
   MSF[1] = (uint32_t)(rem / 60); // M
 }
 
-
-void OpticalModeSense6CommandHandler() {
-  uint8_t len;
-  int page, pagemax, pagemin;
-
-  LOGN("[ModeSense6]");
-  /* Check whether medium is present */
-  if(!m_img) {
-    m_sts |= 0x02;
-    m_phase = PHASE_STATUSIN;
-    return;
-  }
-
-  memset(m_responsebuffer, 0, sizeof(m_responsebuffer));
-
-  len = 1;
-  /* Default medium type */
-  m_responsebuffer[len++] = 0xf0;
-  /* Write protected */
-  m_responsebuffer[len++] = 0x80;
-
-  /* Add block descriptor if DBD is not set */
-  if (m_cmd[1] & 0x08) {
-    m_responsebuffer[len++] = 0;             /* No block descriptor */
-  } else {
-    uint32_t capacity = (m_img->m_fileSize / m_img->m_blocksize) - 1;
-    m_responsebuffer[len++] = 8;             /* Block descriptor length */
-    m_responsebuffer[len++] = (capacity >> 24) & 0xff;
-    m_responsebuffer[len++] = (capacity >> 16) & 0xff;
-    m_responsebuffer[len++] = (capacity >> 8) & 0xff;
-    m_responsebuffer[len++] = capacity & 0xff;
-    m_responsebuffer[len++] = (m_img->m_blocksize >> 24) & 0xff;
-    m_responsebuffer[len++] = (m_img->m_blocksize >> 16) & 0xff;
-    m_responsebuffer[len++] = (m_img->m_blocksize >> 8) & 0xff;
-    m_responsebuffer[len++] = (m_img->m_blocksize) & 0xff;
-  }
-
-  /* Check for requested mode page */
-  page = m_cmd[2] & 0x3F;
-  pagemax = (page != 0x3f) ? page : 0x3e;
-  pagemin = (page != 0x3f) ? page : 0x00;
-  for(page = pagemax; page >= pagemin; page--) {
-    switch (page) {
-      case MODEPAGE_VENDOR_SPECIFIC:
-        /* Accept request only for current values */
-        if (m_cmd[2] & 0xC0) {
-          //DEBUGPRINT(DBG_TRACE, " [2]==%d", m_cmd[2]);
-          /* Prepare sense data */
-          m_sts |= STATUS_CHECK;
-          m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST;
-          m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB;    /* "Invalid field in CDB" */
-          m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
-          m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-          m_sense[m_id][m_lun].m_key_specific[2] = 0x02;
-          m_phase = PHASE_STATUSIN;
-          return;
-        }
-
-        /* Unit attention */
-        m_responsebuffer[len++] = 0x80; // PS, page id
-        m_responsebuffer[len++] = 0x02; // Page length
-        m_responsebuffer[len++] = 0x00;
-        m_responsebuffer[len++] = 0x00;
-        break;
-#if 0
-      case MODEPAGE_DCRC_PARAMETERS:
-        m_responsebuffer[len++] = 0x82; // PS, page id
-        m_responsebuffer[len++] = 0x0e; // Page length
-        m_responsebuffer[len++] = 0xe6; // Buffer full ratio, 90%
-        m_responsebuffer[len++] = 0x1a; // Buffer empty ratio, 10%
-        m_responsebuffer[len++] = 0x00; // Bus inactivity limit
-        m_responsebuffer[len++] = 0x00;
-        m_responsebuffer[len++] = 0x00; // Disconnect time limit
-        m_responsebuffer[len++] = 0x00;
-        m_responsebuffer[len++] = 0x00; // Connect time limit
-        m_responsebuffer[len++] = 0x00;
-        m_responsebuffer[len++] = 0x00; // Maximum burst size
-        m_responsebuffer[len++] = 0x00;
-        m_responsebuffer[len++] = 0x00; // EMDP, Dimm, DTDC
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Reserved
-        break;
-#endif
-      case MODEPAGE_RW_ERROR_RECOVERY:
-        m_responsebuffer[len++] = 0x81; // PS, page id
-        m_responsebuffer[len++] = 0x0a; // Page length
-        m_responsebuffer[len++] = 0x07; // 
-        m_responsebuffer[len++] = 0x00; // Read Retry Count
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Write Retry Count
-        m_responsebuffer[len++] = 0x00; // Reserved
-        m_responsebuffer[len++] = 0x00; // Recovery Time Limit
-        m_responsebuffer[len++] = 0x00;
-        break;
-      default:
-        if(pagemin == pagemax) {
-          /* Requested mode page is not supported */
-          /* Prepare sense data */
-          m_sts |= STATUS_CHECK;
-          m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST;
-          m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB;       /* "Invalid field in CDB" */
-          m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
-          m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-          m_sense[m_id][m_lun].m_key_specific[2] = 0x02;
-          m_phase = PHASE_STATUSIN;
-          return;
-        }
-    }
-  }
-
-  /* Report size of requested data */
-  m_responsebuffer[0] = len;
-
-  /* Truncate data if necessary */
-  if (m_cmd[4] < len) {
-    len = m_cmd[4];
-  }
-
-  // Send it
-  writeDataPhase(len, m_responsebuffer);
-  m_phase = PHASE_STATUSIN;
-}
-
 void OpticalReadSimpleTOC()
 {
   int MSF = m_cmd[1] & 0x02 ? 1 : 0;
   uint16_t allocationLength = (m_cmd[7] << 8) | m_cmd[8];
   uint8_t len = 0;
 
-  if(!m_img) {
+  if(!m_sel) {
     m_sts |= 0x02;
     m_phase = PHASE_STATUSIN;
     return;
@@ -283,7 +159,7 @@ void OpticalReadSimpleTOC()
   
   memset(m_responsebuffer, 0, sizeof(m_responsebuffer));
 
-  uint32_t capacity = (m_img->m_fileSize / m_img->m_blocksize) - 1;
+  uint32_t capacity = (m_sel->m_fileSize / m_sel->m_blocksize) - 1;
   m_responsebuffer[len++] = 0x00; // toc length, MSB
   m_responsebuffer[len++] = 0x00; // toc length, LSB
   m_responsebuffer[len++] = 0x01; // First track number
@@ -324,11 +200,11 @@ void OpticalReadSimpleTOC()
       break;
     default:
       m_sts |= STATUS_CHECK;
-      m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST; // Illegal Request
-      m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
-      m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 6
-      m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-      m_sense[m_id][m_lun].m_key_specific[2] = 0x06;
+      m_sel->m_sense.m_key = ILLEGAL_REQUEST; // Illegal Request
+      m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
+      m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 6
+      m_sel->m_sense.m_key_specific[1] = 0x00;
+      m_sel->m_sense.m_key_specific[2] = 0x06;
       m_phase = PHASE_STATUSIN;
   }
 
@@ -371,11 +247,11 @@ void OpticalReadFullTOC(int convertBCD)
   // We only support session 1.
   if (m_cmd[6] > 1) {
     m_sts |= STATUS_CHECK;
-    m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST; // Illegal Request
-    m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
-    m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 6
-    m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-    m_sense[m_id][m_lun].m_key_specific[2] = 0x06;
+    m_sel->m_sense.m_key = ILLEGAL_REQUEST; // Illegal Request
+    m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
+    m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 6
+    m_sel->m_sense.m_key_specific[1] = 0x00;
+    m_sel->m_sense.m_key_specific[2] = 0x06;
     m_phase = PHASE_STATUSIN;
     return;
   }
@@ -431,11 +307,11 @@ void OpticalReadTOCCommandHandler()
   }
 
   m_sts |= STATUS_CHECK;
-  m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST; // Illegal Request
-  m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
-  m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 2
-  m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-  m_sense[m_id][m_lun].m_key_specific[2] = 0x02;
+  m_sel->m_sense.m_key = ILLEGAL_REQUEST; // Illegal Request
+  m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
+  m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 2
+  m_sel->m_sense.m_key_specific[1] = 0x00;
+  m_sel->m_sense.m_key_specific[2] = 0x02;
   m_phase = PHASE_STATUSIN;
 }
 
@@ -473,7 +349,7 @@ void OpticalReadDiscInfoCommandHandler()
 {
   uint16_t allocationLength = (m_cmd[7] << 8) | m_cmd[8];
   uint32_t len = sizeof(DiscInfoBlock);
-  uint32_t capacity = (m_img->m_fileSize / m_img->m_blocksize) - 1;
+  uint32_t capacity = (m_sel->m_fileSize / m_sel->m_blocksize) - 1;
   memcpy(m_responsebuffer, DiscInfoBlock, len);
 
   LOGN("[DiscInfo]");
@@ -505,6 +381,8 @@ void OpticalGetConfigurationCommandHandler()
   uint16_t sfi = 0;
   uint8_t len;
 
+  LOGN("[GetConfiguration]");
+
   memset(m_responsebuffer, 0, sizeof(m_responsebuffer));
 
   switch(m_cmd[1] & 0x3) {
@@ -558,11 +436,11 @@ void OpticalEventStatusCommandHandler()
 
   if((m_cmd[1] & 1) == 0) {
     m_sts |= STATUS_CHECK;
-    m_sense[m_id][m_lun].m_key = ILLEGAL_REQUEST; // Illegal Request
-    m_sense[m_id][m_lun].m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
-    m_sense[m_id][m_lun].m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 1
-    m_sense[m_id][m_lun].m_key_specific[1] = 0x00;
-    m_sense[m_id][m_lun].m_key_specific[2] = 0x01;
+    m_sel->m_sense.m_key = ILLEGAL_REQUEST; // Illegal Request
+    m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB; // Invalid field in CDB
+    m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE; // Error in Byte 1
+    m_sel->m_sense.m_key_specific[1] = 0x00;
+    m_sel->m_sense.m_key_specific[2] = 0x01;
     m_phase = PHASE_STATUSIN;
     return;
   }
@@ -587,7 +465,7 @@ void OpticalEventStatusCommandHandler()
           m_responsebuffer[len++] = 0x04; // Media Change Event
           m_responsebuffer[len++] = 0x12; // Supported Event Class
           m_responsebuffer[len++] = 0x00; // No Change
-          m_responsebuffer[len++] = (!m_img) ? 0x00 : 0x02; // Media Present
+          m_responsebuffer[len++] = (!m_sel) ? 0x00 : 0x02; // Media Present
           m_responsebuffer[len++] = 0x00; // Start Slot
           m_responsebuffer[len++] = 0x00; // End Slot
           break;
@@ -617,14 +495,14 @@ void OpticalLockTrayCommandHandler() {
 
 void OpticalReadCapacityCommandHandler() {
   LOGN("[ReadCapacity]");
-  if(!m_img) {
+  if(!m_sel) {
     m_sts |= 0x02; // Image file absent
     m_phase = PHASE_STATUSIN;
     return;
   }
 
-  uint32_t bl = m_img->m_blocksize;
-  uint32_t bc = m_img->m_fileSize / bl;
+  uint32_t bl = m_sel->m_blocksize;
+  uint32_t bc = m_sel->m_fileSize / bl;
   uint8_t buf[8] = {
     (uint8_t)(((uint32_t)(bc >> 24))&0xff), (uint8_t)(((uint32_t)(bc >> 16))&0xff), (uint8_t)(((uint32_t)(bc >> 8))&0xff), (uint8_t)(((uint32_t)(bc))&0xff),
     (uint8_t)(((uint32_t)(bl >> 24))&0xff), (uint8_t)(((uint32_t)(bl >> 16))&0xff), (uint8_t)(((uint32_t)(bl >> 8))&0xff), (uint8_t)(((uint32_t)(bl))&0xff)
@@ -633,28 +511,75 @@ void OpticalReadCapacityCommandHandler() {
   m_phase = PHASE_STATUSIN;
 }
 
-void ConfigureOpticalHandlers(int id) {
+void ConfigureOpticalHandlers(VirtualDevice_t *vdev) {
   for(int c = 0; c < 256; c++)
-    m_handler[id][c] = &UnknownCommandHandler;
+    vdev->m_handler[c] = &UnknownCommandHandler;
 
-  m_handler[id][CMD_TEST_UNIT_READY]               = &TestUnitCommandHandler;
-  m_handler[id][CMD_REZERO_UNIT]                   = &RezeroUnitCommandHandler;
-  m_handler[id][CMD_REQUEST_SENSE]                 = &RequestSenseCommandHandler;
-  m_handler[id][CMD_READ6]                         = &Read6CommandHandler;
-  m_handler[id][CMD_SEEK6]                         = &Seek6CommandHandler;
-  m_handler[id][CMD_INQUIRY]                       = &InquiryCommandHandler;
-  m_handler[id][CMD_MODE_SELECT6]                  = &ModeSelect6CommandHandler;
-  m_handler[id][CMD_MODE_SENSE6]                   = &OpticalModeSense6CommandHandler;
-  m_handler[id][CMD_START_STOP_UNIT]               = &StartStopUnitCommandHandler;
-  m_handler[id][CMD_PREVENT_REMOVAL]               = &OpticalLockTrayCommandHandler;
-  m_handler[id][CMD_READ_CAPACITY10]               = &OpticalReadCapacityCommandHandler;
-  m_handler[id][CMD_READ10]                        = &Read10CommandHandler;
-  m_handler[id][CMD_SEEK10]                        = &Seek10CommandHandler;
-  m_handler[id][CMD_READ_TOC]                      = &OpticalReadTOCCommandHandler;
-  m_handler[id][CMD_READ_HEADER]                   = &OpticalHeaderCommandHandler;
-  m_handler[id][CMD_GET_CONFIGURATION]             = &OpticalGetConfigurationCommandHandler;
-  m_handler[id][CMD_GET_EVENT_STATUS_NOTIFICATION] = &OpticalEventStatusCommandHandler;
-  m_handler[id][CMD_READ_DISC_INFORMATION]         = &OpticalReadDiscInfoCommandHandler;
-  m_handler[id][CMD_MODE_SELECT10]                 = &ModeSelect10CommandHandler;
-  m_handler[id][CMD_MODE_SENSE10]                  = &ModeSense10CommandHandler;
+  vdev->m_handler[CMD_TEST_UNIT_READY]               = &TestUnitCommandHandler;
+  vdev->m_handler[CMD_REZERO_UNIT]                   = &RezeroUnitCommandHandler;
+  vdev->m_handler[CMD_REQUEST_SENSE]                 = &RequestSenseCommandHandler;
+  vdev->m_handler[CMD_READ6]                         = &Read6CommandHandler;
+  vdev->m_handler[CMD_SEEK6]                         = &Seek6CommandHandler;
+  vdev->m_handler[CMD_INQUIRY]                       = &InquiryCommandHandler;
+  vdev->m_handler[CMD_MODE_SELECT6]                  = &ModeSelect6CommandHandler;
+  vdev->m_handler[CMD_MODE_SENSE6]                   = &ModeSenseCommandHandler;
+  vdev->m_handler[CMD_START_STOP_UNIT]               = &StartStopUnitCommandHandler;
+  vdev->m_handler[CMD_PREVENT_REMOVAL]               = &OpticalLockTrayCommandHandler;
+  vdev->m_handler[CMD_READ_CAPACITY10]               = &OpticalReadCapacityCommandHandler;
+  vdev->m_handler[CMD_READ10]                        = &Read10CommandHandler;
+  vdev->m_handler[CMD_SEEK10]                        = &Seek10CommandHandler;
+  vdev->m_handler[CMD_READ_TOC]                      = &OpticalReadTOCCommandHandler;
+  vdev->m_handler[CMD_READ_HEADER]                   = &OpticalHeaderCommandHandler;
+  vdev->m_handler[CMD_GET_CONFIGURATION]             = &OpticalGetConfigurationCommandHandler;
+  vdev->m_handler[CMD_GET_EVENT_STATUS_NOTIFICATION] = &OpticalEventStatusCommandHandler;
+  vdev->m_handler[CMD_READ_DISC_INFORMATION]         = &OpticalReadDiscInfoCommandHandler;
+  vdev->m_handler[CMD_MODE_SELECT10]                 = &ModeSelect10CommandHandler;
+  vdev->m_handler[CMD_MODE_SENSE10]                  = &ModeSenseCommandHandler;
 }
+
+// If config file exists, read the first three lines and copy the contents.
+// File must be well formed or you will get junk in the SCSI Vendor fields.
+void ConfigureOptical(VirtualDevice_t *vdev, const char *image_name) {
+  memcpy(vdev->m_inquiryresponse, SCSI_CDROM_INQUIRY_RESPONSE, sizeof(SCSI_CDROM_INQUIRY_RESPONSE));
+
+  if(image_name) {
+    char configname[MAX_FILE_PATH+1];
+    memcpy(configname, image_name, MAX_FILE_PATH+1);
+    char *psuffix = strstr(configname, ".img");
+    if(psuffix) {
+      strcpy(psuffix, ".cfg");
+    } else {
+      sprintf(configname, "cd%d%d.cfg", vdev->m_id, vdev->m_lun);
+    }
+  
+    FsFile config_file = sd.open(configname, O_RDONLY);
+    if (config_file.isOpen()) {
+      char vendor[9];
+      memset(vendor, 0, sizeof(vendor));
+      config_file.readBytes(vendor, sizeof(vendor));
+      LOGN("SCSI VENDOR: ");
+      LOGN(vendor);
+      memcpy(&(vdev->m_inquiryresponse[8]), vendor, 8);
+    
+      char product[17];
+      memset(product, 0, sizeof(product));
+      config_file.readBytes(product, sizeof(product));
+      LOGN("SCSI PRODUCT: ");
+      LOGN(product);
+      memcpy(&(vdev->m_inquiryresponse[16]), product, 16);
+    
+      char version[5];
+      memset(version, 0, sizeof(version));
+      config_file.readBytes(version, sizeof(version));
+      LOGN("SCSI VERSION: ");
+      LOGN(version);
+      memcpy(&(vdev->m_inquiryresponse[32]), version, 4);
+      config_file.close();
+    }
+  }
+
+  vdev->m_type = DEV_OPTICAL;
+  ConfigureOpticalHandlers(vdev);
+}
+
+#endif
diff --git a/src/partitions.ino b/src/partitions.ino
new file mode 100755
index 0000000..45015ef
--- /dev/null
+++ b/src/partitions.ino
@@ -0,0 +1,25 @@
+#if 0
+#include "config.h"
+#include "cmd.h"
+
+typedef struct PartType_s {
+  unsigned char Type;
+  const char *Name;
+} PartType_t;
+PartType_t PartType[] = {
+  { 0x00, "Empty" },
+  { 0x01, "FAT12" },
+  { 0x04, "FAT16 <32MB" },
+  { 0x05, "Extended" },
+  { 0x06, "FAT16 >32MB" },
+  { 0x07, "NTFS" },
+  { 0x0b, "FAT32" },
+  { 0x0c, "FAT32 LBA" },
+  { 0x0e, "FAT16 LBA" },
+  { 0x0f, "Extended LBA" },
+  { 0xf8, "TinySCSI Disk LUN" },
+  { 0xf9, "TinySCSI Optical LUN" },
+  { 0xff, NULL }
+};
+
+#endif
diff --git a/src/sasi.ino b/src/sasi.ino
new file mode 100755
index 0000000..bdb9746
--- /dev/null
+++ b/src/sasi.ino
@@ -0,0 +1,60 @@
+#include "config.h"
+#include "scsi_defs.h"
+
+#if SUPPORT_SASI
+
+/*
+ * dtc510b_setDriveparameter
+ */
+#define PACKED  __attribute__((packed))
+typedef struct PACKED dtc500_cmd_c2_param_struct
+{
+  uint8_t StepPlusWidth;        // Default is 13.6usec (11)
+  uint8_t StepPeriod;         // Default is  3  msec.(60)
+  uint8_t StepMode;         // Default is  Bufferd (0)
+  uint8_t MaximumHeadAdress;      // Default is 4 heads (3)
+  uint8_t HighCylinderAddressuint8_t;  // Default set to 0   (0)
+  uint8_t LowCylinderAddressuint8_t;   // Default is 153 cylinders (152)
+  uint8_t ReduceWrietCurrent;     // Default is above Cylinder 128 (127)
+  uint8_t DriveType_SeekCompleteOption;// (0)
+  uint8_t Reserved8;          // (0)
+  uint8_t Reserved9;          // (0)
+} DTC510_CMD_C2_PARAM;
+
+static void logStrHex(const char *msg,uint32_t num)
+{
+    LOG(msg);
+    LOGHEX6N(num);
+}
+
+void DTCsetDriveParameterCommandHandler() {
+  LOGN("[DTC510B setDriveParameter]");
+
+  DTC510_CMD_C2_PARAM DriveParameter;
+  uint16_t maxCylinder;
+  uint16_t numLAD;
+  //uint32_t stepPulseUsec;
+  int StepPeriodMsec;
+
+  // receive paramter
+  writeDataPhase(sizeof(DriveParameter),(uint8_t *)(&DriveParameter));
+ 
+  maxCylinder =
+    (((uint16_t)DriveParameter.HighCylinderAddressuint8_t)<<8) |
+    (DriveParameter.LowCylinderAddressuint8_t);
+  numLAD = maxCylinder * (DriveParameter.MaximumHeadAdress+1);
+  //stepPulseUsec  = calcStepPulseUsec(DriveParameter.StepPlusWidth);
+  StepPeriodMsec = DriveParameter.StepPeriod*50;
+  logStrHex (" StepPlusWidth      : ",DriveParameter.StepPlusWidth);
+  logStrHex (" StepPeriod         : ",DriveParameter.StepPeriod   );
+  logStrHex (" StepMode           : ",DriveParameter.StepMode     );
+  logStrHex (" MaximumHeadAdress  : ",DriveParameter.MaximumHeadAdress);
+  logStrHex (" CylinderAddress    : ",maxCylinder);
+  logStrHex (" ReduceWrietCurrent : ",DriveParameter.ReduceWrietCurrent);
+  logStrHex (" DriveType/SeekCompleteOption : ",DriveParameter.DriveType_SeekCompleteOption);
+  logStrHex (" Maximum LAD        : ",numLAD-1);
+  m_sts = 0;
+  m_phase = PHASE_STATUSIN;
+}
+
+#endif
diff --git a/src/scsi_defs.h b/src/scsi_defs.h
index 7eb0aba..74b068c 100755
--- a/src/scsi_defs.h
+++ b/src/scsi_defs.h
@@ -1,3 +1,5 @@
+#ifndef __SCSI_DEFS_H
+#define __SCSI_DEFS_H
 
 /* Mode pages */
 #define MODEPAGE_VENDOR_SPECIFIC    0x00
@@ -14,6 +16,8 @@
 #define CMD_REQUEST_SENSE         0x03
 #define CMD_FORMAT_UNIT           0x04
 #define CMD_READ_BLOCK_LIMITS     0x05
+#define CMD_FORMAT_UNIT_ALT       0x06
+#define CMD_REASSIGN_BLOCKS       0x07
 #define CMD_READ6                 0x08
 #define CMD_WRITE6                0x0A  /* Optional */
 #define CMD_SEEK6                 0x0B  /* Optional */
@@ -39,6 +43,7 @@
 #define CMD_WRITEANDVERIFY10      0x2E  /* Optional */
 #define CMD_VERIFY10              0x2F
 #define CMD_PREFETCH_CACHE10      0x34  /* Optional */
+#define CMD_READPOSITION10        0x34  /* Optional */
 #define CMD_SYNCHRONIZE_CACHE10   0x35  /* Optional */
 #define CMD_LOCKUNLOCK_CACHE10    0x36  /* Optional */
 #define CMD_READ_DEFECT_DATA      0x37  /* Optional */
@@ -57,6 +62,7 @@
 #define CMD_MODE_SENSE10          0x5A  /* Optional */
 /* 12 Byte command opcodes */
 #define CMD_REPORT_LUNS           0xA0  /* Optional */
+#define CMD_SET_DRIVE_PARAMETER   0xC2
 #define CMD_MAC_UNKNOWN           0xEE  /* Unknown */
 
 /* Dayna SCSI/Link Ethernet */
@@ -81,11 +87,16 @@
 #define CMD_CABLETRON_LOAD_IMAGE   0x0d09
 #define CMD_CABLETRON_SET_ADDR     0x0d0a
 
+/*
+ *  SCSI DEVICE CODES
+ */
+#define DEV_DISK            0x00
+#define DEV_TAPE            0x01
+#define DEV_OPTICAL         0x05
 
 /*
  *  SCSI MESSAGE CODES
  */
-
 #define COMMAND_COMPLETE    0x00
 #define EXTENDED_MESSAGE    0x01
 #define     EXTENDED_MODIFY_DATA_POINTER    0x00
@@ -149,3 +160,9 @@
 #define UNIT_POWERON_RESET   0x29
 #define NO_MEDIA             0x3A
 #define ERROR_IN_OPCODE      0xC0
+
+#if USE_DB2ID_TABLE
+extern const uint8_t db2scsiid[256];
+#endif
+
+#endif
diff --git a/src/scsibus.ino b/src/scsibus.ino
new file mode 100755
index 0000000..db75797
--- /dev/null
+++ b/src/scsibus.ino
@@ -0,0 +1,293 @@
+#include "config.h"
+#include "scsi_defs.h"
+
+#define SCSI_DB_MASK 0x00ff0800
+
+/*
+ *  Data uint8_t to GPIOB register setting value and parity table
+*/
+
+// Parity bit generation
+#define PTY(V)   ((1^((V)^((V)>>1)^((V)>>2)^((V)>>3)^((V)>>4)^((V)>>5)^((V)>>6)^((V)>>7)))&1)
+
+// Set DBP
+#define DBP(D)    ((((uint32_t)(D)<<16)|(PTY(D)<<11)) ^ SCSI_DB_MASK)
+
+#define DBP8(D)   DBP(D),DBP(D+1),DBP(D+2),DBP(D+3),DBP(D+4),DBP(D+5),DBP(D+6),DBP(D+7)
+#define DBP32(D)  DBP8(D),DBP8(D+8),DBP8(D+16),DBP8(D+24)
+
+// BSRR register control value that simultaneously performs DB set, DP set, and REQ = H (inactrive)
+const uint32_t db_bsrr[256]={
+  DBP32(0x00),DBP32(0x20),DBP32(0x40),DBP32(0x60),
+  DBP32(0x80),DBP32(0xA0),DBP32(0xC0),DBP32(0xE0)
+};
+// Parity bit acquisition
+#define PARITY(DB) ((db_bsrr[DB & 0xff] & 0x0800) >> 11)
+
+// Macro cleaning
+#undef DBP32
+#undef DBP8
+//#undef DBP
+//#undef PTY
+
+#if USE_DB2ID_TABLE
+/* DB to SCSI-ID translation table */
+const uint8_t db2scsiid[256]={
+  0xff,
+  0,
+  1,1,
+  2,2,2,2,
+  3,3,3,3,3,3,3,3,
+  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
+  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
+  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
+  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
+  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
+  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
+  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
+  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7
+};
+#endif
+
+// Put DB and DP in output mode
+inline void SCSI_DB_OUTPUT(uint8_t d) {
+//  GPIOB_PDDR = db_bsrr[d];
+//  GPIOB_PDOR = db_bsrr[d] ^ SCSI_DB_MASK;
+
+  GPIOB_PDOR = db_bsrr[d];
+}
+
+// Put DB and DP in input mode
+inline void SCSI_DB_INPUT()  {
+  GPIOB_PDDR = 0x00000000;
+  GPIOB_PDOR = SCSI_DB_MASK;
+}
+
+/*
+ * Bus reset interrupt.
+ */
+void onBusReset(void)
+{
+#if SUPPORT_SASI
+  // SASI I / F for X1 turbo has RST pulse write cycle +2 clock ==
+  // I can't filter because it only activates about 1.25us
+  if(!m_sasi_mode) {
+#endif
+    if(digitalRead(RST)) return;
+    delayMicroseconds(20);
+    if(digitalRead(RST)) return;
+#if SUPPORT_SASI
+  }
+#endif
+
+  SCSI_DB_INPUT();
+
+  LOGN("BusReset!");
+  m_isBusReset = true;
+}
+
+/*
+ * Read by handshake.
+ */
+inline uint8_t readHandshake(void)
+{
+  SET_REQ_ACTIVE();
+  //SCSI_DB_INPUT();
+  while(!m_isBusReset && !GET_ACK());
+  uint8_t r = readIO();
+  SET_REQ_INACTIVE();
+  while(GET_ACK()) { if(m_isBusReset) return 0; }
+  return r;  
+}
+
+inline void readHandshakeBlock(uint8_t *d, uint16_t len)
+{
+  //SCSI_DB_INPUT();
+  while(len) {
+    SET_REQ_ACTIVE();
+    while(!m_isBusReset && !GET_ACK());
+    *d++ = readIO();
+    len--;
+    SET_REQ_INACTIVE();
+    while(GET_ACK()) { 
+      yield();
+      if(m_isBusReset) return;
+    }
+  }
+}
+
+/*
+ * Write with a handshake.
+ */
+inline void writeHandshake(uint8_t d)
+{
+  GPIOB_PDDR = SCSI_DB_MASK;
+  SCSI_DB_OUTPUT(d);
+
+  // ACK.Fall to DB output delay 100ns(MAX)  (DTC-510B)
+  SET_REQ_ACTIVE();   // (30ns)
+  while(!m_isBusReset && !GET_ACK());
+  // ACK.Fall to REQ.Raise delay 500ns(typ.) (DTC-510B)
+  GPIOB_PDDR = DBP(0xff);
+  GPIOB_PDOR = DBP(0xff) ^ SCSI_DB_MASK;  // DB=0xFF
+  SET_REQ_INACTIVE();
+
+  // REQ.Raise to DB hold time 0ns
+  SCSI_DB_INPUT(); // (150ns)
+  while( GET_ACK()) { if(m_isBusReset) return; }
+}
+
+inline void writeHandshakeBlock(const uint8_t *d, uint16_t len)
+{
+  GPIOB_PDDR = SCSI_DB_MASK;
+  while(len) {
+    SCSI_DB_OUTPUT(*d++);
+    SET_REQ_ACTIVE();   // (30ns)
+    while(!GET_ACK());
+    len--;
+    SET_REQ_INACTIVE();
+    while( GET_ACK()) {
+      if(m_isBusReset) return;
+    }
+  }
+  SCSI_DB_INPUT(); // (150ns)
+}
+
+/*
+ * Data in phase.
+ *  Send len uint8_ts of data array p.
+ */
+void writeDataPhase(int len, const uint8_t* p)
+{
+  //LOGN("DATAIN PHASE");
+  SET_MSG_INACTIVE(); //  gpio_write(MSG, low);
+  SET_CD_INACTIVE();; //  gpio_write(CD, low);
+  SET_IO_ACTIVE();  //  gpio_write(IO, high);
+
+#if READ_SPEED_OPTIMIZE
+  writeHandshakeBlock(p, len);
+#else
+  for (int i = 0; i < len; i++) {
+    if(m_isBusReset) {
+      m_phase = PHASE_BUSFREE;
+      return;
+    }
+    writeHandshake(p[i]);
+  }
+#endif
+  if(m_isBusReset) {
+    m_phase = PHASE_BUSFREE;
+  }
+}
+
+/* 
+ * Data in phase.
+ *  Send len block while reading from SD card.
+ */
+void writeDataPhaseSD(uint32_t adds, uint32_t len)
+{
+#if READ_SPEED_OPTIMIZE
+  uint32_t bigread = (MAX_BLOCKSIZE / m_sel->m_blocksize);
+#endif
+  uint32_t i = 0;
+
+  //LOGN("DATAIN PHASE(SD)");
+  uint32_t pos = adds * m_sel->m_blocksize;
+  m_sel->m_file.seek(pos);
+
+  SET_MSG_INACTIVE(); //  gpio_write(MSG, low);
+  SET_CD_INACTIVE(); //  gpio_write(CD, low);
+  SET_IO_ACTIVE(); //  gpio_write(IO, high);
+
+  while(i < len) {
+      // Asynchronous reads will make it faster ...
+#if READ_SPEED_OPTIMIZE
+    if((len-i) >= bigread) {
+      m_sel->m_file.read(m_buf, MAX_BLOCKSIZE);
+      writeHandshakeBlock(m_buf, MAX_BLOCKSIZE);
+      i += bigread;
+    } else {
+      m_sel->m_file.read(m_buf, m_sel->m_blocksize * (len-i));
+      writeHandshakeBlock(m_buf, m_sel->m_blocksize * (len-i));
+      i = len;
+    }
+#else
+    m_sel->m_file.read(m_buf, m_sel->m_blocksize);
+    for(unsigned int j = 0; j < m_sel->m_blocksize; j++) {
+      if(m_isBusReset) {
+        m_phase = PHASE_BUSFREE;
+        return;
+      }
+      writeHandshake(m_buf[j]);
+    }
+#endif
+    if(m_isBusReset) {
+      m_phase = PHASE_BUSFREE;
+      return;
+    }
+  }
+}
+
+/*
+ * Data out phase.
+ *  len block read
+ */
+void readDataPhase(int len, uint8_t* p)
+{
+  //LOGN("DATAOUT PHASE");
+  SET_MSG_INACTIVE();
+  SET_CD_INACTIVE();;
+  SET_IO_INACTIVE();;
+
+#if WRITE_SPEED_OPTIMIZE
+  readHandshakeBlock(p, len);
+#else
+  for(int i = 0; i < len; i++)
+    p[i] = readHandshake();
+#endif
+  if(m_isBusReset) {
+    m_phase = PHASE_BUSFREE;
+  }
+}
+
+/*
+ * Data out phase.
+ *  Write to SD card while reading len block.
+ */
+void readDataPhaseSD(uint32_t adds, uint32_t len)
+{
+#if WRITE_SPEED_OPTIMIZE
+  uint32_t bigread = (MAX_BLOCKSIZE / m_sel->m_blocksize);
+#endif
+  uint32_t i = 0;
+
+  //LOGN("DATAOUT PHASE(SD)");
+  uint32_t pos = adds * m_sel->m_blocksize;
+  m_sel->m_file.seek(pos);
+  SET_MSG_INACTIVE();
+  SET_CD_INACTIVE();;
+  SET_IO_INACTIVE();;
+
+  while(i < len) {
+#if WRITE_SPEED_OPTIMIZE
+    if((len-i) >= bigread) {
+      readHandshakeBlock(m_buf, MAX_BLOCKSIZE);
+      m_sel->m_file.write(m_buf, MAX_BLOCKSIZE);
+      i += bigread;
+    } else {
+      readHandshakeBlock(m_buf, m_sel->m_blocksize * (len-i));
+      m_sel->m_file.write(m_buf, m_sel->m_blocksize * (len-i));
+      i = len;
+    }
+#else
+    for(unsigned int j = 0; j <  m_sel->m_blocksize; j++) {
+      if(m_isBusReset) {
+        return;
+      }
+      m_buf[j] = readHandshake();
+    }
+    m_sel->m_file.write(m_buf, m_sel->m_blocksize);
+#endif
+  }
+  m_sel->m_file.flush();
+}
diff --git a/src/tape.ino b/src/tape.ino
new file mode 100755
index 0000000..d488763
--- /dev/null
+++ b/src/tape.ino
@@ -0,0 +1,316 @@
+#include "config.h"
+#include "scsi_defs.h"
+
+#if SUPPORT_TAPE
+/*
+ * READ6 / 10 Command processing.
+ */
+void TapeReadCommandHandler()
+{
+  uint32_t len;
+
+  LOG("[Read6]");
+  len = (m_cmd[4] << 16) | (m_cmd[3] << 8) | m_cmd[2];
+  switch(m_cmd[1] & 0x3) {
+    case 0:
+      if(len == 0) return;
+      break;
+    case 1: // Fixed
+      break;
+    case 2: // SILI
+      break;
+    case 3: // Illegal Request
+      m_sts = 0x02;
+      m_sel->m_sense.m_key = ILLEGAL_REQUEST;
+      m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB;       /* "Invalid field in CDB" */
+      m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 1" */
+      m_sel->m_sense.m_key_specific[1] = 0x00;
+      m_sel->m_sense.m_key_specific[2] = 0x01;
+      m_phase = PHASE_STATUSIN;
+      return;
+  }
+  LOG("-R ");
+  LOGHEX4N(len);
+
+  if(!m_sel) return 0x02; // Image file absent
+  
+  LED_ON();
+  writeDataPhaseTape(len);
+  LED_OFF();
+
+  m_phase = PHASE_STATUSIN;
+}
+
+/*
+ * WRITE6 / 10 Command processing.
+ */
+void TapeWriteCommandHandler()
+{
+  uint32_t len;
+
+  LOG("[Write6]-W ");
+  LOGHEX4N(len);
+  
+  if(!m_sel) return 0x02; // Image file absent
+  
+  LED_ON();
+  m_sel->m_file.write(&len, 4);
+  readDataPhaseTape(len);
+  m_sel->m_file.write(&len, 4);
+  LED_OFF();
+
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapeModeSense6CommandHandler() {
+  uint8_t len;
+  int page, pagemax, pagemin;
+
+  LOGN("[ModeSense6]");
+  /* Check whether medium is present */
+  if(!m_sel) {
+    m_sts |= 0x02;
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+
+  memset(m_responsebuffer, 0, sizeof(m_responsebuffer));
+
+  len = 1;
+  /* Default medium type */
+  m_responsebuffer[len++] = 0xf0;
+  /* Write protected */
+  m_responsebuffer[len++] = 0x80;
+
+  /* Add block descriptor if DBD is not set */
+  if (m_cmd[1] & 0x08) {
+    m_responsebuffer[len++] = 0;             /* No block descriptor */
+  } else {
+    uint32_t capacity = (m_sel->m_fileSize / m_sel->m_blocksize) - 1;
+    m_responsebuffer[len++] = 8;             /* Block descriptor length */
+    m_responsebuffer[len++] = (capacity >> 24) & 0xff;
+    m_responsebuffer[len++] = (capacity >> 16) & 0xff;
+    m_responsebuffer[len++] = (capacity >> 8) & 0xff;
+    m_responsebuffer[len++] = capacity & 0xff;
+    m_responsebuffer[len++] = (m_sel->m_blocksize >> 24) & 0xff;
+    m_responsebuffer[len++] = (m_sel->m_blocksize >> 16) & 0xff;
+    m_responsebuffer[len++] = (m_sel->m_blocksize >> 8) & 0xff;
+    m_responsebuffer[len++] = (m_sel->m_blocksize) & 0xff;
+  }
+
+  /* Check for requested mode page */
+  page = m_cmd[2] & 0x3F;
+  pagemax = (page != 0x3f) ? page : 0x3e;
+  pagemin = (page != 0x3f) ? page : 0x00;
+  for(page = pagemax; page >= pagemin; page--) {
+    switch (page) {
+      default:
+        if(pagemin == pagemax) {
+          /* Requested mode page is not supported */
+          /* Prepare sense data */
+          m_sts |= STATUS_CHECK;
+          m_sel->m_sense.m_key = ILLEGAL_REQUEST;
+          m_sel->m_sense.m_code = INVALID_FIELD_IN_CDB;       /* "Invalid field in CDB" */
+          m_sel->m_sense.m_key_specific[0] = ERROR_IN_OPCODE;  /* "Error in Byte 2" */
+          m_sel->m_sense.m_key_specific[1] = 0x00;
+          m_sel->m_sense.m_key_specific[2] = 0x02;
+          m_phase = PHASE_STATUSIN;
+          return;
+        }
+    }
+  }
+
+  /* Report size of requested data */
+  m_responsebuffer[0] = len;
+
+  /* Truncate data if necessary */
+  if (m_cmd[4] < len) {
+    len = m_cmd[4];
+  }
+
+  // Send it
+  writeDataPhase(len, m_responsebuffer);
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapeLoadUnloadCommandHandler() {
+  if(m_cmd[4] & 1) {
+    LOGN("[Load]");
+  } else {
+    LOGN("[Unload]");
+  }
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapePreventRemovalCommandHandler() {
+  if(m_cmd[4] & 1) {
+    LOGN("[Prevent Removal]");
+  } else {
+    LOGN("[Allow Removal]");
+  }
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapeReadCapacityCommandHandler() {
+  LOGN("[ReadCapacity]");
+  if(!m_sel) {
+    m_sts |= 0x02; // Image file absent
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+
+  uint32_t bl = m_sel->m_blocksize;
+  uint32_t bc = m_sel->m_fileSize / bl;
+  uint8_t buf[8] = {
+    (uint8_t)(((uint32_t)(bc >> 24))&0xff), (uint8_t)(((uint32_t)(bc >> 16))&0xff), (uint8_t)(((uint32_t)(bc >> 8))&0xff), (uint8_t)(((uint32_t)(bc))&0xff),
+    (uint8_t)(((uint32_t)(bl >> 24))&0xff), (uint8_t)(((uint32_t)(bl >> 16))&0xff), (uint8_t)(((uint32_t)(bl >> 8))&0xff), (uint8_t)(((uint32_t)(bl))&0xff)
+  };
+  writeDataPhase(8, buf);
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapeEraseCommandHandler() {
+  LOGN("[Erase]");
+  if(!m_sel) {
+    m_sts |= 0x02; // Image file absent
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+  // Truncate the current .tap file (maybe archive it by renaming instead of truncating?)
+}
+
+void TapeReadBlockLimitsCommandHandler() {
+  uint16_t len = 0;
+  LOGN("[ReadBlockLimits]");
+  if(!m_sel) {
+    m_sts |= 0x02; // Image file absent
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+  m_responsebuffer[len++] = 0x00;
+  m_responsebuffer[len++] = 0xFF; // Maximum Block Length
+  m_responsebuffer[len++] = 0xFF;
+  m_responsebuffer[len++] = 0xFF;
+  m_responsebuffer[len++] = 0x00; // Minimum Block Length
+  m_responsebuffer[len++] = 0x00;
+
+  writeDataPhase(len, buf);
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapeRewindUnitCommandHandler() {
+  LOGN("[Rewind]");
+  if(!m_sel) {
+    m_sts |= 0x02; // Image file absent
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+  m_sel->m_filemarks = 0;
+  m_sel->m_file.seek(0);
+  m_phase = PHASE_STATUSIN;
+}
+
+void TapeReadPositionCommandHandler() {
+  uint16_t len = 0;
+  LOGN("[ReadPosition]");
+  if(!m_sel) {
+    m_sts |= 0x02; // Image file absent
+    m_phase = PHASE_STATUSIN;
+    return;
+  }
+  m_responsebuffer[len++] = (m_sel->m_filemarks == 0) ? (1 << 7) : 0;
+  m_responsebuffer[len++] = 0x00; // Partition(0)
+  m_responsebuffer[len++] = 0x00; // Reserved
+  m_responsebuffer[len++] = 0x00; // Reserved
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff); // First Block Location
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff00) >> 8;
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff0000) >> 16;
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff000000) >> 24;
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff); // Last Block Location
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff00) >> 8;
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff0000) >> 16;
+  m_responsebuffer[len++] = (m_sel->m_filemarks & 0xff000000) >> 24;
+  m_responsebuffer[len++] = 0x00; // Reserved
+  m_responsebuffer[len++] = 0x00; // Blocks in buffer (0)
+  m_responsebuffer[len++] = 0x00;
+  m_responsebuffer[len++] = 0x00;
+  m_responsebuffer[len++] = 0x00; // Bytes in buffer (0)
+  m_responsebuffer[len++] = 0x00;
+  m_responsebuffer[len++] = 0x00;
+  m_responsebuffer[len++] = 0x00;
+
+  writeDataPhase(len, buf);
+  m_phase = PHASE_STATUSIN;
+}
+
+void ConfigureTapeHandlers(VirtualDevice_t *vdev) {
+  for(int c = 0; c < 256; c++)
+    vdev->m_handler[c] = &UnknownCommandHandler;
+
+  vdev->m_handler[CMD_ERASE]                         = &TapeEraseCommandHandler;
+  vdev->m_handler[CMD_TEST_UNIT_READY]               = &TestUnitCommandHandler;
+  vdev->m_handler[CMD_REZERO_UNIT]                   = &TapeRewindUnitCommandHandler;
+  vdev->m_handler[CMD_REQUEST_SENSE]                 = &RequestSenseCommandHandler;
+  vdev->m_handler[CMD_READ_BLOCK_LIMITS]             = &TapeReadBlockLimitsCommandHandler;
+  vdev->m_handler[CMD_READ6]                         = &TapeReadCommandHandler;
+  vdev->m_handler[CMD_WRITE6]                        = &TapeWriteCommandHandler;
+  vdev->m_handler[CMD_SEEK6]                         = &TapeSeekCommandHandler;
+  vdev->m_handler[CMD_INQUIRY]                       = &InquiryCommandHandler;
+  vdev->m_handler[CMD_MODE_SELECT6]                  = &ModeSelect6CommandHandler;
+  vdev->m_handler[CMD_MODE_SENSE6]                   = &TapeModeSense6CommandHandler;
+  vdev->m_handler[CMD_START_STOP_UNIT]               = &TapeLoadUnloadCommandHandler;
+  vdev->m_handler[CMD_PREVENT_REMOVAL]               = &TapePreventRemovalCommandHandler;
+  vdev->m_handler[CMD_READ_CAPACITY10]               = &TapeReadCapacityCommandHandler;
+  vdev->m_handler[CMD_READ10]                        = &TapeReadCommandHandler;
+  vdev->m_handler[CMD_WRITE10]                       = &TapeWriteCommandHandler;
+  vdev->m_handler[CMD_SEEK10]                        = &TapeSeekCommandHandler;
+  vdev->m_handler[CMD_READPOSITION10]                = &TapeReadPositionCommandHandler;
+  vdev->m_handler[CMD_MODE_SELECT10]                 = &ModeSelect10CommandHandler;
+  vdev->m_handler[CMD_MODE_SENSE10]                  = &TapeModeSense10CommandHandler;
+}
+
+// If config file exists, read the first three lines and copy the contents.
+// File must be well formed or you will get junk in the SCSI Vendor fields.
+void ConfigureTape(VirtualDevice_t *vdev, const char *image_name) {
+  memcpy(vdev->m_inquiryresponse, SCSI_TAPE_INQUIRY_RESPONSE, sizeof(SCSI_CDROM_INQUIRY_RESPONSE));
+
+  if(image_name) {
+    char configname[MAX_FILE_PATH+1];
+    memcpy(configname, image_name, MAX_FILE_PATH+1);
+    char *psuffix = strstr(configname, ".img");
+    if(psuffix) {
+      strcpy(psuffix, ".cfg");
+    } else {
+      sprintf(configname, "mt%d%d.cfg", vdev->m_id, vdev->m_lun);
+    }
+  
+    FsFile config_file = sd.open(configname, O_RDONLY);
+    if (config_file.isOpen()) {
+      char vendor[9];
+      memset(vendor, 0, sizeof(vendor));
+      config_file.readBytes(vendor, sizeof(vendor));
+      LOGN("SCSI VENDOR: ");
+      LOGN(vendor);
+      memcpy(&(vdev->m_inquiryresponse[8]), vendor, 8);
+    
+      char product[17];
+      memset(product, 0, sizeof(product));
+      config_file.readBytes(product, sizeof(product));
+      LOGN("SCSI PRODUCT: ");
+      LOGN(product);
+      memcpy(&(vdev->m_inquiryresponse[16]), product, 16);
+    
+      char version[5];
+      memset(version, 0, sizeof(version));
+      config_file.readBytes(version, sizeof(version));
+      LOGN("SCSI VERSION: ");
+      LOGN(version);
+      memcpy(&(vdev->m_inquiryresponse[32]), version, 4);
+      config_file.close();
+    }
+  }
+
+  vdev->m_type = DEV_TAPE;
+  ConfigureTapeHandlers(vdev);
+}
+#endif